Kunal Mahajan
/
IrobotCreateTest
main.cpp@0:342fcbecbfbf, 2012-04-02 (annotated)
- Committer:
- kunaljacy
- Date:
- Mon Apr 02 01:56:01 2012 +0000
- Revision:
- 0:342fcbecbfbf
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kunaljacy | 0:342fcbecbfbf | 1 | #include "mbed.h" |
kunaljacy | 0:342fcbecbfbf | 2 | |
kunaljacy | 0:342fcbecbfbf | 3 | Serial device(p9, p10); // tx, rx |
kunaljacy | 0:342fcbecbfbf | 4 | |
kunaljacy | 0:342fcbecbfbf | 5 | // Create Command |
kunaljacy | 0:342fcbecbfbf | 6 | const char Start = 128; |
kunaljacy | 0:342fcbecbfbf | 7 | const char FullMode = 132; |
kunaljacy | 0:342fcbecbfbf | 8 | const char DriveDirect = 145; // 4: [Right Hi] [Right Low] [Left Hi] [Left Low] |
kunaljacy | 0:342fcbecbfbf | 9 | |
kunaljacy | 0:342fcbecbfbf | 10 | int speed_left = 400; |
kunaljacy | 0:342fcbecbfbf | 11 | int speed_right = 400; |
kunaljacy | 0:342fcbecbfbf | 12 | void forward(); |
kunaljacy | 0:342fcbecbfbf | 13 | void start(); |
kunaljacy | 0:342fcbecbfbf | 14 | void stop(); |
kunaljacy | 0:342fcbecbfbf | 15 | void reverse(); |
kunaljacy | 0:342fcbecbfbf | 16 | void left(); |
kunaljacy | 0:342fcbecbfbf | 17 | void right(); |
kunaljacy | 0:342fcbecbfbf | 18 | |
kunaljacy | 0:342fcbecbfbf | 19 | // try to move the Irobot create using basic commands |
kunaljacy | 0:342fcbecbfbf | 20 | int main(){ |
kunaljacy | 0:342fcbecbfbf | 21 | while(1){ |
kunaljacy | 0:342fcbecbfbf | 22 | wait(5); // wait for Create to power up to accept serial commands |
kunaljacy | 0:342fcbecbfbf | 23 | device.baud(57600); // set baud rate for Create factory default |
kunaljacy | 0:342fcbecbfbf | 24 | start(); |
kunaljacy | 0:342fcbecbfbf | 25 | wait(.5); |
kunaljacy | 0:342fcbecbfbf | 26 | // Move around with motor commands |
kunaljacy | 0:342fcbecbfbf | 27 | forward(); |
kunaljacy | 0:342fcbecbfbf | 28 | wait(3); |
kunaljacy | 0:342fcbecbfbf | 29 | stop(); |
kunaljacy | 0:342fcbecbfbf | 30 | wait(.1); |
kunaljacy | 0:342fcbecbfbf | 31 | reverse(); |
kunaljacy | 0:342fcbecbfbf | 32 | wait(3); |
kunaljacy | 0:342fcbecbfbf | 33 | left(); |
kunaljacy | 0:342fcbecbfbf | 34 | wait(3); |
kunaljacy | 0:342fcbecbfbf | 35 | stop(); |
kunaljacy | 0:342fcbecbfbf | 36 | wait(.1); |
kunaljacy | 0:342fcbecbfbf | 37 | right(); |
kunaljacy | 0:342fcbecbfbf | 38 | wait(3); |
kunaljacy | 0:342fcbecbfbf | 39 | stop(); |
kunaljacy | 0:342fcbecbfbf | 40 | wait(.5); |
kunaljacy | 0:342fcbecbfbf | 41 | } |
kunaljacy | 0:342fcbecbfbf | 42 | } |
kunaljacy | 0:342fcbecbfbf | 43 | |
kunaljacy | 0:342fcbecbfbf | 44 | void start(){ |
kunaljacy | 0:342fcbecbfbf | 45 | device.putc(Start); |
kunaljacy | 0:342fcbecbfbf | 46 | device.putc(FullMode); |
kunaljacy | 0:342fcbecbfbf | 47 | wait(.5); |
kunaljacy | 0:342fcbecbfbf | 48 | } |
kunaljacy | 0:342fcbecbfbf | 49 | |
kunaljacy | 0:342fcbecbfbf | 50 | void forward(){ |
kunaljacy | 0:342fcbecbfbf | 51 | 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)); |
kunaljacy | 0:342fcbecbfbf | 52 | } |
kunaljacy | 0:342fcbecbfbf | 53 | |
kunaljacy | 0:342fcbecbfbf | 54 | void stop(){ |
kunaljacy | 0:342fcbecbfbf | 55 | device.printf("%c%c%c%c%c", DriveDirect, char(0), char(0), char(0), char(0)); |
kunaljacy | 0:342fcbecbfbf | 56 | } |
kunaljacy | 0:342fcbecbfbf | 57 | |
kunaljacy | 0:342fcbecbfbf | 58 | // Reverse - reverse drive motors |
kunaljacy | 0:342fcbecbfbf | 59 | void reverse() { |
kunaljacy | 0:342fcbecbfbf | 60 | 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)); |
kunaljacy | 0:342fcbecbfbf | 61 | } |
kunaljacy | 0:342fcbecbfbf | 62 | |
kunaljacy | 0:342fcbecbfbf | 63 | // Left - drive motors set to rotate to left |
kunaljacy | 0:342fcbecbfbf | 64 | void left() { |
kunaljacy | 0:342fcbecbfbf | 65 | 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)); |
kunaljacy | 0:342fcbecbfbf | 66 | } |
kunaljacy | 0:342fcbecbfbf | 67 | |
kunaljacy | 0:342fcbecbfbf | 68 | // Right - drive motors set to rotate to right |
kunaljacy | 0:342fcbecbfbf | 69 | void right() { |
kunaljacy | 0:342fcbecbfbf | 70 | 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)); |
kunaljacy | 0:342fcbecbfbf | 71 | } |