Revision 2:ce95322fe535, committed 2015-04-30

Comitter:

mb4899

Date:

Thu Apr 30 00:09:53 2015 +0000

Parent:

1:8b83b8a03351

Child:

3:042a104c558f

Commit message:

Code for m3pi_receiver

Changed in this revision

--- a/main.cpp	Tue Apr 21 15:48:31 2015 +0000
+++ b/main.cpp	Thu Apr 30 00:09:53 2015 +0000
@@ -4,55 +4,73 @@
 Serial xbee(p28, p27);
 m3pi m3pi;
 
+void set_command();
 void execute();
-void set_command();
+
+// Once we're finished, these can go in a separate header file:
 
-//once we're finished, these can go in a separate header file.
-const char LEFT = 'A';
-const char RIGHT = 'B';
-const char STOP = 'D';
-const char FORWARD = 'E';
-const char BACKWARD = 'F';
-const float EX_TIME = 1.0;
+const int[4] WORDSET1 = {1, 2, 3, 6};
+const int[4] WORDSET2 = {0, 1, 4, 5};
+const int[11] WORDSET3 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
 
-char command;
-char prev_command; //in case we need a command history
+const int[3] ARGS = {6, 0, 0}; // Initial arguments for wordsets 1-3
+
 float speed;
 
-int main() {
-    prev_command = STOP;
-    command = STOP;
-    speed = 0.3;
+int main()
+{
     m3pi.cls();
-    
+
     while (1) {
-        //set_command();
-        m3pi.locate(0,1);
-        m3pi.printf("%f", m3pi.battery());
-        //m3pi.printf("%c",command);
-        //execute();
+        set_command();
+        // m3pi.locate(0,1);
+        // m3pi.printf("%f", m3pi.battery());
+        // m3pi.printf("%c",command);
+        execute();
     }
 }
 
-void execute() {
-    switch (command) {
-        case LEFT:
-            m3pi.left(speed); break;
-        case RIGHT:
-            m3pi.right(speed); break;
-        case STOP:
-            m3pi.stop(); break;
-        case FORWARD:
-            m3pi.forward(speed); break;
-        case BACKWARD:
-            m3pi.backward(speed); break;
-    }
+void set_command()
+{
+    if (xbee.readable())
+        m3pi.scanf("|%i,%i,%i@", &ARGS[0], &ARGS[1], &ARGS[2]);
 }
 
-void set_command() {
-    if (xbee.readable()) {
-        prev_command = command;
-        command = xbee.getc();
-        wait(EX_TIME);
+void execute()
+{
+    speed = 0.25; // Default initial speed
+    
+    // If first argument...
+    switch (ARGS[0]) {
+        case WORDSET1[0]:                           // is "move",
+            if (ARGS[1] == WORDSET1[2]) {           // only proceed if second argument is "forward",
+                m3pi.forward();
+            } else if (ARGS[1] == WORDSET1[3]) {    // or if second argument is "backward"
+                m3pi.backward();
+            } break;
+            
+        case WORDSET1[1]:                           // is "turn",
+            if (ARGS[1] == WORDSET1[0]) {           // only proceed if second argument is "left",
+                m3pi.turn();
+                m3pi.left();
+            } else if (ARGS[1] == WORDSET1[1]) {    // or if second argument is "right"
+                m3pi.turn();
+                m3pi.right();
+            } break;
+            
+        case WORDSET1[2]:                           // is "run",
+            if (ARGS[1] == WORDSET1[2]) {           // only proceed if second argument is "forward",
+                speed *= 2;                         // (then, double speed)
+                m3pi.forward();
+            } else if (ARGS[1] == WORDSET1[3]) {    // or if second argument is "backward"
+                speed *= 2;                         // (then, double speed)
+                m3pi.backward();
+            } break;
+            
+        case WORDSET1[3]:                           // is "stop",
+            break;                                  // do nothing (robot will stop once out of the switch block)
     }
+    
+    wait(ARGS[2]);                                  // Set execution time of command
+    m3pi.stop();
 }
\ No newline at end of file