Diff: main.cpp

Revision:

5:25faf509ee9b

Parent:

4:76b9213714cc

Child:

6:f34538eea724

--- a/main.cpp	Fri Jun 29 10:14:59 2018 +0000
+++ b/main.cpp	Fri Jun 29 12:56:28 2018 +0000
@@ -30,6 +30,7 @@
 }
 
 void DriveSingleMotor(int m, int speed, int dir);
+void driveMotor(bool left, bool middle, bool right);
 
 // main() runs in its own thread in the OS
 int main()
@@ -49,7 +50,7 @@
         right = rightIR.read_u16() < threshold ? true : false;
 
         // if middle detects black
-        if(middle) {
+        /*if(middle) {
             if(left) { // turn left
                 pc.printf("turn left\r\n");
                 DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
@@ -63,7 +64,8 @@
                 DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
                 DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
             }
-        }
+        }*/
+        driveMotor(left, middle, right);
     }
 }
 
@@ -124,3 +126,23 @@
     }
 }
 
+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);
+            }
+        }
+}
+