Lu-Hsuan Chen / Mbed OS motorcar_pid

motor car with PID running feature

Dependencies:   Ping

motorcar/motorcar.cpp@4:982dcc2390a2, 2018-08-01 (annotated)

Committer:
cudaChen
Date:
Wed Aug 01 08:24:24 2018 +0000
Revision:
4:982dcc2390a2
Parent:
3:4be8f486a120
Child:
5:3caf5b6ed35a
[milestone] finish the first version of motor car with PID feature

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cudaChen 2:1dcd81fdef9e 1 #include "mbed.h"
cudaChen 2:1dcd81fdef9e 2
cudaChen 2:1dcd81fdef9e 3 #include "motorcar.h"
cudaChen 2:1dcd81fdef9e 4 //#include "Ping.h"
cudaChen 2:1dcd81fdef9e 5
cudaChen 2:1dcd81fdef9e 6 // output used for controlling motors
cudaChen 4:982dcc2390a2 7 AnalogOut M1_enable(PA_4); // right motor enable signal
cudaChen 4:982dcc2390a2 8 AnalogOut M2_enable(PA_5); // left motor enable signal
cudaChen 2:1dcd81fdef9e 9 DigitalOut M1_in1(D3); // connected to right motor (IN1)
cudaChen 2:1dcd81fdef9e 10 DigitalOut M1_in2(D2); // connected to right motor (IN2)
cudaChen 2:1dcd81fdef9e 11 DigitalOut M2_in3(D1); // connected to left motor (IN3)
cudaChen 2:1dcd81fdef9e 12 DigitalOut M2_in4(D0); // connected to left motor (IN4)
cudaChen 2:1dcd81fdef9e 13
cudaChen 2:1dcd81fdef9e 14 // input used for IR sensors
cudaChen 2:1dcd81fdef9e 15 AnalogIn leftIR(A1); // left sensor
cudaChen 2:1dcd81fdef9e 16 AnalogIn middleIR(A3); // middle sensor
cudaChen 2:1dcd81fdef9e 17 AnalogIn rightIR(A5); // right sensor
cudaChen 2:1dcd81fdef9e 18
cudaChen 2:1dcd81fdef9e 19 // used for storing returned IR sensors' value
cudaChen 2:1dcd81fdef9e 20 int left, middle, right;
cudaChen 2:1dcd81fdef9e 21
cudaChen 2:1dcd81fdef9e 22 // PID factors
cudaChen 2:1dcd81fdef9e 23 int interror = 0;
cudaChen 2:1dcd81fdef9e 24 int olderror = 0;
cudaChen 2:1dcd81fdef9e 25 int values = 0;
cudaChen 2:1dcd81fdef9e 26 float Kp = 1, Ki = 0, Kd = 0;
cudaChen 2:1dcd81fdef9e 27 /* Though STM32 only has 12-bit DAC,
cudaChen 2:1dcd81fdef9e 28 * mbed still cam map the value by
cudaChen 2:1dcd81fdef9e 29 * using write_u16()
cudaChen 2:1dcd81fdef9e 30 */
cudaChen 2:1dcd81fdef9e 31 int limit = 65535; // 2^16 - 1
cudaChen 2:1dcd81fdef9e 32
cudaChen 2:1dcd81fdef9e 33 // used for output message via serial for ease of debugging
cudaChen 4:982dcc2390a2 34 //Serial pc(SERIAL_TX, SERIAL_RX);
cudaChen 2:1dcd81fdef9e 35
cudaChen 2:1dcd81fdef9e 36 void readSensorValue() {
cudaChen 2:1dcd81fdef9e 37 left = leftIR.read_u16();
cudaChen 2:1dcd81fdef9e 38 middle = middleIR.read_u16();
cudaChen 2:1dcd81fdef9e 39 right = rightIR.read_u16();
cudaChen 2:1dcd81fdef9e 40
cudaChen 2:1dcd81fdef9e 41 values = left * (-1) + middle * 0 + right * 1;
cudaChen 2:1dcd81fdef9e 42
cudaChen 4:982dcc2390a2 43 //pc.printf("left middle right: %d %d %d\r\n", left, middle, right);
cudaChen 4:982dcc2390a2 44 //wait(1);
cudaChen 2:1dcd81fdef9e 45 //pc.printf("IR values: %d\r\n", ret);
cudaChen 2:1dcd81fdef9e 46 }
cudaChen 2:1dcd81fdef9e 47
cudaChen 2:1dcd81fdef9e 48 void runPID() {
cudaChen 2:1dcd81fdef9e 49 int error = values;
cudaChen 2:1dcd81fdef9e 50 interror += error;
cudaChen 2:1dcd81fdef9e 51 int lasterror = error - olderror;
cudaChen 2:1dcd81fdef9e 52 olderror = error;
cudaChen 2:1dcd81fdef9e 53 int power = error * Kp + interror * Ki + lasterror * Kd;
cudaChen 2:1dcd81fdef9e 54
cudaChen 2:1dcd81fdef9e 55 // limit the output of PID value
cudaChen 2:1dcd81fdef9e 56 if(power > limit) {
cudaChen 2:1dcd81fdef9e 57 power = limit;
cudaChen 2:1dcd81fdef9e 58 }
cudaChen 2:1dcd81fdef9e 59 if(power < -limit) {
cudaChen 2:1dcd81fdef9e 60 power = -limit;
cudaChen 2:1dcd81fdef9e 61 }
cudaChen 2:1dcd81fdef9e 62
cudaChen 2:1dcd81fdef9e 63 // PID works here
cudaChen 2:1dcd81fdef9e 64 if(power > 0) {
cudaChen 2:1dcd81fdef9e 65 speed(limit, limit - power);
cudaChen 2:1dcd81fdef9e 66 } else {
cudaChen 2:1dcd81fdef9e 67 speed(limit + power, limit);
cudaChen 2:1dcd81fdef9e 68 }
cudaChen 2:1dcd81fdef9e 69 }
cudaChen 2:1dcd81fdef9e 70
cudaChen 2:1dcd81fdef9e 71 void init() {
cudaChen 2:1dcd81fdef9e 72 speed(0, 0);
cudaChen 2:1dcd81fdef9e 73 }
cudaChen 2:1dcd81fdef9e 74
cudaChen 2:1dcd81fdef9e 75 void speed(int left, int right) {
cudaChen 2:1dcd81fdef9e 76 // control right wheel
cudaChen 2:1dcd81fdef9e 77 if(right < 0) {
cudaChen 2:1dcd81fdef9e 78 // go backward
cudaChen 2:1dcd81fdef9e 79 right = -right;
cudaChen 2:1dcd81fdef9e 80 M1_in1 = 0;
cudaChen 2:1dcd81fdef9e 81 M1_in2 = 1;
cudaChen 2:1dcd81fdef9e 82 } else if(right == 0) {
cudaChen 2:1dcd81fdef9e 83 // stop
cudaChen 2:1dcd81fdef9e 84 M1_in1 = 0;
cudaChen 2:1dcd81fdef9e 85 M1_in2 = 0;
cudaChen 2:1dcd81fdef9e 86 } else {
cudaChen 2:1dcd81fdef9e 87 // go forward
cudaChen 2:1dcd81fdef9e 88 M1_in1 = 1;
cudaChen 2:1dcd81fdef9e 89 M1_in2 = 0;
cudaChen 2:1dcd81fdef9e 90 }
cudaChen 2:1dcd81fdef9e 91 M1_enable.write_u16(right);
cudaChen 2:1dcd81fdef9e 92
cudaChen 2:1dcd81fdef9e 93 // control left wheel
cudaChen 2:1dcd81fdef9e 94 if(left < 0) {
cudaChen 2:1dcd81fdef9e 95 // go backward
cudaChen 2:1dcd81fdef9e 96 left = -left;
cudaChen 2:1dcd81fdef9e 97 M2_in3 = 0;
cudaChen 2:1dcd81fdef9e 98 M2_in4 = 1;
cudaChen 2:1dcd81fdef9e 99 } else if(left == 0) {
cudaChen 2:1dcd81fdef9e 100 // stop
cudaChen 2:1dcd81fdef9e 101 M2_in3 = 0;
cudaChen 2:1dcd81fdef9e 102 M2_in4 = 0;
cudaChen 2:1dcd81fdef9e 103 } else {
cudaChen 2:1dcd81fdef9e 104 // go forward
cudaChen 2:1dcd81fdef9e 105 M2_in3 = 1;
cudaChen 2:1dcd81fdef9e 106 M2_in4 = 0;
cudaChen 2:1dcd81fdef9e 107 }
cudaChen 2:1dcd81fdef9e 108 M2_enable.write_u16(left);
cudaChen 2:1dcd81fdef9e 109 }