iRobot.cpp

00001 #include "mbed.h"
00002 #include "iRobot.h"
00003 
00004 // Definitions of iRobot Create OpenInterface Command Numbers
00005 // See the Create OpenInterface manual for a complete list
00006 
00007 iRobot::iRobot(PinName tx, PinName rx) : device(tx, rx)
00008 {
00009     speed_left = 100;
00010     speed_right = 100;
00011     // set baud rate for Create factory default
00012     device.baud(57600);
00013 }
00014 
00015 void iRobot::changeSpeed(int speed)
00016 {
00017     speed_left = speed;
00018     speed_right = speed;
00019 }
00020 
00021 char iRobot::sensorCode()
00022 {
00023     char error = Sensor_Data_Byte_Error;
00024     Sensor_Data_Byte_Error = 0;
00025     return error;
00026 }
00027 
00028 // Start  - send start and safe mode, start streaming sensor data
00029 void iRobot::start()
00030 {
00031     device.printf("%c%c", Start, SafeMode);
00032     //device.putc(Start);
00033     //device.putc(SafeMode);
00034     wait(.5);
00035      /* device.printf("%c%c%c", SensorStream, char(1), BumpsandDrops);
00036     device.putc(SensorStream);
00037     device.putc(1);
00038     device.putc(BumpsandDrops);
00039     wait(.2);*/
00040     // Setup a serial interrupt function to receive data
00041     device.attach(this, &iRobot::receive_sensor);
00042 }
00043 // Stop  - turn off drive motors
00044 void iRobot::stop()
00045 {
00046     device.printf("%c%c%c%c%c", DriveDirect, char(0),  char(0),  char(0),  char(0));
00047 }
00048 // Forward  - turn on drive motors
00049 void iRobot::forward()
00050 {
00051     device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF),  char(speed_right&0xFF),
00052                   char((speed_left>>8)&0xFF),  char(speed_left&0xFF));
00053 
00054 }
00055 // Reverse - reverse drive motors
00056 void iRobot::reverse()
00057 {
00058     device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF),  char((-speed_right)&0xFF),
00059                   char(((-speed_left)>>8)&0xFF),  char((-speed_left)&0xFF));
00060 
00061 }
00062 // Left - drive motors set to rotate to left
00063 void iRobot::left()
00064 {
00065     device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF),  char(speed_right&0xFF),
00066                   char(((-speed_left)>>8)&0xFF),  char((-speed_left)&0xFF));
00067 }
00068 // Right - drive motors set to rotate to right
00069 void iRobot::right()
00070 {
00071     device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF),  char((-speed_right)&0xFF),
00072                   char((speed_left>>8)&0xFF),  char(speed_left&0xFF));
00073 
00074 }
00075 // Angle - The angle in degrees that iRobot Create has turned since the 
00076 //angle was last requested 
00077 int iRobot::getAngle()
00078 {
00079     char angleBuff[2];
00080     device.printf("%c%c", Sensors, Angle);
00081     device.gets(angleBuff, 2);
00082     return atoi(angleBuff);
00083 }
00084 
00085 // Turn Angle - The angle in degrees that iRobot Create has turned since the 
00086 //angle was last requested ccw is pos angle, cw is neg angle
00087 void iRobot::turnAngle(int angle)
00088 {
00089     char angleTurn[3];
00090     angleTurn[0] = waitAngle;
00091     angleTurn[1] = angle >> 8;
00092     angleTurn[2] = angle & 0xFF;
00093     if(angle > 0){
00094         device.printf("%c%c%c%c",char(137), char(0), char(200), char(0), char(1));
00095     }
00096     else if(angle < 0){
00097         device.printf("%c%c%c%c",char(137), char(0), char(200), char(255), char(255));
00098     }
00099     device.printf("%c%c%c", angleTurn[0], angleTurn[1], angleTurn[2]);
00100     wait(.05);
00101 }
00102 // Charger - search and return to charger using IR beacons (if found)
00103 void iRobot::charger()
00104 {
00105     device.printf("%c%c", Demo, char(1));
00106 }
00107 
00108 // Play Song  - define and play a song
00109 void iRobot::playsong()   // Send out notes & duration to define song and then play song
00110 {
00111 
00112     device.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",
00113                   Song, char(0), char(16), char(91), char(24), char(89), char(12), char(87), char(36), char(87),
00114                   char(24), char(89), char(12), char(91), char(24), char(91), char(12), char(91), char(12), char(89),
00115                   char(12),char(87), char(12), char(89), char(12), char(91), char(12), char(89), char(12), char(87),
00116                   char(24), char(86), char(12), char(87), char(48));
00117 
00118     wait(.2);
00119     device.printf("%c%c", PlaySong, char(0));
00120 }
00121 
00122 // Interrupt Routine to read in serial sensor data packets - BumpandDrop sensor only
00123 void iRobot::receive_sensor()
00124 {
00125     char start_character;
00126     printf("reading\n");
00127 // Loop just in case more than one character is in UART's receive FIFO buffer
00128     while (device.readable()) {
00129         switch (Sensor_byte_count) {
00130 // Wait for Sensor Data Packet Header of 19
00131             case 0: {
00132                 start_character = device.getc();
00133                 if (start_character == 19) Sensor_byte_count++;
00134                 break;
00135             }
00136 // Number of Packet Bytes
00137             case 1: {
00138                 Sensor_Num_Bytes = device.getc();
00139                 Sensor_byte_count++;
00140                 break;
00141             }
00142 // Sensor ID of next data value
00143             case 2: {
00144                 Sensor_ID = device.getc();
00145                 Sensor_byte_count++;
00146                 break;
00147             }
00148 // Sensor data value
00149             case 3: {
00150                 Sensor_Data_Byte = device.getc();
00151                 if(Sensor_Data_Byte != 0){
00152                     Sensor_Data_Byte_Error = Sensor_Data_Byte;
00153                  }
00154                 Sensor_byte_count++;
00155                 break;
00156             }
00157 // Read Checksum and update LEDs with sensor data
00158             case 4: {
00159                 Sensor_Checksum = device.getc();
00160                 // Could add code here to check the checksum and ignore a bad data packet
00161                 /*led1 = Sensor_Data_Byte &0x01;
00162                 led2 = Sensor_Data_Byte &0x02;
00163                 led3 = Sensor_Data_Byte &0x04;
00164                 led4 = Sensor_Data_Byte &0x08;*/
00165                 Sensor_byte_count = 0;
00166                 break;
00167             }
00168         }
00169     }
00170     return;
00171 }