main.cpp

Committer:
kunaljacy
Date:
2012-04-02
Revision:
0:342fcbecbfbf

File content as of revision 0:342fcbecbfbf:

#include "mbed.h"

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

//         Create Command          
const char Start = 128;
const char FullMode = 132;
const char DriveDirect = 145;          // 4:   [Right Hi] [Right Low] [Left Hi] [Left Low]

int speed_left = 400;
int speed_right = 400;
void forward();
void start();
void stop();
void reverse();
void left();
void right();

// try to move the Irobot create using basic commands
int main(){
    while(1){
        wait(5);              // wait for Create to power up to accept serial commands
        device.baud(57600);   // set baud rate for Create factory default
        start();
        wait(.5);
        // Move around with motor commands
        forward();
        wait(3);
        stop();
        wait(.1);
        reverse();
        wait(3);
        left();
        wait(3);
        stop();
        wait(.1);
        right();
        wait(3);
        stop();
        wait(.5);
    }
}

void start(){
    device.putc(Start);
    device.putc(FullMode);
    wait(.5);
}

void 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)); 
}

void stop(){
    device.printf("%c%c%c%c%c", DriveDirect, char(0),  char(0),  char(0),  char(0));
}

// Reverse - reverse drive motors
void 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 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 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));
}