Kunal Mahajan
/
XbeeReceive
Program for receiving XBee packets and controlling the MoboRobo.
main.cpp
- Committer:
- kunaljacy
- Date:
- 2012-05-05
- Revision:
- 0:04e850e5bb86
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(); }