Diff: main.cpp

Revision:

0:04e850e5bb86

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat May 05 04:07:13 2012 +0000
@@ -0,0 +1,426 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+
+
+// Serial Communication with XBee, iRobot, and PC
+MODSERIAL xbee_receive(p9, p10);    // tx, rx
+Serial irobot(p13, p14);         // tx, rx
+Serial pc(USBTX, USBRX);
+
+// LEDs for wireless communication status
+DigitalOut l1(LED1);
+DigitalOut l2(LED2);
+
+// PWM channels for arm motor speed control
+PwmOut m1s(p21);    // shoulder 
+PwmOut m2s(p22);    // elbow
+PwmOut m3s(p23);    // gripper
+PwmOut m4s(p24);    // wrist
+
+// Digial channels for arm motor direction control
+DigitalOut m1d1(p19);    // shoulder 
+DigitalOut m1d2(p20);
+DigitalOut m2d1(p27);    // elbow
+DigitalOut m2d2(p28);
+DigitalOut m3d1(p29);   // gripper
+DigitalOut m3d2(p30);
+DigitalOut m4d1(p25);   // wrist
+DigitalOut m4d2(p26);
+
+// Create Command
+const char Start = 128;
+const char FullMode = 132;
+const char DriveDirect = 145;          // 4:   [Right Hi] [Right Low] [Left Hi] [Left Low]
+const char SensorStream = 148;
+const char PlaySong = 141;
+const char Song = 140;
+const char BumpsandDrops = 7;
+
+/* Global variables with sensor packet info */
+char Sensor_byte_count = 0;
+char Sensor_Data_Byte = 0;
+char Sensor_ID = 0;
+char Sensor_Num_Bytes = 0;
+char Sensor_Checksum = 0;
+short mode;
+
+void start();
+void stop();
+void autonomous();
+void receive_sensor();
+void findBomb();
+void diffuseBomb();
+void returnHome();
+void turn_right();
+void turn_left();
+void returnHome();
+void playsong();
+
+typedef struct {
+    short xAxis;
+    short yAxis;
+    short grip;
+    short left_velocity;
+    short right_velocity;
+    short wrist;
+    short elbow;
+    short mode;
+} xbee_packet;
+
+xbee_packet packet;
+char packetFound = 0;
+
+void receive_xbee_packet() {
+    char tmp;
+    packetFound = 0;
+    char cs = 0;
+    char data[16];
+    l1 = 0;
+    l2 = 0;
+    if (xbee_receive.rxBufferGetCount() < 20) {
+        l1 = 1;
+        return;
+    }
+    while (xbee_receive.rxBufferGetCount() > 19) {
+        tmp = xbee_receive.getc();
+        if (tmp == 0xF1) {
+            tmp = xbee_receive.getc();
+            if (tmp == 0xE2) {
+                packetFound = 1;
+                break;
+            }
+        }
+    }
+    if (packetFound == 1) {
+        l2 = 1;
+        tmp = xbee_receive.getc();
+        cs = tmp;
+        data[0] = xbee_receive.getc();
+        cs ^= data[0];
+        data[1] = xbee_receive.getc();
+        cs ^= data[1];
+        data[2] = xbee_receive.getc();
+        cs ^= data[2];
+        data[3] = xbee_receive.getc();
+        cs ^= data[3];
+        data[4] = xbee_receive.getc();
+        cs ^= data[4];
+        data[5] = xbee_receive.getc();
+        cs ^= data[5];
+        data[6] = xbee_receive.getc();
+        cs ^= data[6];
+        data[7] = xbee_receive.getc();
+        cs ^= data[7];
+        data[8] = xbee_receive.getc();
+        cs ^= data[8];
+        data[9] = xbee_receive.getc();
+        cs ^= data[9];
+         data[10] = xbee_receive.getc();
+        cs ^= data[10];
+         data[11] = xbee_receive.getc();
+        cs ^= data[11];
+         data[12] = xbee_receive.getc();
+        cs ^= data[12];
+         data[13] = xbee_receive.getc();
+        cs ^= data[13];
+        data[14] = xbee_receive.getc();
+        cs ^= data[14];
+        data[15] = xbee_receive.getc();
+        cs ^= data[15];
+        tmp = xbee_receive.getc();
+        if (cs == tmp) {
+            memcpy(&packet, data, 16);
+        }
+    }
+}
+
+void receive_sensor() {
+    char start_character;
+    Sensor_Data_Byte = 0;
+    // Loop just in case more than one character is in UART's receive FIFO buffer
+    while (irobot.readable()) {
+        switch (Sensor_byte_count) {
+            // Wait for Sensor Data Packet Header of 19
+            case 0: {
+                start_character = irobot.getc();
+                if (start_character == 19) Sensor_byte_count++;
+                break;
+            }
+            // Number of Packet Bytes
+            case 1: {
+                Sensor_Num_Bytes = irobot.getc();
+                Sensor_byte_count++;
+                break;
+            }
+            // Sensor ID of next data value
+            case 2: {
+                Sensor_ID = irobot.getc();
+                Sensor_byte_count++;
+                break;
+            }
+            // Sensor data value
+            case 3: {
+                Sensor_Data_Byte = irobot.getc();
+                irobot.getc();
+                Sensor_byte_count = 0;
+                return;
+            }
+            // Read Checksum and update LEDs with sensor data
+            case 4: {
+                Sensor_Checksum = irobot.getc();
+                // Could add code here to check the checksum and ignore a bad data packet
+                Sensor_byte_count = 0;
+                break;
+            }
+        }
+    }
+    return;
+}
+
+void actuate() {
+    short right = packet.right_velocity;
+    short left = packet.left_velocity;
+    short xAxis = packet.xAxis;
+    short yAxis = packet.yAxis;
+    short grip = packet.grip;
+    short wrist = packet.wrist;
+    short elbow = packet.elbow;
+    
+    if (right || left){
+        irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
+        return;
+    }
+    stop();
+    if (xAxis < 0) {
+        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));
+        return;
+    } else if (xAxis > 0) {
+        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));
+        return;
+    }
+    stop();
+    if (yAxis < 0) {
+        m1d1 = 1;
+        m1d2 = 0;
+        m1s.write((float) (-yAxis)/50);
+    } else if (yAxis > 0) {
+        m1d1 = 0;
+        m1d2 = 1;
+        m1s.write((float) yAxis/50);
+    } else {
+        m1d1 = 0;
+        m1d2 = 0;
+        m1s = 0;
+    }
+    if (elbow < 0) {
+    //    pc.printf("elbow\r\n");
+        m2d1 = 1;
+        m2d2 = 0;
+        m2s.write((float) (-elbow)/50);
+    } else if (elbow > 0) {
+        m2d1 = 0;
+        m2d2 = 1;
+        m2s.write((float) elbow/50);
+    } else {
+        m2d1 = 0;
+        m2d2 = 0;
+        m2s.write(0);
+    }
+    if (wrist < 0) {
+        m4d1 = 1;
+        m4d2 = 0;
+        m4s.write((float) 0.2);
+    } else if (wrist > 0) {
+    //    pc.printf("wrist\r\n");
+        m4d1 = 0;
+        m4d2 = 1;
+        m4s.write((float) 0.2);
+    } else {
+        m4d1 = 0;
+        m4d2 = 0;
+        m4s.write(0);
+    }
+    if (grip == -1) {
+    //    pc.printf("gripper\r\n");
+        m3d1 = 1;
+        m3d2 = 0;
+        m3s.write((float) 0.2);
+    } else if (grip == 1) {
+        m3d1 = 0;
+        m3d2 = 1;
+        m3s.write((float) 0.2);
+    } else {
+        m3d1 = 0;
+        m3d2 = 0;
+        m3s.write(0);
+    }
+}
+
+int main() {
+    int count = 0;
+    wait(5);              // wait for Create to power up to accept serial commands
+    irobot.baud(57600);   // set baud rate for Create factory default
+    start();
+    wait(.5);
+    while (1) {
+        receive_xbee_packet();
+        mode = packet.mode;
+        if (packetFound == 1 && !mode) {
+            //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);
+            actuate();
+        } else if (packetFound && mode && !count) {
+            autonomous();
+        }
+        else if (packetFound && mode && count) {
+            stop();
+        }
+    }
+}
+
+// SEMI-AUTONOMOUS CODE GOES HERE
+void autonomous(){
+    
+    char bnd;
+    
+    stop();
+    /*
+    findBomb();
+    diffuseBomb();
+    findBomb();
+    diffuseBomb();
+    returnHome();
+    
+    //pc.printf("BND: %X\r\n", Sensor_Data_Byte);
+    
+    wait(0.5);
+    */
+   
+    irobot.printf("%c%c", 136, char(1));
+    while(1){
+        receive_xbee_packet();
+        mode = packet.mode;
+        if (!mode){
+            start();
+            return;
+        }
+    }
+}
+
+void start() {
+    irobot.putc(Start);
+    irobot.putc(FullMode);
+    wait(.5);
+}
+
+void stop() {
+    irobot.printf("%c%c%c%c%c", DriveDirect, char(0),  char(0),  char(0),  char(0));
+}
+
+// Reverse - reverse drive motors
+void reverse() {
+    int speed_right = 200;
+    int speed_left = 200;
+    irobot.putc(DriveDirect);
+    irobot.putc(char(((-speed_right)>>8)&0xFF));
+    irobot.putc(char((-speed_right)&0xFF));
+    irobot.putc(char(((-speed_left)>>8)&0xFF));
+    irobot.putc(char((-speed_left)&0xFF));
+    wait(.85);
+}
+
+// Left - drive motors set to rotate to left
+void turn_left() {
+    int speed_right = 200;
+    int speed_left = 200;
+    irobot.putc(DriveDirect);
+    irobot.putc(char(((speed_right)>>8)&0xFF));
+    irobot.putc(char((speed_right)&0xFF));
+    irobot.putc(char(((-speed_left)>>8)&0xFF));
+    irobot.putc(char((-speed_left)&0xFF));
+    wait(.85);
+}
+
+// Right - drive motors set to rotate to right
+void turn_right() {
+    int speed_right = 200;
+    int speed_left = 200;
+    irobot.putc(DriveDirect);
+    irobot.putc(char(((-speed_right)>>8)&0xFF));
+    irobot.putc(char((-speed_right)&0xFF));
+    irobot.putc(char(((speed_left)>>8)&0xFF));
+    irobot.putc(char((speed_left)&0xFF));
+    wait(.85);
+}
+
+// Play Song  - define and play a song
+void playsong() { // Send out notes & duration to define song and then play song
+
+    irobot.putc(Song);
+    irobot.putc(char(0));
+    irobot.putc(char(2));
+    irobot.putc(char(64));
+    irobot.putc(char(24));    
+    irobot.putc(char(36));
+    irobot.putc(char(36));
+    wait(0.2);
+    irobot.putc(PlaySong);
+    irobot.putc(char(0));
+}
+
+void findBomb(){    
+    int right = 400;
+    int left = 400;
+    int c = 0;
+    Sensor_Data_Byte = 0;
+    printf("before while loop: %X\r\n", Sensor_Data_Byte);
+    while((Sensor_Data_Byte != 3) && (Sensor_Data_Byte != 2) && (Sensor_Data_Byte != 1)){
+        irobot.putc(SensorStream);
+        irobot.putc(1);
+        irobot.putc(BumpsandDrops);  
+        receive_sensor();
+        printf("bnd: %X\r\n", Sensor_Data_Byte);
+        irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
+        if (!c) {
+            wait(1);
+        }
+        c = 1;
+    }
+    Sensor_Data_Byte = 0;
+    playsong(); 
+    pc.printf("bomb found\r\n");   
+}
+
+void diffuseBomb(){
+    // diffuse the bomb --- motor values here
+    //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);
+    Sensor_Data_Byte = 0;
+    int right;
+    int left;
+    pc.printf("diffusing bomb\r\n");
+    stop();
+    wait(10);
+    right = -100;
+    left = -100;
+    irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
+    wait(0.25);
+    stop();
+    pc.printf("bomb diffused, turning right\r\n");
+    // turn right
+    turn_right();
+    pc.printf("turned right, going home/diffusing next bomb\r\n");
+    Sensor_Data_Byte = 0;
+}
+
+void returnHome(){
+    // move the robot for some time
+    int right = 200;
+    int left = 200;
+    irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
+    wait(8);
+    // turn right
+    turn_right();
+    // move the robot for some time
+    irobot.printf("%c%c%c%c%c", DriveDirect, char((right >> 8) & 0xFF), char(right & 0xFF), char((left >> 8) & 0xFF), char(left & 0xFF));
+    wait(3);
+    stop();
+}
\ No newline at end of file