turning
Dependencies: Rectangle Servo TextLCD mbed
Fork of FollowMeBot by
Diff: iRobot/iRobot.cpp
- Revision:
- 1:6c399fc35deb
- Parent:
- 0:3ed56271dd2d
- Child:
- 3:de12b39ad805
diff -r 3ed56271dd2d -r 6c399fc35deb iRobot/iRobot.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/iRobot/iRobot.cpp Mon Nov 18 21:09:13 2013 +0000 @@ -0,0 +1,143 @@ +#include "mbed.h" +#include "iRobot.h" + +// Definitions of iRobot Create OpenInterface Command Numbers +// See the Create OpenInterface manual for a complete list + +iRobot::iRobot(PinName tx, PinName rx) : device(tx, rx) +{ + speed_left = 100; + speed_right = 100; + // set baud rate for Create factory default + device.baud(57600); +} + +void iRobot::changeSpeed(int speed) +{ + speed_left = speed; + speed_right = speed; +} + +char iRobot::sensorCode() +{ + char error = Sensor_Data_Byte_Error; + Sensor_Data_Byte_Error = 0; + return error; +} + +// Start - send start and safe mode, start streaming sensor data +void iRobot::start() +{ + // device.printf("%c%c", Start, SafeMode); + device.putc(Start); + device.putc(SafeMode); + wait(.5); + // device.printf("%c%c%c", SensorStream, char(1), BumpsandDrops); + device.putc(SensorStream); + device.putc(1); + device.putc(BumpsandDrops); + wait(.2); + // Setup a serial interrupt function to receive data + device.attach(this, &iRobot::receive_sensor); +} +// Stop - turn off drive motors +void iRobot::stop() +{ + device.printf("%c%c%c%c%c", DriveDirect, char(0), char(0), char(0), char(0)); +} +// Forward - turn on drive motors +void iRobot::forward() +{ + device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF), char(speed_right&0xFF), + char((speed_left>>8)&0xFF), char(speed_left&0xFF)); + +} +// Reverse - reverse drive motors +void iRobot::reverse() +{ + device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF), char((-speed_right)&0xFF), + char(((-speed_left)>>8)&0xFF), char((-speed_left)&0xFF)); + +} +// Left - drive motors set to rotate to left +void iRobot::left() +{ + device.printf("%c%c%c%c%c", DriveDirect, char((speed_right>>8)&0xFF), char(speed_right&0xFF), + char(((-speed_left)>>8)&0xFF), char((-speed_left)&0xFF)); +} +// Right - drive motors set to rotate to right +void iRobot::right() +{ + device.printf("%c%c%c%c%c", DriveDirect, char(((-speed_right)>>8)&0xFF), char((-speed_right)&0xFF), + char((speed_left>>8)&0xFF), char(speed_left&0xFF)); + +} +// Charger - search and return to charger using IR beacons (if found) +void iRobot::charger() +{ + device.printf("%c%c", Demo, char(1)); +} + +// Play Song - define and play a song +void iRobot::playsong() // Send out notes & duration to define song and then play song +{ + + 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", + Song, char(0), char(16), char(91), char(24), char(89), char(12), char(87), char(36), char(87), + char(24), char(89), char(12), char(91), char(24), char(91), char(12), char(91), char(12), char(89), + char(12),char(87), char(12), char(89), char(12), char(91), char(12), char(89), char(12), char(87), + char(24), char(86), char(12), char(87), char(48)); + + wait(.2); + device.printf("%c%c", PlaySong, char(0)); +} + +// Interrupt Routine to read in serial sensor data packets - BumpandDrop sensor only +void iRobot::receive_sensor() +{ + char start_character; +// Loop just in case more than one character is in UART's receive FIFO buffer + while (device.readable()) { + switch (Sensor_byte_count) { +// Wait for Sensor Data Packet Header of 19 + case 0: { + start_character = device.getc(); + if (start_character == 19) Sensor_byte_count++; + break; + } +// Number of Packet Bytes + case 1: { + Sensor_Num_Bytes = device.getc(); + Sensor_byte_count++; + break; + } +// Sensor ID of next data value + case 2: { + Sensor_ID = device.getc(); + Sensor_byte_count++; + break; + } +// Sensor data value + case 3: { + Sensor_Data_Byte = device.getc(); + if(Sensor_Data_Byte != 0){ + Sensor_Data_Byte_Error = Sensor_Data_Byte; + } + Sensor_byte_count++; + break; + } +// Read Checksum and update LEDs with sensor data + case 4: { + Sensor_Checksum = device.getc(); + // Could add code here to check the checksum and ignore a bad data packet + /*led1 = Sensor_Data_Byte &0x01; + led2 = Sensor_Data_Byte &0x02; + led3 = Sensor_Data_Byte &0x04; + led4 = Sensor_Data_Byte &0x08;*/ + Sensor_byte_count = 0; + break; + } + } + } + return; +} \ No newline at end of file