Lu-Hsuan Chen / Mbed OS motorcar_pid

motor car with PID running feature

Dependencies:   Ping

motorcar/motorcar.cpp@2:1dcd81fdef9e, 2018-07-31 (annotated)

Committer:
cudaChen
Date:
Tue Jul 31 08:09:05 2018 +0000
Revision:
2:1dcd81fdef9e
Child:
3:4be8f486a120
[add] add files motorcar.h and motorcar.cpp

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 2:1dcd81fdef9e 7 AnalogOut M1_enable(D6);
cudaChen 2:1dcd81fdef9e 8 AnalogOut M2_enable(D5);
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 2:1dcd81fdef9e 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 2:1dcd81fdef9e 43 //pc.printf("left middle right: %d %d %d\r\n", left, middle, right);
cudaChen 2:1dcd81fdef9e 44 //pc.printf("IR values: %d\r\n", ret);
cudaChen 2:1dcd81fdef9e 45 }
cudaChen 2:1dcd81fdef9e 46
cudaChen 2:1dcd81fdef9e 47 void runPID() {
cudaChen 2:1dcd81fdef9e 48 int error = values;
cudaChen 2:1dcd81fdef9e 49 interror += error;
cudaChen 2:1dcd81fdef9e 50 int lasterror = error - olderror;
cudaChen 2:1dcd81fdef9e 51 olderror = error;
cudaChen 2:1dcd81fdef9e 52 int power = error * Kp + interror * Ki + lasterror * Kd;
cudaChen 2:1dcd81fdef9e 53
cudaChen 2:1dcd81fdef9e 54 // limit the output of PID value
cudaChen 2:1dcd81fdef9e 55 if(power > limit) {
cudaChen 2:1dcd81fdef9e 56 power = limit;
cudaChen 2:1dcd81fdef9e 57 }
cudaChen 2:1dcd81fdef9e 58 if(power < -limit) {
cudaChen 2:1dcd81fdef9e 59 power = -limit;
cudaChen 2:1dcd81fdef9e 60 }
cudaChen 2:1dcd81fdef9e 61
cudaChen 2:1dcd81fdef9e 62 // PID works here
cudaChen 2:1dcd81fdef9e 63 if(power > 0) {
cudaChen 2:1dcd81fdef9e 64 speed(limit, limit - power);
cudaChen 2:1dcd81fdef9e 65 } else {
cudaChen 2:1dcd81fdef9e 66 speed(limit + power, limit);
cudaChen 2:1dcd81fdef9e 67 }
cudaChen 2:1dcd81fdef9e 68 }
cudaChen 2:1dcd81fdef9e 69
cudaChen 2:1dcd81fdef9e 70 void init() {
cudaChen 2:1dcd81fdef9e 71 speed(0, 0);
cudaChen 2:1dcd81fdef9e 72 }
cudaChen 2:1dcd81fdef9e 73
cudaChen 2:1dcd81fdef9e 74 void speed(int left, int right) {
cudaChen 2:1dcd81fdef9e 75 // control right wheel
cudaChen 2:1dcd81fdef9e 76 if(right < 0) {
cudaChen 2:1dcd81fdef9e 77 // go backward
cudaChen 2:1dcd81fdef9e 78 right = -right;
cudaChen 2:1dcd81fdef9e 79 M1_in1 = 0;
cudaChen 2:1dcd81fdef9e 80 M1_in2 = 1;
cudaChen 2:1dcd81fdef9e 81 } else if(right == 0) {
cudaChen 2:1dcd81fdef9e 82 // stop
cudaChen 2:1dcd81fdef9e 83 M1_in1 = 0;
cudaChen 2:1dcd81fdef9e 84 M1_in2 = 0;
cudaChen 2:1dcd81fdef9e 85 } else {
cudaChen 2:1dcd81fdef9e 86 // go forward
cudaChen 2:1dcd81fdef9e 87 M1_in1 = 1;
cudaChen 2:1dcd81fdef9e 88 M1_in2 = 0;
cudaChen 2:1dcd81fdef9e 89 }
cudaChen 2:1dcd81fdef9e 90 M1_enable.write_u16(right);
cudaChen 2:1dcd81fdef9e 91
cudaChen 2:1dcd81fdef9e 92 // control left wheel
cudaChen 2:1dcd81fdef9e 93 if(left < 0) {
cudaChen 2:1dcd81fdef9e 94 // go backward
cudaChen 2:1dcd81fdef9e 95 left = -left;
cudaChen 2:1dcd81fdef9e 96 M2_in3 = 0;
cudaChen 2:1dcd81fdef9e 97 M2_in4 = 1;
cudaChen 2:1dcd81fdef9e 98 } else if(left == 0) {
cudaChen 2:1dcd81fdef9e 99 // stop
cudaChen 2:1dcd81fdef9e 100 M2_in3 = 0;
cudaChen 2:1dcd81fdef9e 101 M2_in4 = 0;
cudaChen 2:1dcd81fdef9e 102 } else {
cudaChen 2:1dcd81fdef9e 103 // go forward
cudaChen 2:1dcd81fdef9e 104 M2_in3 = 1;
cudaChen 2:1dcd81fdef9e 105 M2_in4 = 0;
cudaChen 2:1dcd81fdef9e 106 }
cudaChen 2:1dcd81fdef9e 107 M2_enable.write_u16(left);
cudaChen 2:1dcd81fdef9e 108 }