Revision 21:093c8525349a, committed 2018-07-19

Comitter:

cudaChen

Date:

Thu Jul 19 07:41:10 2018 +0000

Parent:

20:cc12f841a06e

Commit message:

[change] merge "PID control" and "obstacle avoidance" feature

Changed in this revision

diff -r cc12f841a06e -r 093c8525349a autocar/autocar.cpp
--- a/autocar/autocar.cpp	Thu Jul 19 07:26:27 2018 +0000
+++ b/autocar/autocar.cpp	Thu Jul 19 07:41:10 2018 +0000
@@ -63,6 +63,20 @@
     *right = rightIR.read_u16() < threshold ? true : false;
 }
 
+void readSensorValues(int *values, bool *hasObstacle, int range)
+{
+    left = leftIR.read_u16();
+    middle = middleIR.read_u16();
+    right = rightIR.read_u16();
+    *values = left * (-1) + middle * 0 + right * 1;
+    
+    int distance;
+    ultrasonic.Send();
+    wait_ms(30);
+    distance = ultrasonic.Read_cm();
+    *hasObstacle = distance < range ? true : false;
+}
+
 // m: 0 => M1      1 => M2
 // power 0~10     dir: 0=>正向　1=>反向
 void DriveSingleMotor(int m, int speed, int dir)
@@ -236,6 +250,54 @@
     }
 }
 
+void run(int values, float Kp, float Ki, float Kd, bool hasObstacle)
+{
+    if(hasObstacle)
+    {
+        turnRight();
+        wait(0.3);
+        forward();
+        wait(0.5);
+        turnLeft();
+        wait(0.3);
+        forward();
+        wait(0.5);
+        turnLeft();
+        wait(0.3);
+        forward();
+        wait(0.5);
+        turnRight();
+        wait(0.3);
+        
+        return;
+    }
+    
+    int error = values; // 'P'
+    interror += error;
+    int lasterror = error - olderror;
+    olderror = error; // 注意olderror的順序
+     int power = error * Kp + interror * Ki + lasterror * Kd;
+    /* error(P值)、interror(I值)、lasterror(D值)、olderror用來取D值的 */
+    
+    // limit PID output value (for 12-bit ADC in STM32)
+    if(power > limit) {
+        power = limit;
+    } else if(power < -limit) {
+        power = -limit;
+    }
+    
+    //pc.printf("%d \r\n", power);
+    
+    // control the direction of auto car
+    if(power < -800) {
+        turnLeft();
+    } else if (power > 800) {
+        turnRight();
+    } else {
+        forward();
+    }
+}
+
 void init()
 {
     DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);

diff -r cc12f841a06e -r 093c8525349a autocar/autocar.h
--- a/autocar/autocar.h	Thu Jul 19 07:26:27 2018 +0000
+++ b/autocar/autocar.h	Thu Jul 19 07:41:10 2018 +0000
@@ -25,6 +25,7 @@
 void readIR(bool* left, bool* middle, bool* right, int threshold);
 int readIRValues();
 void readSensor(bool* left, bool* middle, bool* right, bool* hasObstacle, int threshold, int range);
+void readSensorValues(int *values, bool *hasObstacle, int range);
 
 long map(long x, long in_min, long in_max, long out_min, long out_max);
 
@@ -33,6 +34,7 @@
 void driveMotor(bool left, bool middle, bool right);
 void driveMotor(bool left, bool middle, bool right, bool hasObstacle);
 void driveMotorPID(int values, float Kp, float Ki, float Kd);
+void run(int values, float Kp, float Ki, float Kd, bool hasObstacle);
 
 void init();
 void stop();

diff -r cc12f841a06e -r 093c8525349a main.cpp
--- a/main.cpp	Thu Jul 19 07:26:27 2018 +0000
+++ b/main.cpp	Thu Jul 19 07:41:10 2018 +0000
@@ -66,11 +66,13 @@
             // on track (black): < 500
             //readIR(&left, &middle, &right, threshold);
             //readSensor(&left, &middle, &right, &hasObstacle, threshold, range);
-            values = readIRValues(); // read IR values
+            //values = readIRValues(); // read IR values
+            readSensorValues(&values, &hasObstacle, range);
 
             //driveMotor(left, middle, right);
             //driveMotor(left, middle, right, hasObstacle);
-            driveMotorPID(values, Kp, Ki, Kd);
+            //driveMotorPID(values, Kp, Ki, Kd);
+            run(values, Kp, Ki, Kd, hasObstacle);
         }
     }
 }