Revision 4:c7a00aca4520, committed 2015-04-30

Comitter:

bc6599

Date:

Thu Apr 30 01:40:02 2015 +0000

Parent:

3:042a104c558f

Child:

5:854aea46d7b8

Commit message:

It's working!

Changed in this revision

--- a/main.cpp	Thu Apr 30 00:26:33 2015 +0000
+++ b/main.cpp	Thu Apr 30 01:40:02 2015 +0000
@@ -12,54 +12,55 @@
 const int WORDSET1[4] = {1, 2, 3, 6};
 const int WORDSET2[4] = {0, 1, 4, 5};
 
-int args[3] = {6, 0, 0}; // Initial arguments for wordsets 1-3
-
-float speed;
+//int args[3] = {6, 0, 0}; // Initial arguments for wordsets 1-3
+int command1;
+int command2;
+int command3;
 
 int main()
 {
     m3pi.cls();
-
+    
+    command1 = 6;
+    command2 = 0;
+    command3 = 0;
+    
     while (1) {
-        set_command();
-        // m3pi.locate(0,1);
-        // m3pi.printf("%f", m3pi.battery());
-        // m3pi.printf("%c",command);
+        if (xbee.readable()) {
+            xbee.scanf("|%i,%i,%i@", &command1, &command2, &command3);
+        }
+        m3pi.locate(0,1);
+        m3pi.printf("%i,%i,%i", command1, command2, command3);
         execute();
     }
 }
 
-void set_command()
-{
-    if (xbee.readable())
-        m3pi.scanf("|%i,%i,%i@", &args[0], &args[1], &args[2]);
-}
-
 void execute()
 {
-    speed = 0.25; // Default initial speed
+    float speed = 0.25; // Default initial speed
+    float turn_speed = 0.125; // Default turn speed
     
     // If first argument...
-    switch (args[0]) {
+    switch (command1) {
         case 1:                           // is "move",
-            if (args[1] == WORDSET2[2]) {           // only proceed if second argument is "forward",
+            if (command2 == WORDSET2[2]) {           // only proceed if second argument is "forward",
                 m3pi.forward(speed);
-            } else if (args[1] == WORDSET2[3]) {    // or if second argument is "backward"
+            } else if (command2 == WORDSET2[3]) {    // or if second argument is "backward"
                 m3pi.backward(speed);
             } break;
             
         case 2:                           // is "turn",
-            if (args[1] == WORDSET2[0]) {           // only proceed if second argument is "left",
-                m3pi.left(speed);
-            } else if (args[1] == WORDSET2[1]) {    // or if second argument is "right"
-                m3pi.right(speed);
+            if (command2 == WORDSET2[0]) {           // only proceed if second argument is "left",
+                m3pi.left(turn_speed);
+            } else if (command2 == WORDSET2[1]) {    // or if second argument is "right"
+                m3pi.right(turn_speed);
             } break;
             
         case 3:                           // is "run",
-            if (args[1] == WORDSET2[2]) {           // only proceed if second argument is "forward",
+            if (command2 == WORDSET2[2]) {           // only proceed if second argument is "forward",
                 speed *= 2;                         // (then, double speed)
                 m3pi.forward(speed);
-            } else if (args[1] == WORDSET2[3]) {    // or if second argument is "backward"
+            } else if (command2 == WORDSET2[3]) {    // or if second argument is "backward"
                 speed *= 2;                         // (then, double speed)
                 m3pi.backward(speed);
             } break;
@@ -69,6 +70,7 @@
             break;                                  // do nothing (robot will stop once out of the switch block)
     }
     
-    wait(args[2]);                                  // Set execution time of command
+    wait(command3);                                  // Set execution time of command
     m3pi.stop();
+    command1 = 6; command2 = 0; command3 = 0;
 }
\ No newline at end of file