File content as of revision 7:41b5307e4c03:

#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((35>>8)&0xFF),  char(35&0xFF),
                  char((35>>8)&0xFF),  char(35&0xFF));
//10-speed
}
// 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((50>>8)&0xFF),  char(50&0xFF),
                  char(((-50)>>8)&0xFF),  char((-50)&0xFF));
}
// Right - drive motors set to rotate to right
void iRobot::right()
{
    device.printf("%c%c%c%c%c", DriveDirect, char(((-50)>>8)&0xFF),  char((-50)&0xFF),
                  char((50>>8)&0xFF),  char(50&0xFF));

}
// Angle - The angle in degrees that iRobot Create has turned since the 
//angle was last requested 
int iRobot::getAngle()
{
    char angleBuff[2];
    device.printf("%c%c", Sensors, Angle);
    device.gets(angleBuff, 2);
    return atoi(angleBuff);
}

// Turn Angle - The angle in degrees that iRobot Create has turned since the 
//angle was last requested ccw is pos angle, cw is neg angle
void iRobot::turnAngle(int angle)
{
    char angleTurn[3];
    angleTurn[0] = waitAngle;
    angleTurn[1] = angle >> 8;
    angleTurn[2] = angle & 0xFF;
    if(angle > 0){
        device.printf("%c%c%c%c",char(137), char(0), char(200), char(0), char(1));
    }
    else if(angle < 0){
        device.printf("%c%c%c%c",char(137), char(0), char(200), char(255), char(255));
    }
    device.printf("%c%c%c", angleTurn[0], angleTurn[1], angleTurn[2]);
    wait(.05);
}
// 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;
    //printf("reading\n");
// 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;
}