Diff: main.cpp

Revision:

0:21c11d775cc4

Child:

1:dcd6c9be9e4b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 17 00:07:38 2015 +0000
@@ -0,0 +1,189 @@
+#include "mbed.h"
+ 
+Serial device(p9, p10);  // tx, rx
+AnalogIn sensor(p20);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+ 
+// Definitions of iRobot Create OpenInterface Command Numbers
+// See the Create OpenInterface manual for a complete list
+ 
+ 
+//                 Create Command              // Arguments
+const char         Start = 128;
+const char         SafeMode = 131;
+const char         FullMode = 132;
+const char         Drive = 137;                // 4:   [Vel. Hi] [Vel Low] [Rad. Hi] [Rad. Low]
+const char         DriveDirect = 145;          // 4:   [Right Hi] [Right Low] [Left Hi] [Left Low]
+const char         Demo = 136;                 // 2:    Run Demo x
+const char         Sensors = 142;              // 1:    Sensor Packet ID
+const char         CoverandDock = 143;         // 1:    Return to Charger
+const char         SensorStream = 148;               // x+1: [# of packets requested] IDs of requested packets to stream
+const char         QueryList = 149;            // x+1: [# of packets requested] IDs of requested packets to stream
+const char         StreamPause = 150;          // 1:    0 = stop stream, 1 = start stream
+const char         PlaySong = 141;
+const char         Song = 140;
+                /* iRobot Create Sensor IDs */
+const char         BumpsandDrops = 7;
+const char         Distance = 19;
+const char         Angle = 20;
+ 
+int speed_left =  200;
+int speed_right = 200;
+void start();
+void forward();
+void reverse();
+void left();
+void right();
+void stop();
+void playsong();
+void charger();
+void testDistance();
+ 
+// Demo to move around using basic commands
+int main() {
+// wait for Create to power up to accept serial commands
+    wait(5);
+// set baud rate for Create factory default
+    device.baud(57600);
+// Start command mode and select sensor data to send back
+    start();
+    wait(.5);
+    
+    forward();
+    wait(.5);
+    stop();
+    
+    testDistance();
+    wait(10);
+        playsong();
+
+// Move around with motor commands
+    //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);
+    device.putc(SafeMode);
+    wait(.5);
+  //  device.printf("%c%c%c", SensorStream, char(1), BumpsandDrops);
+    device.putc(SensorStream);
+    device.putc(1);
+    device.putc(BumpsandDrops);
+    wait(.5);
+}
+// Stop  - turn off drive motors
+void stop() {
+    device.printf("%c%c%c%c%c", DriveDirect, char(0),  char(0),  char(0),  char(0));
+}
+// Forward  - turn on drive motors
+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));
+ 
+}
+// 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));
+ 
+}
+// Charger - search and return to charger using IR beacons (if found)
+void charger() {
+    device.printf("%c%c", Demo, char(1));
+}
+// 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));
+}
+
+void testDistance() {
+    int timer = 0;
+    while(timer < 100000000) {
+        if(sensor > 0.75) {
+            led1 = 1;
+            led2 = 1;
+            led3 = 1;
+            led4 = 1;
+            forward();
+        } else if(sensor > 0.50){
+            led1 = 1;
+            led2 = 1;
+            led3 = 1;
+            led4 = 0;
+            left();
+            wait(1);
+            forward();
+            wait(1);
+            stop();
+        } else if(sensor > 0.25){
+            led1 = 1;
+            led2 = 1;
+            led3 = 0;
+            led4 = 0;
+            right();
+            wait(1);
+            forward();
+            wait(1);
+            stop();
+        } else if(sensor > 0.1){
+            led1 = 1;
+            led2 = 0;
+            led3 = 0;
+            led4 = 0;
+//            forward();
+    stop();
+        } else {
+            led1 = 0;
+            led2 = 0;
+            led3 = 0;
+            led4 = 0;
+            stop();
+        }
+        wait(1);
+        timer = timer + 1;
+        timer = timer - 1;
+    }
+}
\ No newline at end of file