Lu-Hsuan Chen / Mbed OS motorcar_pid

motor car with PID running feature

Dependencies:   Ping

motorcar/motorcar.cpp@5:3caf5b6ed35a, 2018-08-13 (annotated)

Committer:
cudaChen
Date:
Mon Aug 13 08:49:25 2018 +0000
Revision:
5:3caf5b6ed35a
Parent:
4:982dcc2390a2 
[add] add lines for obstacle aviodance

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 5:3caf5b6ed35a 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 5:3caf5b6ed35a 22 // used for ultra sonic sensor
cudaChen 5:3caf5b6ed35a 23 AnalogIn Ping ultraSonic(D12);
cudaChen 5:3caf5b6ed35a 24 int distance;
cudaChen 5:3caf5b6ed35a 25 bool hasObstacle = false;
cudaChen 5:3caf5b6ed35a 26 int range = 20;
cudaChen 5:3caf5b6ed35a 27
cudaChen 2:1dcd81fdef9e 28 // PID factors
cudaChen 2:1dcd81fdef9e 29 int interror = 0;
cudaChen 2:1dcd81fdef9e 30 int olderror = 0;
cudaChen 2:1dcd81fdef9e 31 int values = 0;
cudaChen 2:1dcd81fdef9e 32 float Kp = 1, Ki = 0, Kd = 0;
cudaChen 2:1dcd81fdef9e 33 /* Though STM32 only has 12-bit DAC,
cudaChen 2:1dcd81fdef9e 34 * mbed still cam map the value by
cudaChen 2:1dcd81fdef9e 35 * using write_u16()
cudaChen 2:1dcd81fdef9e 36 */
cudaChen 2:1dcd81fdef9e 37 int limit = 65535; // 2^16 - 1
cudaChen 2:1dcd81fdef9e 38
cudaChen 2:1dcd81fdef9e 39 // used for output message via serial for ease of debugging
cudaChen 4:982dcc2390a2 40 //Serial pc(SERIAL_TX, SERIAL_RX);
cudaChen 2:1dcd81fdef9e 41
cudaChen 2:1dcd81fdef9e 42 void readSensorValue() {
cudaChen 5:3caf5b6ed35a 43 ultraSonic.Send();
cudaChen 5:3caf5b6ed35a 44 wait_ms(30);
cudaChen 5:3caf5b6ed35a 45 distance = ultraSonic.Read_cm();
cudaChen 5:3caf5b6ed35a 46 hasObstacle = distance < range ? true : false;
cudaChen 5:3caf5b6ed35a 47
cudaChen 2:1dcd81fdef9e 48 left = leftIR.read_u16();
cudaChen 2:1dcd81fdef9e 49 middle = middleIR.read_u16();
cudaChen 2:1dcd81fdef9e 50 right = rightIR.read_u16();
cudaChen 2:1dcd81fdef9e 51
cudaChen 2:1dcd81fdef9e 52 values = left * (-1) + middle * 0 + right * 1;
cudaChen 2:1dcd81fdef9e 53
cudaChen 4:982dcc2390a2 54 //pc.printf("left middle right: %d %d %d\r\n", left, middle, right);
cudaChen 4:982dcc2390a2 55 //wait(1);
cudaChen 2:1dcd81fdef9e 56 //pc.printf("IR values: %d\r\n", ret);
cudaChen 2:1dcd81fdef9e 57 }
cudaChen 2:1dcd81fdef9e 58
cudaChen 2:1dcd81fdef9e 59 void runPID() {
cudaChen 5:3caf5b6ed35a 60 // if there is an obstacle
cudaChen 5:3caf5b6ed35a 61 if(hasObstacle) {
cudaChen 5:3caf5b6ed35a 62 speed(0, 0);
cudaChen 5:3caf5b6ed35a 63 wait_ms(200);
cudaChen 5:3caf5b6ed35a 64 speed(0, 200);
cudaChen 5:3caf5b6ed35a 65 wait_ms(200);
cudaChen 5:3caf5b6ed35a 66 speed(0, 0);
cudaChen 5:3caf5b6ed35a 67 wait_ms(200);
cudaChen 5:3caf5b6ed35a 68 speed(200, 200);
cudaChen 5:3caf5b6ed35a 69 wait_ms(800);
cudaChen 5:3caf5b6ed35a 70 speed(0, 0);
cudaChen 5:3caf5b6ed35a 71 wait_ms(200);
cudaChen 5:3caf5b6ed35a 72 speed(200, 0);
cudaChen 5:3caf5b6ed35a 73 wait_ms(200);
cudaChen 5:3caf5b6ed35a 74 speed(0, 0);
cudaChen 5:3caf5b6ed35a 75 wait_ms(200);
cudaChen 5:3caf5b6ed35a 76 return;
cudaChen 5:3caf5b6ed35a 77 }
cudaChen 5:3caf5b6ed35a 78
cudaChen 2:1dcd81fdef9e 79 int error = values;
cudaChen 2:1dcd81fdef9e 80 interror += error;
cudaChen 2:1dcd81fdef9e 81 int lasterror = error - olderror;
cudaChen 2:1dcd81fdef9e 82 olderror = error;
cudaChen 2:1dcd81fdef9e 83 int power = error * Kp + interror * Ki + lasterror * Kd;
cudaChen 2:1dcd81fdef9e 84
cudaChen 2:1dcd81fdef9e 85 // limit the output of PID value
cudaChen 2:1dcd81fdef9e 86 if(power > limit) {
cudaChen 2:1dcd81fdef9e 87 power = limit;
cudaChen 2:1dcd81fdef9e 88 }
cudaChen 2:1dcd81fdef9e 89 if(power < -limit) {
cudaChen 2:1dcd81fdef9e 90 power = -limit;
cudaChen 2:1dcd81fdef9e 91 }
cudaChen 2:1dcd81fdef9e 92
cudaChen 2:1dcd81fdef9e 93 // PID works here
cudaChen 2:1dcd81fdef9e 94 if(power > 0) {
cudaChen 2:1dcd81fdef9e 95 speed(limit, limit - power);
cudaChen 2:1dcd81fdef9e 96 } else {
cudaChen 2:1dcd81fdef9e 97 speed(limit + power, limit);
cudaChen 2:1dcd81fdef9e 98 }
cudaChen 2:1dcd81fdef9e 99 }
cudaChen 2:1dcd81fdef9e 100
cudaChen 2:1dcd81fdef9e 101 void init() {
cudaChen 2:1dcd81fdef9e 102 speed(0, 0);
cudaChen 2:1dcd81fdef9e 103 }
cudaChen 2:1dcd81fdef9e 104
cudaChen 2:1dcd81fdef9e 105 void speed(int left, int right) {
cudaChen 2:1dcd81fdef9e 106 // control right wheel
cudaChen 2:1dcd81fdef9e 107 if(right < 0) {
cudaChen 2:1dcd81fdef9e 108 // go backward
cudaChen 2:1dcd81fdef9e 109 right = -right;
cudaChen 2:1dcd81fdef9e 110 M1_in1 = 0;
cudaChen 2:1dcd81fdef9e 111 M1_in2 = 1;
cudaChen 2:1dcd81fdef9e 112 } else if(right == 0) {
cudaChen 2:1dcd81fdef9e 113 // stop
cudaChen 2:1dcd81fdef9e 114 M1_in1 = 0;
cudaChen 2:1dcd81fdef9e 115 M1_in2 = 0;
cudaChen 2:1dcd81fdef9e 116 } else {
cudaChen 2:1dcd81fdef9e 117 // go forward
cudaChen 2:1dcd81fdef9e 118 M1_in1 = 1;
cudaChen 2:1dcd81fdef9e 119 M1_in2 = 0;
cudaChen 2:1dcd81fdef9e 120 }
cudaChen 2:1dcd81fdef9e 121 M1_enable.write_u16(right);
cudaChen 2:1dcd81fdef9e 122
cudaChen 2:1dcd81fdef9e 123 // control left wheel
cudaChen 2:1dcd81fdef9e 124 if(left < 0) {
cudaChen 2:1dcd81fdef9e 125 // go backward
cudaChen 2:1dcd81fdef9e 126 left = -left;
cudaChen 2:1dcd81fdef9e 127 M2_in3 = 0;
cudaChen 2:1dcd81fdef9e 128 M2_in4 = 1;
cudaChen 2:1dcd81fdef9e 129 } else if(left == 0) {
cudaChen 2:1dcd81fdef9e 130 // stop
cudaChen 2:1dcd81fdef9e 131 M2_in3 = 0;
cudaChen 2:1dcd81fdef9e 132 M2_in4 = 0;
cudaChen 2:1dcd81fdef9e 133 } else {
cudaChen 2:1dcd81fdef9e 134 // go forward
cudaChen 2:1dcd81fdef9e 135 M2_in3 = 1;
cudaChen 2:1dcd81fdef9e 136 M2_in4 = 0;
cudaChen 2:1dcd81fdef9e 137 }
cudaChen 2:1dcd81fdef9e 138 M2_enable.write_u16(left);
cudaChen 2:1dcd81fdef9e 139 }