Kunal Mahajan
/
IrobotCreateTest
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)); }