The FollowMeBot follows the user based on the color of his or her shirt. The device hosts a webcam on a servo to find the object and orient the robot. The color is chosen through the user interface with push buttons. Currently, the MATLAB code supports red and green detection. The purpose of FollowMeBot is to be able to follow the user in order to be helpful for daily tasks.

Dependencies:   Rectangle Servo TextLCD mbed

Fork of FollowMeBot3 by Rahul Shetty

iRobot/iRobot.cpp

Committer:
dhamilton31
Date:
2013-11-25
Revision:
3:de12b39ad805
Parent:
1:6c399fc35deb
Child:
5:5fe2f8603be8

File content as of revision 3:de12b39ad805:

#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));

}
// 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;
}