Diff: autocar/autocar.cpp

Revision:

19:d06f5a3ed0bc

Parent:

18:d7509436e9ef

Child:

21:093c8525349a

--- a/autocar/autocar.cpp	Wed Jul 18 12:38:54 2018 +0000
+++ b/autocar/autocar.cpp	Wed Jul 18 14:18:39 2018 +0000
@@ -42,7 +42,7 @@
 
     int ret = left * (-1) + middle * 0 + right * 1;
 
-    pc.printf("left middle right: %d %d %d\r\n", left, middle, right);
+    //pc.printf("left middle right: %d %d %d\r\n", left, middle, right);
     pc.printf("IR values: %d\r\n", ret);
 
     return ret;
@@ -125,35 +125,27 @@
     int status = left * 4 + middle * 2 + right;
     switch(status) {
         case 7: // go straight
-            pc.printf("7. go straight\r\n");
             forward();
             break;
         case 6: // turn left
-            pc.printf("6. turn left\r\n");
             turnLeft();
             break;
         case 5: // go straight
-            pc.printf("5. go straight\r\n");
             forward();
             break;
         case 4: // turn left
-            pc.printf("4. turn left\r\n");
             turnLeft();
             break;
         case 3: // turn right
-            pc.printf("3. turn right\r\n");
             turnRight();
             break;
         case 2: // go straight
-            pc.printf("2. go straight\r\n");
             forward();
             break;
         case 1: // turn right
-            pc.printf("1. turn right\r\n");
             turnRight();
             break;
         case 0: // go straight
-            pc.printf("0. go straight\r\n");
             forward();
             break;
         default:
@@ -166,29 +158,20 @@
 {
     if(hasObstacle)
     {
-        stop();
-        wait_ms(500);
         turnRight();
-        wait_ms(200);
-        stop();
-        wait_ms(500);
+        wait(0.3);
         forward();
-        wait_ms(500);
-        stop();
-        wait_ms(500);
-        
+        wait(0.5);
         turnLeft();
-        wait_ms(200);
-        stop();
-        wait_ms(500);
+        wait(0.3);
         forward();
-        wait_ms(500);
-        stop();
-        wait_ms(500);
+        wait(0.5);
+        turnLeft();
+        wait(0.3);
+        forward();
+        wait(0.5);
         turnRight();
-        wait_ms(200);
-        stop();
-        wait_ms(500);
+        wait(0.3);
         
         return;
     }
@@ -196,35 +179,27 @@
     int status = left * 4 + middle * 2 + right;
     switch(status) {
         case 7: // go straight
-            pc.printf("7. go straight\r\n");
             forward();
             break;
         case 6: // turn left
-            pc.printf("6. turn left\r\n");
             turnLeft();
             break;
         case 5: // go straight
-            pc.printf("5. go straight\r\n");
             forward();
             break;
         case 4: // turn left
-            pc.printf("4. turn left\r\n");
             turnLeft();
             break;
         case 3: // turn right
-            pc.printf("3. turn right\r\n");
             turnRight();
             break;
         case 2: // go straight
-            pc.printf("2. go straight\r\n");
             forward();
             break;
         case 1: // turn right
-            pc.printf("1. turn right\r\n");
             turnRight();
             break;
         case 0: // go straight
-            pc.printf("0. go straight\r\n");
             forward();
             break;
         default:
@@ -233,7 +208,7 @@
     }
 }
 
-void driveMotorPID(float values, float Kp, float Ki, float Kd)
+void driveMotorPID(int values, float Kp, float Ki, float Kd)
 {
     int error = values; // 'P'
     interror += error;
@@ -249,10 +224,12 @@
         power = -limit;
     }
     
+    //pc.printf("%d \r\n", power);
+    
     // control the direction of auto car
-    if(power < -700) {
+    if(power < -800) {
         turnLeft();
-    } else if (power > 700) {
+    } else if (power > 800) {
         turnRight();
     } else {
         forward();