Diff: main.cpp

Revision:

10:a14380381d86

Parent:

9:ce1dd89ff5e8

Child:

11:3e9d4c345ebd

--- a/main.cpp	Sat Jun 30 06:53:13 2018 +0000
+++ b/main.cpp	Sat Jun 30 11:36:15 2018 +0000
@@ -17,10 +17,10 @@
 DigitalOut M1_in2(D2);
 DigitalOut M2_in3(D1);
 DigitalOut M2_in4(D0);
-const int MOTOR_M1=0;
-const int MOTOR_M2=1;
-const int DIR_FORWARD= 0;
-const int DIR_BACKWARD=1;
+const int MOTOR_M1 = 0;
+const int MOTOR_M2 = 1;
+const int DIR_FORWARD = 0;
+const int DIR_BACKWARD = 1;
 const int PWR_STOP = 0;
 
 // used for IR sensors
@@ -28,6 +28,11 @@
 AnalogIn middleIR(A3);
 AnalogIn rightIR(A5);
 
+// used for controlling the direction of auto car
+void forward();
+void turnLeft();
+void turnRight();
+
 long map(long x, long in_min, long in_max, long out_min, long out_max)
 {
     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
@@ -35,6 +40,7 @@
 
 void DriveSingleMotor(int m, int speed, int dir);
 void driveMotor(bool left, bool middle, bool right);
+void init();
 
 // main() runs in its own thread in the OS
 int main()
@@ -46,33 +52,36 @@
     // here I use 500 as threshold
     int threshold = 500;
 
+    // set two motors to stop
+    init();
+
     while (true) {
         int reading1 = pb.read();
-        
+
         if(reading1 != lastButtonState) {
             wait_ms(20);
-            
+
             int reading2 = pb.read();
-            
+
             if(reading2 == reading1) {
                 lastButtonState = reading2;
             }
-            
+
             if(lastButtonState == 1) {
                 ledState = !ledState;
             }
         }
-        
+
         led1.write(ledState);
-        
+
         if(ledState) {
-        // not on track: > 500
-        // on track (black): < 500
-        left = leftIR.read_u16() < threshold ? true : false;
-        middle = middleIR.read_u16() < threshold ? true : false;
-        right = rightIR.read_u16() < threshold ? true : false;
+            // not on track: > 500
+            // on track (black): < 500
+            left = leftIR.read_u16() < threshold ? true : false;
+            middle = middleIR.read_u16() < threshold ? true : false;
+            right = rightIR.read_u16() < threshold ? true : false;
 
-        driveMotor(left, middle, right);
+            driveMotor(left, middle, right);
         }
     }
 }
@@ -140,43 +149,35 @@
     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);
+            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);
+            turnLeft();
             break;
         case 5: // go straight
             pc.printf("5. go straight\r\n");
-            DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
-            DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+            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);
+            turnLeft();
             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);
+            turnRight();
             break;
         case 2: // go straight
             pc.printf("2. go straight\r\n");
-            DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
-            DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+            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);
+            turnRight();
             break;
         case 0: // go straight
             pc.printf("0. go straight\r\n");
-            DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
-            DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+            forward();
             break;
         default:
             pc.printf("invalid\r\n");
@@ -184,3 +185,28 @@
     }
 }
 
+void init()
+{
+    DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
+    DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
+}
+
+void forward()
+{
+    DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
+    DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+}
+
+void turnLeft()
+{
+    DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);
+    DriveSingleMotor(MOTOR_M2, 2, DIR_FORWARD);
+}
+
+void turnRight()
+{
+    DriveSingleMotor(MOTOR_M1, 2, DIR_FORWARD);
+    DriveSingleMotor(MOTOR_M2, PWR_STOP, DIR_FORWARD);
+}
+
+