Diff: autocar/autocar.cpp

Revision:

21:093c8525349a

Parent:

19:d06f5a3ed0bc

--- 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);