iRobot Create demo

Dependencies:   mbed

main.cpp

Committer:
4180_1
Date:
2014-10-16
Revision:
0:78d882d33841

File content as of revision 0:78d882d33841:

#include "mbed.h"

Serial device(p9, p10);  // tx, rx

// Definitions of iRobot Roomba SCI Command Numbers
// See the Create OpenInterface manual for a complete list


//                 Create Command              // Arguments
const char         Start = 128;
const char         Control = 130;
const char         FullMode = 132;
const char         Drive = 137;                // 4:   [Vel. Hi] [Vel Low] [Rad. Hi] [Rad. Low]
const char         Sensors = 142;              // 1:    Sensor Packet ID
const char         CoverandDock = 143;         // 0:    Return to Charger
const char         Clean = 135;                 // 0:    Start Cleaning
const char         PlaySong = 141;
const char         Song = 140;
                /* iRobot Roomba Sensor IDs */
const char         BumpsandDrops = 1;

int speed =  400;
int radius = 0x8000;
void start();
void forward();
void reverse();
void left();
void right();
void stop();
void playsong();
void charger();

// Demo to move around using basic commands
int main() {
// wait for Roomba to power up to accept serial commands
    wait(5);
// set baud rate for Roomba factory default
    device.baud(57600);
// Start command mode and select sensor data to send back
    start();
    wait(.5);
// Send commands to move around
    forward();
    wait(.5);
    stop();
    wait(.1);
    reverse();
    wait(.5);
    left();
    wait(1);
    stop();
    wait(.1);
    right();
    wait(1);
    stop();
    wait(.5);
// Play a song
    playsong();
    wait(10);
// Search for battery charger IR beacon
    charger();
}


// Start  - send start and safe mode, start streaming sensor data
void start() {
   // device.printf("%c%c", Start, SafeMode);
    device.putc(Start);
    wait(.1);
    device.putc(Control);
    wait(.5);
  //  device.printf("%c%c", SensorStream, char(1));
    device.putc(Sensors);
    device.putc(BumpsandDrops);
    wait(.5);
}
// Stop  - turn off drive motors
void stop() {
    device.printf("%c%c%c%c%c", Drive, char(0),  char(0),  char(0),  char(0));
}
// Forward  - turn on drive motors
void forward() {
    device.printf("%c%c%c%c%c", Drive, char((speed>>8)&0xFF),  char(speed&0xFF),  
    char((radius>>8)&0xFF),  char(radius&0xFF));

}
// Reverse - reverse drive motors
void reverse() {
    device.printf("%c%c%c%c%c", Drive, char(((-speed)>>8)&0xFF),  char((-speed)&0xFF),  
    char(((radius)>>8)&0xFF),  char((radius)&0xFF));

}
// Left - drive motors set to rotate to left
void left() {
    device.printf("%c%c%c%c%c", Drive, char((speed>>8)&0xFF),  char(speed&0xFF),  
    char(((1)>>8)&0xFF),  char((1)&0xFF));
}
// Right - drive motors set to rotate to right
void right() {
    device.printf("%c%c%c%c%c", Drive, char(((speed)>>8)&0xFF),  char((speed)&0xFF),  
    char((-1>>8)&0xFF),  char(-1&0xFF));

}
// Charger - search and return to charger using IR beacons (if found)
void charger() {
    device.printf("%c", Clean );
    wait(.2);
    device.printf("%c", CoverandDock );
}
// Play Song  - define and play a song
void 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));
}