Diff: main.cpp

Revision:

16:47d3b1f2e90d

Parent:

15:9bc36f47c8cf

Child:

17:740028fe5c99

--- a/main.cpp	Mon Nov 20 03:18:42 2017 +0000
+++ b/main.cpp	Mon Nov 20 04:01:33 2017 +0000
@@ -4,15 +4,75 @@
 #include "motordriver.h"
 #include "MultiModalRobot.h"
  
+
+ 
 Motor lw(p26, p29, p30, 1); // pwm, fwd, rev   LEFT WHEEL
 Motor rw(p25, p28, p27, 1); // pwm, fwd, rev   RIGHT WHEEL
 MultiModalRobot robot(lw, rw);
  
+Serial blue(p13, p14); 
+ 
 int main() {
-    wait(5);
-    for (float s= -1.0; s < 1.0 ; s += 0.01) {
-       robot.driveWheels(s, -s);
-       wait(0.02);
+    char bnum = 0;
+    char bhit = 0;
+    char needToStopRobot = 0;
+    float leftSpeed = 0;
+    float rightSpeed = 0;
+    float DEFAULT_SPEED = 0.75;
+    while(1){
+        wait(0.1);
+        if(blue.readable() && blue.getc() == '!'){
+            if(blue.readable() && blue.getc() == 'B'){
+                bnum = blue.getc();
+                bhit = blue.getc();
+                blue.getc();
+                switch(bnum){
+                    case '1':
+                        needToStopRobot = 1;
+                        break;
+                    case '5': //up
+                        if(bhit=='1'){
+                            leftSpeed = rightSpeed = DEFAULT_SPEED;
+                        } else {
+                            needToStopRobot = 1;
+                        }
+                        break;  
+                    case '6': //down
+                        if(bhit=='1'){
+                            leftSpeed = rightSpeed = -DEFAULT_SPEED;
+                        } else {
+                            needToStopRobot = 1;
+                        }
+                        break;
+                    case '7': //left
+                        if(bhit=='1'){
+                            leftSpeed = -DEFAULT_SPEED;
+                            rightSpeed = DEFAULT_SPEED;
+                        } else {
+                            needToStopRobot = 1;
+                        }
+                        break;
+                    case '8': //right
+                        if(bhit=='1'){
+                            leftSpeed = DEFAULT_SPEED;
+                            rightSpeed = -DEFAULT_SPEED;
+                        } else {
+                            needToStopRobot = 1;
+                        }
+                        break;
+                    default:
+                        robot.stop(0.5);
+                        break;
+                }
+            }           
+        }
+        if(needToStopRobot){
+            robot.stop(0.5);
+            needToStopRobot = 0;
+            leftSpeed = 0;
+            rightSpeed = 0;
+        } else {
+            robot.driveWheels(leftSpeed, rightSpeed);   
+        }
     }
-    robot.stop(0.5);
 }
\ No newline at end of file