main.cpp

00001 #include "mbed.h"
00002 
00003 Serial device(p9, p10);  // tx, rx
00004 
00005 //         Create Command          
00006 const char Start = 128;
00007 const char FullMode = 132;
00008 const char DriveDirect = 145;          // 4:   [Right Hi] [Right Low] [Left Hi] [Left Low]
00009 
00010 int speed_left = 400;
00011 int speed_right = 400;
00012 void forward();
00013 void start();
00014 void stop();
00015 void reverse();
00016 void left();
00017 void right();
00018 
00019 // try to move the Irobot create using basic commands
00020 int main(){
00021     while(1){
00022         wait(5);              // wait for Create to power up to accept serial commands
00023         device.baud(57600);   // set baud rate for Create factory default
00024         start();
00025         wait(.5);
00026         // Move around with motor commands
00027         forward();
00028         wait(3);
00029         stop();
00030         wait(.1);
00031         reverse();
00032         wait(3);
00033         left();
00034         wait(3);
00035         stop();
00036         wait(.1);
00037         right();
00038         wait(3);
00039         stop();
00040         wait(.5);
00041     }
00042 }
00043 
00044 void start(){
00045     device.putc(Start);
00046     device.putc(FullMode);
00047     wait(.5);
00048 }
00049 
00050 void forward(){
00051     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)); 
00052 }
00053 
00054 void stop(){
00055     device.printf("%c%c%c%c%c", DriveDirect, char(0),  char(0),  char(0),  char(0));
00056 }
00057 
00058 // Reverse - reverse drive motors
00059 void reverse() {
00060     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));
00061 }
00062 
00063 // Left - drive motors set to rotate to left
00064 void left() {
00065     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));
00066 }
00067 
00068 // Right - drive motors set to rotate to right
00069 void right() {
00070     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));
00071 }