Jacob Katz
This is the code we used in our final implementation of our spherical robot.
Revision 0:63c4c959e87f, committed 2012-05-06
- Comitter:
- katzjacob
- Date:
- Sun May 06 05:41:04 2012 +0000
- Commit message:
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Sun May 06 05:41:04 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AjK/code/MODSERIAL/#af2af4c61c2f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun May 06 05:41:04 2012 +0000
@@ -0,0 +1,218 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+
+
+//motor a
+PwmOut motFront(p21);
+DigitalOut frontIN1(p24);
+DigitalOut frontIN2(p25);
+
+//motor b
+PwmOut motLeft(p22);
+DigitalOut leftIN1(p27);
+DigitalOut leftIN2(p26);
+
+//motor c/d - motors are same in control just opposite direction so hardware can accomplish this
+PwmOut motRight(p23);
+DigitalOut rightIN1(p28);
+DigitalOut rightIN2(p29);
+
+//motor standby
+DigitalOut motRLstby(p30);
+DigitalOut motFstby(p20);
+
+//xBee configurations
+MODSERIAL pc(USBTX, USBRX); // tx, rx
+MODSERIAL xbee(p13, p14); //tx, rx
+//MODSERIAL bluetooth(p9, p10);
+DigitalOut rstXbee(p15);
+
+//motor globals
+int rightMotor;
+int leftMotor;
+
+DigitalOut LEDlow(p19);
+DigitalOut LEDmid(p18);
+DigitalOut LEDhigh(p17);
+DigitalOut LEDantenna(p16);
+
+/*************************************************
+function to receive packets from controller xbee
+*************************************************/
+void xbeeReceive()
+{
+ char packetFound = 0;
+ char currentByte;
+ if(xbee.rxBufferGetCount() == 0)
+ {
+ return;
+ }
+ while(xbee.rxBufferGetCount() >= 5)
+ {
+ currentByte = xbee.getc();
+ if(currentByte == 0xAA)
+ {
+ currentByte = xbee.getc();
+ if(currentByte == 0xBB)
+ {
+ //pc.printf("found the preamble");
+ currentByte = xbee.getc();
+ if(currentByte == 0x02)
+ {
+ //pc.printf("found a packet");
+ packetFound = 1;
+ break;
+ }
+ }
+ }
+ }
+ if(packetFound == 1)
+ {
+ //pc.printf("reading the packet");
+ //read in the motor commands
+ int rightTemp = xbee.getc();
+ int leftTemp = xbee.getc();
+ //calculate checksum
+ char checkSumCalc = 0;
+ checkSumCalc ^= rightTemp;
+ checkSumCalc ^= leftTemp;
+ char checkSumSent = xbee.getc();
+ if(checkSumCalc == checkSumSent)
+ {
+ rightMotor = rightTemp;
+ leftMotor = leftTemp;
+ }
+ else
+ {
+ // pc.printf("\n bad packet \n");
+ }
+ }
+}
+
+/*************************************************
+main function
+*************************************************/
+int main() {
+ //reset the xbee
+ rstXbee = 0;
+ wait_ms(1);
+ rstXbee=1;
+ wait_ms(1);
+
+ //led reset verification
+ myled1 = 1;
+ wait(0.1);
+ myled2 = 1;
+ myled1 = 0;
+ wait(0.1);
+ myled2=0;
+
+ //initialize motor stuff
+ motRLstby=1;
+ motFstby=1;
+ motFront = motLeft = motRight=0;
+ frontIN1 = 0;
+ frontIN2 = 0;
+ leftIN1 = 0;
+ leftIN2 = 0;
+ rightIN1 = 0;
+ rightIN2 = 0;
+
+ rightMotor = 0;
+ leftMotor = 0;
+
+ LEDlow=LEDmid=LEDhigh=LEDantenna=0;
+
+ while(1) {
+
+ // bluetooth.putc('q');
+
+
+ xbeeReceive(); //read data from xbee
+ // printf("right: %d ", rightMotor);
+ // printf("left: %d \r\n", leftMotor);
+
+ //rightMotor=rightMotor-8;
+ //motor control
+
+ //right motor
+ if(rightMotor <= 135 && rightMotor >= 128)
+ {
+ //turn off motor
+ motRight = 0.0;
+
+ }
+ else if(rightMotor > 135)
+ {
+ //right motor forward
+ rightIN1=1;
+ rightIN2=0;
+ motRight = (((float)rightMotor-135.0)/120.0);
+ }
+ else if(rightMotor < 128)
+ {
+ //right motor backwards
+ rightIN1=0;
+ rightIN2=1;
+ motRight = ((255.0-(float)rightMotor)/255.0);
+ }
+ //left motor
+ if(leftMotor <= 142 && leftMotor >= 136)
+ {
+ //turn off motor
+ motLeft = 0.0;
+
+ }
+
+ else if(leftMotor > 142)
+ {
+ //left motor forward
+ leftIN1=0;
+ leftIN2=1;
+ motLeft = (((float)leftMotor-142.0)/113.0);
+ }
+ else if(leftMotor < 136)
+ {
+ //left motor backwards
+ leftIN1=1;
+ leftIN2=0;
+ motLeft = ((255.0-(float)leftMotor)/255.0);
+ }
+ //front and back motors
+ if(rightMotor < 15 && leftMotor > 240) //only kick in if there is a large different in directions of motors
+ {
+ //rotate clockwise
+ motFstby=1;
+ frontIN1 = 1;
+ frontIN2 = 0;
+ motFront = 1;
+ LEDantenna=LEDlow=LEDmid=LEDhigh=1;
+ // printf("
+
+
+ }
+ else if(leftMotor < 15 && rightMotor > 240)
+ {
+ //rotate counter-clockwise
+ motFstby=1;
+ frontIN1 = 0;
+ frontIN2 = 1;
+ motFront = 1;
+ LEDantenna=LEDlow=LEDmid=LEDhigh=1;
+ }
+ else
+ {
+ motFront=0;
+ motFstby = 0;
+ LEDantenna=LEDlow=LEDmid=LEDhigh=0;
+ }
+
+
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun May 06 05:41:04 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/737756e0b479