Diff: main.cpp

Revision:

6:f34538eea724

Parent:

5:25faf509ee9b

Child:

7:4edd049209a6

--- a/main.cpp	Fri Jun 29 12:56:28 2018 +0000
+++ b/main.cpp	Fri Jun 29 13:12:48 2018 +0000
@@ -49,22 +49,6 @@
         middle = middleIR.read_u16() < threshold ? true : false;
         right = rightIR.read_u16() < threshold ? true : false;
 
-        // if middle detects black
-        /*if(middle) {
-            if(left) { // turn left
-                pc.printf("turn left\r\n");
-                DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
-                DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
-            } else if(right) { // turn right
-                pc.printf("turn right\r\n");
-                DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
-                DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
-            } else { // go straight
-                pc.printf("go straight\r\n");
-                DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
-                DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
-            }
-        }*/
         driveMotor(left, middle, right);
     }
 }
@@ -128,21 +112,51 @@
 
 void driveMotor(bool left, bool middle, bool right)
 {
-    // if middle detects black
-        if(middle) {
-            if(left) { // turn left
-                pc.printf("turn left\r\n");
-                DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
-                DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
-            } else if(right) { // turn right
-                pc.printf("turn right\r\n");
-                DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
-                DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
-            } else { // go straight
-                pc.printf("go straight\r\n");
-                DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
-                DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
-            }
-        }
+    int status = left * 4 + middle * 2 + right;
+    switch(status) {
+        case 7: // go straight
+            pc.printf("7. go straight\r\n");
+            DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
+            DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+            break;
+        case 6: // turn left
+            pc.printf("6. turn left\r\n");
+            DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
+            DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+            break;
+        case 5: // go straight
+            pc.printf("5. go straight\r\n");
+            DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
+            DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+            break;
+        case 4: // turn left
+            pc.printf("4. turn left\r\n");
+            DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
+            DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+            break;
+        case 3: // turn right
+            pc.printf("3. turn right\r\n");
+            DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
+            DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
+            break;
+        case 2: // go straight
+            pc.printf("2. go straight\r\n");
+            DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
+            DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+            break;
+        case 1: // turn right
+            pc.printf("1. turn right\r\n");
+            DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
+            DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
+            break;
+        case 0: // go straight
+            pc.printf("0. go straight\r\n");
+            DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
+            DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+            break;
+        default:
+            pc.printf("invalid\r\n");
+            break;
+    }
 }