main.cpp

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 
00004 
00005 // Serial Communication with XBee, iRobot, and PC
00006 MODSERIAL xbee_receive(p9, p10);    // tx, rx
00007 Serial irobot(p13, p14);         // tx, rx
00008 Serial pc(USBTX, USBRX);
00009 
00010 // LEDs for wireless communication status
00011 DigitalOut l1(LED1);
00012 DigitalOut l2(LED2);
00013 
00014 // PWM channels for arm motor speed control
00015 PwmOut m1s(p21);    // shoulder 
00016 PwmOut m2s(p22);    // elbow
00017 PwmOut m3s(p23);    // gripper
00018 PwmOut m4s(p24);    // wrist
00019 
00020 // Digial channels for arm motor direction control
00021 DigitalOut m1d1(p19);    // shoulder 
00022 DigitalOut m1d2(p20);
00023 DigitalOut m2d1(p27);    // elbow
00024 DigitalOut m2d2(p28);
00025 DigitalOut m3d1(p29);   // gripper
00026 DigitalOut m3d2(p30);
00027 DigitalOut m4d1(p25);   // wrist
00028 DigitalOut m4d2(p26);
00029 
00030 // Create Command
00031 const char Start = 128;
00032 const char FullMode = 132;
00033 const char DriveDirect = 145;          // 4:   [Right Hi] [Right Low] [Left Hi] [Left Low]
00034 const char SensorStream = 148;
00035 const char PlaySong = 141;
00036 const char Song = 140;
00037 const char BumpsandDrops = 7;
00038 
00039 /* Global variables with sensor packet info */
00040 char Sensor_byte_count = 0;
00041 char Sensor_Data_Byte = 0;
00042 char Sensor_ID = 0;
00043 char Sensor_Num_Bytes = 0;
00044 char Sensor_Checksum = 0;
00045 short mode;
00046 
00047 void start();
00048 void stop();
00049 void autonomous();
00050 void receive_sensor();
00051 void findBomb();
00052 void diffuseBomb();
00053 void returnHome();
00054 void turn_right();
00055 void turn_left();
00056 void returnHome();
00057 void playsong();
00058 
00059 typedef struct {
00060     short xAxis;
00061     short yAxis;
00062     short grip;
00063     short left_velocity;
00064     short right_velocity;
00065     short wrist;
00066     short elbow;
00067     short mode;
00068 } xbee_packet;
00069 
00070 xbee_packet packet;
00071 char packetFound = 0;
00072 
00073 void receive_xbee_packet() {
00074     char tmp;
00075     packetFound = 0;
00076     char cs = 0;
00077     char data[16];
00078     l1 = 0;
00079     l2 = 0;
00080     if (xbee_receive.rxBufferGetCount() < 20) {
00081         l1 = 1;
00082         return;
00083     }
00084     while (xbee_receive.rxBufferGetCount() > 19) {
00085         tmp = xbee_receive.getc();
00086         if (tmp == 0xF1) {
00087             tmp = xbee_receive.getc();
00088             if (tmp == 0xE2) {
00089                 packetFound = 1;
00090                 break;
00091             }
00092         }
00093     }
00094     if (packetFound == 1) {
00095         l2 = 1;
00096         tmp = xbee_receive.getc();
00097         cs = tmp;
00098         data[0] = xbee_receive.getc();
00099         cs ^= data[0];
00100         data[1] = xbee_receive.getc();
00101         cs ^= data[1];
00102         data[2] = xbee_receive.getc();
00103         cs ^= data[2];
00104         data[3] = xbee_receive.getc();
00105         cs ^= data[3];
00106         data[4] = xbee_receive.getc();
00107         cs ^= data[4];
00108         data[5] = xbee_receive.getc();
00109         cs ^= data[5];
00110         data[6] = xbee_receive.getc();
00111         cs ^= data[6];
00112         data[7] = xbee_receive.getc();
00113         cs ^= data[7];
00114         data[8] = xbee_receive.getc();
00115         cs ^= data[8];
00116         data[9] = xbee_receive.getc();
00117         cs ^= data[9];
00118          data[10] = xbee_receive.getc();
00119         cs ^= data[10];
00120          data[11] = xbee_receive.getc();
00121         cs ^= data[11];
00122          data[12] = xbee_receive.getc();
00123         cs ^= data[12];
00124          data[13] = xbee_receive.getc();
00125         cs ^= data[13];
00126         data[14] = xbee_receive.getc();
00127         cs ^= data[14];
00128         data[15] = xbee_receive.getc();
00129         cs ^= data[15];
00130         tmp = xbee_receive.getc();
00131         if (cs == tmp) {
00132             memcpy(&packet, data, 16);
00133         }
00134     }
00135 }
00136 
00137 void receive_sensor() {
00138     char start_character;
00139     Sensor_Data_Byte = 0;
00140     // Loop just in case more than one character is in UART's receive FIFO buffer
00141     while (irobot.readable()) {
00142         switch (Sensor_byte_count) {
00143             // Wait for Sensor Data Packet Header of 19
00144             case 0: {
00145                 start_character = irobot.getc();
00146                 if (start_character == 19) Sensor_byte_count++;
00147                 break;
00148             }
00149             // Number of Packet Bytes
00150             case 1: {
00151                 Sensor_Num_Bytes = irobot.getc();
00152                 Sensor_byte_count++;
00153                 break;
00154             }
00155             // Sensor ID of next data value
00156             case 2: {
00157                 Sensor_ID = irobot.getc();
00158                 Sensor_byte_count++;
00159                 break;
00160             }
00161             // Sensor data value
00162             case 3: {
00163                 Sensor_Data_Byte = irobot.getc();
00164                 irobot.getc();
00165                 Sensor_byte_count = 0;
00166                 return;
00167             }
00168             // Read Checksum and update LEDs with sensor data
00169             case 4: {
00170                 Sensor_Checksum = irobot.getc();
00171                 // Could add code here to check the checksum and ignore a bad data packet
00172                 Sensor_byte_count = 0;
00173                 break;
00174             }
00175         }
00176     }
00177     return;
00178 }
00179 
00180 void actuate() {
00181     short right = packet.right_velocity;
00182     short left = packet.left_velocity;
00183     short xAxis = packet.xAxis;
00184     short yAxis = packet.yAxis;
00185     short grip = packet.grip;
00186     short wrist = packet.wrist;
00187     short elbow = packet.elbow;
00188     
00189     if (right || left){
00190         irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
00191         return;
00192     }
00193     stop();
00194     if (xAxis < 0) {
00195         irobot.printf("%c%c%c%c%c", DriveDirect, char(((-xAxis * 5) >> 8) & 0xFF), char(-xAxis * 5 & 0xFF), char(((xAxis * 5) >> 8) & 0xFF), char((xAxis * 5) & 0xFF));
00196         return;
00197     } else if (xAxis > 0) {
00198         irobot.printf("%c%c%c%c%c", DriveDirect, char(((-xAxis * 5) >> 8) & 0xFF), char(-xAxis * 5 & 0xFF), char(((xAxis * 5) >> 8) & 0xFF), char((xAxis * 5) & 0xFF));
00199         return;
00200     }
00201     stop();
00202     if (yAxis < 0) {
00203         m1d1 = 1;
00204         m1d2 = 0;
00205         m1s.write((float) (-yAxis)/50);
00206     } else if (yAxis > 0) {
00207         m1d1 = 0;
00208         m1d2 = 1;
00209         m1s.write((float) yAxis/50);
00210     } else {
00211         m1d1 = 0;
00212         m1d2 = 0;
00213         m1s = 0;
00214     }
00215     if (elbow < 0) {
00216     //    pc.printf("elbow\r\n");
00217         m2d1 = 1;
00218         m2d2 = 0;
00219         m2s.write((float) (-elbow)/50);
00220     } else if (elbow > 0) {
00221         m2d1 = 0;
00222         m2d2 = 1;
00223         m2s.write((float) elbow/50);
00224     } else {
00225         m2d1 = 0;
00226         m2d2 = 0;
00227         m2s.write(0);
00228     }
00229     if (wrist < 0) {
00230         m4d1 = 1;
00231         m4d2 = 0;
00232         m4s.write((float) 0.2);
00233     } else if (wrist > 0) {
00234     //    pc.printf("wrist\r\n");
00235         m4d1 = 0;
00236         m4d2 = 1;
00237         m4s.write((float) 0.2);
00238     } else {
00239         m4d1 = 0;
00240         m4d2 = 0;
00241         m4s.write(0);
00242     }
00243     if (grip == -1) {
00244     //    pc.printf("gripper\r\n");
00245         m3d1 = 1;
00246         m3d2 = 0;
00247         m3s.write((float) 0.2);
00248     } else if (grip == 1) {
00249         m3d1 = 0;
00250         m3d2 = 1;
00251         m3s.write((float) 0.2);
00252     } else {
00253         m3d1 = 0;
00254         m3d2 = 0;
00255         m3s.write(0);
00256     }
00257 }
00258 
00259 int main() {
00260     int count = 0;
00261     wait(5);              // wait for Create to power up to accept serial commands
00262     irobot.baud(57600);   // set baud rate for Create factory default
00263     start();
00264     wait(.5);
00265     while (1) {
00266         receive_xbee_packet();
00267         mode = packet.mode;
00268         if (packetFound == 1 && !mode) {
00269             //pc.printf("X: %d\tY: %d\tF: %d\tL: %d\tR: %d\tE: %d\tW: %d\r\n", packet.xAxis, packet.yAxis, packet.grip, packet.left_velocity, packet.right_velocity, packet.elbow, packet.wrist);
00270             actuate();
00271         } else if (packetFound && mode && !count) {
00272             autonomous();
00273         }
00274         else if (packetFound && mode && count) {
00275             stop();
00276         }
00277     }
00278 }
00279 
00280 // SEMI-AUTONOMOUS CODE GOES HERE
00281 void autonomous(){
00282     
00283     char bnd;
00284     
00285     stop();
00286     /*
00287     findBomb();
00288     diffuseBomb();
00289     findBomb();
00290     diffuseBomb();
00291     returnHome();
00292     
00293     //pc.printf("BND: %X\r\n", Sensor_Data_Byte);
00294     
00295     wait(0.5);
00296     */
00297    
00298     irobot.printf("%c%c", 136, char(1));
00299     while(1){
00300         receive_xbee_packet();
00301         mode = packet.mode;
00302         if (!mode){
00303             start();
00304             return;
00305         }
00306     }
00307 }
00308 
00309 void start() {
00310     irobot.putc(Start);
00311     irobot.putc(FullMode);
00312     wait(.5);
00313 }
00314 
00315 void stop() {
00316     irobot.printf("%c%c%c%c%c", DriveDirect, char(0),  char(0),  char(0),  char(0));
00317 }
00318 
00319 // Reverse - reverse drive motors
00320 void reverse() {
00321     int speed_right = 200;
00322     int speed_left = 200;
00323     irobot.putc(DriveDirect);
00324     irobot.putc(char(((-speed_right)>>8)&0xFF));
00325     irobot.putc(char((-speed_right)&0xFF));
00326     irobot.putc(char(((-speed_left)>>8)&0xFF));
00327     irobot.putc(char((-speed_left)&0xFF));
00328     wait(.85);
00329 }
00330 
00331 // Left - drive motors set to rotate to left
00332 void turn_left() {
00333     int speed_right = 200;
00334     int speed_left = 200;
00335     irobot.putc(DriveDirect);
00336     irobot.putc(char(((speed_right)>>8)&0xFF));
00337     irobot.putc(char((speed_right)&0xFF));
00338     irobot.putc(char(((-speed_left)>>8)&0xFF));
00339     irobot.putc(char((-speed_left)&0xFF));
00340     wait(.85);
00341 }
00342 
00343 // Right - drive motors set to rotate to right
00344 void turn_right() {
00345     int speed_right = 200;
00346     int speed_left = 200;
00347     irobot.putc(DriveDirect);
00348     irobot.putc(char(((-speed_right)>>8)&0xFF));
00349     irobot.putc(char((-speed_right)&0xFF));
00350     irobot.putc(char(((speed_left)>>8)&0xFF));
00351     irobot.putc(char((speed_left)&0xFF));
00352     wait(.85);
00353 }
00354 
00355 // Play Song  - define and play a song
00356 void playsong() { // Send out notes & duration to define song and then play song
00357 
00358     irobot.putc(Song);
00359     irobot.putc(char(0));
00360     irobot.putc(char(2));
00361     irobot.putc(char(64));
00362     irobot.putc(char(24));    
00363     irobot.putc(char(36));
00364     irobot.putc(char(36));
00365     wait(0.2);
00366     irobot.putc(PlaySong);
00367     irobot.putc(char(0));
00368 }
00369 
00370 void findBomb(){    
00371     int right = 400;
00372     int left = 400;
00373     int c = 0;
00374     Sensor_Data_Byte = 0;
00375     printf("before while loop: %X\r\n", Sensor_Data_Byte);
00376     while((Sensor_Data_Byte != 3) && (Sensor_Data_Byte != 2) && (Sensor_Data_Byte != 1)){
00377         irobot.putc(SensorStream);
00378         irobot.putc(1);
00379         irobot.putc(BumpsandDrops);  
00380         receive_sensor();
00381         printf("bnd: %X\r\n", Sensor_Data_Byte);
00382         irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
00383         if (!c) {
00384             wait(1);
00385         }
00386         c = 1;
00387     }
00388     Sensor_Data_Byte = 0;
00389     playsong(); 
00390     pc.printf("bomb found\r\n");   
00391 }
00392 
00393 void diffuseBomb(){
00394     // diffuse the bomb --- motor values here
00395     //pc.printf("X: %d\tY: %d\tF: %d\tL: %d\tR: %d\tE: %d\tW: %d\r\n", packet.xAxis, packet.yAxis, packet.grip, packet.left_velocity, packet.right_velocity, packet.elbow, packet.wrist);
00396     Sensor_Data_Byte = 0;
00397     int right;
00398     int left;
00399     pc.printf("diffusing bomb\r\n");
00400     stop();
00401     wait(10);
00402     right = -100;
00403     left = -100;
00404     irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
00405     wait(0.25);
00406     stop();
00407     pc.printf("bomb diffused, turning right\r\n");
00408     // turn right
00409     turn_right();
00410     pc.printf("turned right, going home/diffusing next bomb\r\n");
00411     Sensor_Data_Byte = 0;
00412 }
00413 
00414 void returnHome(){
00415     // move the robot for some time
00416     int right = 200;
00417     int left = 200;
00418     irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
00419     wait(8);
00420     // turn right
00421     turn_right();
00422     // move the robot for some time
00423     irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
00424     wait(3);
00425     stop();
00426 }