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