File content as of revision 0:04e850e5bb86:

#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();
}