Diff: test_main.cpp

Revision:

27:91dc5174333a

Parent:

23:dba0233303b6

Child:

29:1c5254b27851

--- a/test_main.cpp	Thu Nov 03 17:11:46 2016 +0000
+++ b/test_main.cpp	Thu Nov 03 18:19:39 2016 +0000
@@ -301,6 +301,11 @@
     robotController.paintDown();
 }
 
+float xmin = 25;
+float xmax = 40;
+float ymin = 20;
+float ymax = 50;
+
 /* executes the robot command */
 void processCommand(RobotCommand cmd) {
     if (!canCommand || cmd == robotCommand) return;
@@ -324,13 +329,11 @@
             break;
         case FORWARD:
             getXandY(x, y);
-            if (x < 40)
-                robotController.moveTo(x + 5.0f, y);
+            robotController.moveTo(xmax, h + 0.5*d);
             break;
         case BACKWARD:
             getXandY(x, y);
-            if (x > 5)
-                robotController.moveTo(x - 5.0f, y);
+            robotController.moveTo(xmin, h + 0.5*d);
             break;
         case NOTHING:
             
@@ -357,8 +360,9 @@
             break;
         case FORBACK:
             progState = UPDOWN;
-            break;
+            break;   
     }
+    pc.printf("\r\n\n\n1 and 1, switch state to %s.\r\n\n\n", progState == UPDOWN ? "UPDOWN" : "FORBACK");
      //we can only switch once per 2 seconds
     switchBlocker.attach(&unblockSwitch, 2.0f);
     return true;
@@ -370,36 +374,38 @@
     if (emg1 && emg2) {
         switchState();
         processCommand(command);
-        pc.printf("1 and 1, switch state to %s.", progState == UPDOWN ? "UPDOWN" : "FORBACK");
         return;
     }
     switch(progState) {
         case UPDOWN:
             if (emg1) {
                 command = UP;
-                pc.printf("1 and 0, command = UP");
+                pc.printf("1 and 0, command = UP\r\n");
             } else if (emg2) {
                 command = DOWN;
-                pc.printf("0 and 1, command = DOWN");
+                pc.printf("0 and 1, command = DOWN\r\n");
             }
             break;
         case FORBACK:
             if (emg1) {
                 command = FORWARD;
-                pc.printf("1 and 0, command = FORWARD");
+                pc.printf("1 and 0, command = FORWARD\r\n");
             } else if (emg2) {
                 command = BACKWARD;
-                pc.printf("0 and 1, command = BACKWARD");
+                pc.printf("0 and 1, command = BACKWARD\r\n");
             }
             break;
     }
     
+    if (!emg1 && !emg2) {
+        pc.printf("0 and 0\r\n");
+    }
+    
     //execute the command
     processCommand(command);
 }
 
 void consumeBools(bool x, bool y) {
-    //pc.printf("%d\t%d\r\n", x, y);
     onSignal(x, y);
 }