Diff: autocar/autocar.cpp

Revision:

13:87cd0ae37e06

Parent:

12:e95ed962be7a

Child:

14:28cae134a304

--- a/autocar/autocar.cpp	Sat Jun 30 13:08:00 2018 +0000
+++ b/autocar/autocar.cpp	Sun Jul 01 07:58:20 2018 +0000
@@ -18,6 +18,12 @@
 AnalogIn leftIR(A1);
 AnalogIn middleIR(A3);
 AnalogIn rightIR(A5);
+int left, middle, right;
+
+// PID factors
+int interror = 0;
+int olderror = 0;
+int limit = 4096; // 12-bit ADC in STM32
 
 // used for output message via serial
 Serial pc(SERIAL_TX, SERIAL_RX);
@@ -36,6 +42,19 @@
     *right = rightIR.read_u16() < threshold ? true : false;
 }
 
+int readIRValues()
+{
+    left = leftIR.read_u16();
+    middle = middleIR.read_u16();
+    right = rightIR.read_u16();
+
+    int ret = left * (-1) + middle * 0 + right * 1;
+
+    pc.printf("IR values: %d\r\n", ret);
+
+    return ret;
+}
+
 // m: 0 => M1      1 => M2
 // power 0~10     dir: 0=>正向　1=>反向
 void DriveSingleMotor(int m, int speed, int dir)
@@ -135,6 +154,32 @@
     }
 }
 
+void driveMotorPID(int values, float Kp, float Ki, float Kd)
+{
+    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;
+    }
+    
+    // control the direction of auto car
+    if(power < -900) {
+        turnLeft();
+    } else if (power > 900) {
+        turnRight();
+    } else {
+        forward();
+    }
+}
+
 void init()
 {
     DriveSingleMotor(MOTOR_M1, PWR_STOP, DIR_FORWARD);