GDP 4 / Mbed 2 deprecated pid-car-example

car using PID from centre line

Dependencies:   FRDM-TFC mbed CBuffer XBEE mbed_angular_speed motor2 MMA8451Q

Fork of KL25Z_Camera_Test by GDP 4

main.cpp@8:7c5e6b1e7aa5, 2016-11-14 (annotated)

Committer:
maximusismax
Date:
Mon Nov 14 13:42:22 2016 +0000
Revision:
8:7c5e6b1e7aa5
Parent:
7:ad893fc41b95
Child:
9:aa2ce38dec6b
Cleaned up, added lots of stuff

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maximusismax 8:7c5e6b1e7aa5 1 //Autonomous Car GDP controller
maximusismax 8:7c5e6b1e7aa5 2 //Written by various group members
maximusismax 8:7c5e6b1e7aa5 3 //Commented & cleaned up by Max/Adam
maximusismax 8:7c5e6b1e7aa5 4
maximusismax 8:7c5e6b1e7aa5 5 //To-do
maximusismax 8:7c5e6b1e7aa5 6 // -Change xbee transmission to non-blocking
maximusismax 8:7c5e6b1e7aa5 7 // -Improve start/stop detection and resultant action (setting PID values?)
maximusismax 8:7c5e6b1e7aa5 8
maximusismax 8:7c5e6b1e7aa5 9 #include <stdarg.h>
maximusismax 8:7c5e6b1e7aa5 10 #include <stdio.h>
maximusismax 8:7c5e6b1e7aa5 11
maximusismax 0:566127ca8048 12 #include "mbed.h"
maximusismax 0:566127ca8048 13 #include "TFC.h"
FatCookies 4:4afa448c9cce 14 #include "XBEE.h"
maximusismax 8:7c5e6b1e7aa5 15
maximusismax 8:7c5e6b1e7aa5 16 #define BAUD_RATE 57600
FatCookies 6:b0e160c51013 17 #define CAM_THRESHOLD 109
maximusismax 8:7c5e6b1e7aa5 18 #define CAM_DIFF 10
maximusismax 8:7c5e6b1e7aa5 19
FatCookies 3:87a5122682fa 20 //Serial pc(USBTX,USBRX);
FatCookies 3:87a5122682fa 21 Serial pc(PTD3,PTD2);
FatCookies 4:4afa448c9cce 22 XBEE xb(&pc);
FatCookies 3:87a5122682fa 23
maximusismax 8:7c5e6b1e7aa5 24 //Woo global variables!
maximusismax 8:7c5e6b1e7aa5 25 bool onTrack;
FatCookies 3:87a5122682fa 26 char curr_line[128];
FatCookies 3:87a5122682fa 27 uint8_t curr_left;
FatCookies 3:87a5122682fa 28 uint8_t curr_right;
FatCookies 3:87a5122682fa 29 uint8_t right;
FatCookies 3:87a5122682fa 30 uint8_t left;
maximusismax 8:7c5e6b1e7aa5 31 int8_t leftChange;
maximusismax 8:7c5e6b1e7aa5 32 int8_t rightChange;
maximusismax 8:7c5e6b1e7aa5 33 int diff;
maximusismax 8:7c5e6b1e7aa5 34 int prev;
maximusismax 8:7c5e6b1e7aa5 35 int i = 0;
maximusismax 8:7c5e6b1e7aa5 36 float measuredValBuffer[5];
maximusismax 8:7c5e6b1e7aa5 37 uint8_t valBufferIndex;
maximusismax 8:7c5e6b1e7aa5 38
maximusismax 8:7c5e6b1e7aa5 39 //Some PID variables
maximusismax 8:7c5e6b1e7aa5 40 float Kp;
maximusismax 8:7c5e6b1e7aa5 41 float Ki;
maximusismax 8:7c5e6b1e7aa5 42 float Kd;
maximusismax 8:7c5e6b1e7aa5 43 float dt;
maximusismax 8:7c5e6b1e7aa5 44 float p_error;
maximusismax 8:7c5e6b1e7aa5 45 float pid_error;
maximusismax 8:7c5e6b1e7aa5 46 float integral;
maximusismax 8:7c5e6b1e7aa5 47 float measured_value, desired_value, derivative;
maximusismax 8:7c5e6b1e7aa5 48 float output;
FatCookies 3:87a5122682fa 49
FatCookies 3:87a5122682fa 50 Timer t;
FatCookies 4:4afa448c9cce 51
FatCookies 4:4afa448c9cce 52 char curr_cmd = 0;
FatCookies 4:4afa448c9cce 53
FatCookies 4:4afa448c9cce 54 float speed = 0.3;
FatCookies 6:b0e160c51013 55 int frame_counter = 0;
maximusismax 8:7c5e6b1e7aa5 56
maximusismax 8:7c5e6b1e7aa5 57 //Hacky start/stop signal detection
maximusismax 8:7c5e6b1e7aa5 58 int startstop = 0;
FatCookies 7:ad893fc41b95 59 bool seen = false;
maximusismax 8:7c5e6b1e7aa5 60
maximusismax 8:7c5e6b1e7aa5 61 void sendString(const char *format, ...);
maximusismax 8:7c5e6b1e7aa5 62 void initVariables();
maximusismax 8:7c5e6b1e7aa5 63 inline void handleComms();
maximusismax 8:7c5e6b1e7aa5 64 inline float findCentreValue();
maximusismax 8:7c5e6b1e7aa5 65 inline void PIDController();
maximusismax 8:7c5e6b1e7aa5 66 inline void sendImage();
maximusismax 8:7c5e6b1e7aa5 67
maximusismax 0:566127ca8048 68 int main() {
maximusismax 8:7c5e6b1e7aa5 69 //Set up TFC driver stuff
maximusismax 0:566127ca8048 70 TFC_Init();
FatCookies 3:87a5122682fa 71 TFC_InitServos(0.00052,0.00122,0.02);
maximusismax 8:7c5e6b1e7aa5 72 //Old things to make the car move at run-time
maximusismax 8:7c5e6b1e7aa5 73 //Should tie these to a button for the actual races
FatCookies 4:4afa448c9cce 74 //TFC_HBRIDGE_ENABLE;
FatCookies 4:4afa448c9cce 75 //TFC_SetMotorPWM(0.3,0.3);
maximusismax 2:4b6f6fc84793 76
maximusismax 8:7c5e6b1e7aa5 77 //Setup baud rate for serial link, do not change!
maximusismax 8:7c5e6b1e7aa5 78 pc.baud(BAUD_RATE);
maximusismax 0:566127ca8048 79
maximusismax 8:7c5e6b1e7aa5 80 //Initialise/reset PID variables
maximusismax 8:7c5e6b1e7aa5 81 initVariables();
maximusismax 0:566127ca8048 82
maximusismax 0:566127ca8048 83 while(1) {
FatCookies 3:87a5122682fa 84
maximusismax 8:7c5e6b1e7aa5 85 handleComms();
maximusismax 8:7c5e6b1e7aa5 86
maximusismax 8:7c5e6b1e7aa5 87 //If we have an image ready
maximusismax 8:7c5e6b1e7aa5 88 if(TFC_LineScanImageReady>0) {
maximusismax 8:7c5e6b1e7aa5 89 measured_value = findCentreValue();
maximusismax 8:7c5e6b1e7aa5 90
maximusismax 8:7c5e6b1e7aa5 91 PIDController();
maximusismax 8:7c5e6b1e7aa5 92
maximusismax 8:7c5e6b1e7aa5 93 sendImage();
maximusismax 8:7c5e6b1e7aa5 94
maximusismax 8:7c5e6b1e7aa5 95 //Hacky way to detect the start/stop signal
maximusismax 8:7c5e6b1e7aa5 96 if(right - left < 60) {
maximusismax 8:7c5e6b1e7aa5 97 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 98 pc.printf("START STOP!! &d",startstop);
maximusismax 8:7c5e6b1e7aa5 99 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 100
maximusismax 8:7c5e6b1e7aa5 101 if(seen) {
maximusismax 8:7c5e6b1e7aa5 102 seen = false;
maximusismax 8:7c5e6b1e7aa5 103 } else {
maximusismax 8:7c5e6b1e7aa5 104 startstop++;
maximusismax 8:7c5e6b1e7aa5 105 seen = true;
maximusismax 8:7c5e6b1e7aa5 106 }
maximusismax 8:7c5e6b1e7aa5 107 //If we've done 5 laps, stop the car
maximusismax 8:7c5e6b1e7aa5 108 if(startstop >= 1) {
maximusismax 8:7c5e6b1e7aa5 109 TFC_SetMotorPWM(0.0,0.0);
maximusismax 8:7c5e6b1e7aa5 110 TFC_HBRIDGE_DISABLE;
maximusismax 8:7c5e6b1e7aa5 111 startstop = 0;
maximusismax 8:7c5e6b1e7aa5 112 }
maximusismax 8:7c5e6b1e7aa5 113 }
maximusismax 8:7c5e6b1e7aa5 114
maximusismax 8:7c5e6b1e7aa5 115 //Reset image ready flag
maximusismax 8:7c5e6b1e7aa5 116 TFC_LineScanImageReady=0;
maximusismax 8:7c5e6b1e7aa5 117 }
maximusismax 8:7c5e6b1e7aa5 118 }
maximusismax 8:7c5e6b1e7aa5 119 }
maximusismax 8:7c5e6b1e7aa5 120
maximusismax 8:7c5e6b1e7aa5 121 void sendString(const char *format, ...) {
maximusismax 8:7c5e6b1e7aa5 122 va_list arg;
maximusismax 8:7c5e6b1e7aa5 123
maximusismax 8:7c5e6b1e7aa5 124 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 125 va_start (arg, format);
maximusismax 8:7c5e6b1e7aa5 126 pc.vprintf(format,arg);
maximusismax 8:7c5e6b1e7aa5 127 va_end (arg);
maximusismax 8:7c5e6b1e7aa5 128 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 129 }
maximusismax 8:7c5e6b1e7aa5 130
maximusismax 8:7c5e6b1e7aa5 131 void initVariables() {
maximusismax 8:7c5e6b1e7aa5 132 //Tunable PID variables
maximusismax 8:7c5e6b1e7aa5 133 Kp = 125 / 25.0f;
maximusismax 8:7c5e6b1e7aa5 134 Ki = 12.0f / 25.0f;
maximusismax 8:7c5e6b1e7aa5 135 Kd = 0.0f;
maximusismax 8:7c5e6b1e7aa5 136 dt = 0;
maximusismax 8:7c5e6b1e7aa5 137 p_error = 0;
maximusismax 8:7c5e6b1e7aa5 138 pid_error = 0;
maximusismax 8:7c5e6b1e7aa5 139 integral = 0;
maximusismax 8:7c5e6b1e7aa5 140 measured_value = 0;
maximusismax 8:7c5e6b1e7aa5 141 desired_value = 0;
maximusismax 8:7c5e6b1e7aa5 142 derivative = 0;
maximusismax 8:7c5e6b1e7aa5 143
maximusismax 8:7c5e6b1e7aa5 144 valBufferIndex = 0;
maximusismax 8:7c5e6b1e7aa5 145 //Measured value is a float between -1.0 and 1.0 (from left to right)
maximusismax 8:7c5e6b1e7aa5 146 //Desired value is always 0.0 (as in, car is in the middle of the road)
maximusismax 8:7c5e6b1e7aa5 147 }
maximusismax 8:7c5e6b1e7aa5 148
maximusismax 8:7c5e6b1e7aa5 149 inline void sendImage() {
maximusismax 8:7c5e6b1e7aa5 150 //Only send 1/3 of camera frames to GUI program
maximusismax 8:7c5e6b1e7aa5 151 if((frame_counter % 3) == 0) {
maximusismax 8:7c5e6b1e7aa5 152 pc.putc('H');
maximusismax 8:7c5e6b1e7aa5 153 for(i = 0; i < 128; i++) {
maximusismax 8:7c5e6b1e7aa5 154 pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);
maximusismax 8:7c5e6b1e7aa5 155 }
maximusismax 8:7c5e6b1e7aa5 156 }
maximusismax 8:7c5e6b1e7aa5 157 frame_counter++;
maximusismax 8:7c5e6b1e7aa5 158 }
maximusismax 8:7c5e6b1e7aa5 159
maximusismax 8:7c5e6b1e7aa5 160 inline void handleComms() {
maximusismax 8:7c5e6b1e7aa5 161 if(curr_cmd != 0) {
FatCookies 4:4afa448c9cce 162 switch(curr_cmd) {
FatCookies 4:4afa448c9cce 163 case 'A':
FatCookies 4:4afa448c9cce 164 if(xb.cBuffer->available() >= 3) {
FatCookies 4:4afa448c9cce 165 char p = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 166 char i = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 167 char d = xb.cBuffer->read();
FatCookies 6:b0e160c51013 168 Kp = p/25.0f;
FatCookies 6:b0e160c51013 169 Ki = i/25.0f;
FatCookies 6:b0e160c51013 170 Kd = d/25.0f;
FatCookies 4:4afa448c9cce 171 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 172 pc.printf("pid change, Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", Kp, Ki, Kd, p, i, d);
FatCookies 6:b0e160c51013 173 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 174
FatCookies 4:4afa448c9cce 175 curr_cmd = 0;
FatCookies 4:4afa448c9cce 176 }
FatCookies 4:4afa448c9cce 177 break;
FatCookies 4:4afa448c9cce 178
FatCookies 4:4afa448c9cce 179 case 'F':
FatCookies 6:b0e160c51013 180 if(xb.cBuffer->available() >= 1) {
FatCookies 4:4afa448c9cce 181 char a = xb.cBuffer->read();
FatCookies 6:b0e160c51013 182 speed = a/256.0f;
FatCookies 7:ad893fc41b95 183 TFC_SetMotorPWM(speed,speed);
FatCookies 4:4afa448c9cce 184 pc.putc('E');
FatCookies 6:b0e160c51013 185 pc.printf("s = %u %f",a, speed);
FatCookies 6:b0e160c51013 186 pc.putc(0);
FatCookies 4:4afa448c9cce 187 curr_cmd = 0;
FatCookies 4:4afa448c9cce 188
FatCookies 4:4afa448c9cce 189 }
FatCookies 4:4afa448c9cce 190 break;
FatCookies 4:4afa448c9cce 191
FatCookies 4:4afa448c9cce 192 default:
FatCookies 4:4afa448c9cce 193 break;
FatCookies 4:4afa448c9cce 194 }
FatCookies 4:4afa448c9cce 195 }
FatCookies 4:4afa448c9cce 196
FatCookies 6:b0e160c51013 197 if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
FatCookies 4:4afa448c9cce 198 char cmd = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 199 if(cmd == 'D') {
FatCookies 4:4afa448c9cce 200 TFC_InitServos(0.00052,0.00122,0.02);
FatCookies 4:4afa448c9cce 201 TFC_HBRIDGE_ENABLE;
FatCookies 4:4afa448c9cce 202 TFC_SetMotorPWM(speed,speed);
FatCookies 6:b0e160c51013 203 integral = 0;
FatCookies 6:b0e160c51013 204
FatCookies 4:4afa448c9cce 205 } else if (cmd == 'C') {
FatCookies 4:4afa448c9cce 206 TFC_SetMotorPWM(0.0,0.0);
FatCookies 4:4afa448c9cce 207 TFC_HBRIDGE_DISABLE;
FatCookies 4:4afa448c9cce 208 } else if(cmd == 'A') {
FatCookies 4:4afa448c9cce 209 curr_cmd = 'A';
FatCookies 4:4afa448c9cce 210 } else if(cmd == 'F') {
FatCookies 4:4afa448c9cce 211 curr_cmd = 'F';
FatCookies 4:4afa448c9cce 212 }
FatCookies 4:4afa448c9cce 213
FatCookies 4:4afa448c9cce 214 }
maximusismax 8:7c5e6b1e7aa5 215 }
maximusismax 8:7c5e6b1e7aa5 216 inline float findCentreValue() {
maximusismax 8:7c5e6b1e7aa5 217 float measuredValue;
maximusismax 8:7c5e6b1e7aa5 218
maximusismax 8:7c5e6b1e7aa5 219 diff = 0;
maximusismax 8:7c5e6b1e7aa5 220 prev = -1;
maximusismax 8:7c5e6b1e7aa5 221 leftChange = left;
maximusismax 8:7c5e6b1e7aa5 222 for(i = 63; i > 0; i--) {
maximusismax 8:7c5e6b1e7aa5 223 curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
maximusismax 8:7c5e6b1e7aa5 224 diff = prev - curr_left;
maximusismax 8:7c5e6b1e7aa5 225 if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
maximusismax 8:7c5e6b1e7aa5 226 left = i;
maximusismax 8:7c5e6b1e7aa5 227 break;
maximusismax 8:7c5e6b1e7aa5 228 }
maximusismax 8:7c5e6b1e7aa5 229 prev = curr_left;
maximusismax 8:7c5e6b1e7aa5 230 }
maximusismax 8:7c5e6b1e7aa5 231
maximusismax 8:7c5e6b1e7aa5 232 prev = -1;
maximusismax 8:7c5e6b1e7aa5 233 rightChange = right;
maximusismax 8:7c5e6b1e7aa5 234 for(i = 64; i < 128; i++) {
maximusismax 8:7c5e6b1e7aa5 235 curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
maximusismax 8:7c5e6b1e7aa5 236 int diff = prev - curr_right;
maximusismax 8:7c5e6b1e7aa5 237 if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
maximusismax 8:7c5e6b1e7aa5 238 right = i;
maximusismax 8:7c5e6b1e7aa5 239 break;
maximusismax 8:7c5e6b1e7aa5 240 }
maximusismax 8:7c5e6b1e7aa5 241 prev = curr_right;
maximusismax 8:7c5e6b1e7aa5 242 }
maximusismax 8:7c5e6b1e7aa5 243
maximusismax 8:7c5e6b1e7aa5 244 //Calculate how left/right we are
maximusismax 8:7c5e6b1e7aa5 245 measuredValue = (64 - ((left+right)/2))/64.f;
maximusismax 8:7c5e6b1e7aa5 246 measuredValBuffer[valBufferIndex % 5] = measuredValue;
maximusismax 8:7c5e6b1e7aa5 247 valBufferIndex++;
maximusismax 8:7c5e6b1e7aa5 248
maximusismax 8:7c5e6b1e7aa5 249 return measuredValue;
maximusismax 8:7c5e6b1e7aa5 250 }
FatCookies 3:87a5122682fa 251
maximusismax 8:7c5e6b1e7aa5 252 inline void PIDController() {
maximusismax 8:7c5e6b1e7aa5 253 //PID Stuff!
FatCookies 3:87a5122682fa 254 t.start();
maximusismax 8:7c5e6b1e7aa5 255 dt = t.read();
maximusismax 8:7c5e6b1e7aa5 256 pid_error = desired_value - measured_value;
maximusismax 8:7c5e6b1e7aa5 257 integral = integral + pid_error * dt;
maximusismax 8:7c5e6b1e7aa5 258 derivative = (pid_error - p_error) / dt;
maximusismax 8:7c5e6b1e7aa5 259 output = Kp * pid_error + Ki * integral + Kd * derivative;
maximusismax 8:7c5e6b1e7aa5 260 p_error = pid_error;
maximusismax 8:7c5e6b1e7aa5 261
maximusismax 8:7c5e6b1e7aa5 262 if(integral > 1.0f) {
FatCookies 6:b0e160c51013 263 integral = 1.0f;
FatCookies 6:b0e160c51013 264 }
FatCookies 6:b0e160c51013 265 if(integral < -1.0f) {
FatCookies 6:b0e160c51013 266 integral = -1.0f;
FatCookies 6:b0e160c51013 267 }
FatCookies 6:b0e160c51013 268
maximusismax 8:7c5e6b1e7aa5 269 if((-1.0 <= output) && (output <= 1.0))
FatCookies 6:b0e160c51013 270 {
maximusismax 8:7c5e6b1e7aa5 271 TFC_SetServo(0, output);
maximusismax 8:7c5e6b1e7aa5 272 }
maximusismax 8:7c5e6b1e7aa5 273 else //Unhappy PID state
maximusismax 8:7c5e6b1e7aa5 274 {
maximusismax 8:7c5e6b1e7aa5 275 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 276 pc.printf("pid unhappy");
maximusismax 8:7c5e6b1e7aa5 277 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 278 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 279 pc.printf("out = %f p_err = %f", output, p_error);
maximusismax 8:7c5e6b1e7aa5 280 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 281 TFC_InitServos(0.00052, 0.00122, 0.02);
maximusismax 8:7c5e6b1e7aa5 282 //output, pid_error, p_error, integral, derivative = 0;
maximusismax 8:7c5e6b1e7aa5 283
maximusismax 8:7c5e6b1e7aa5 284 if(output >= 1.0f) {
maximusismax 8:7c5e6b1e7aa5 285 TFC_SetServo(0, 0.9f);
maximusismax 8:7c5e6b1e7aa5 286 output = 1.0f;
maximusismax 8:7c5e6b1e7aa5 287 } else {
maximusismax 8:7c5e6b1e7aa5 288 TFC_SetServo(0, -0.9f);
maximusismax 8:7c5e6b1e7aa5 289 output = -1.0f;
maximusismax 8:7c5e6b1e7aa5 290 }
maximusismax 8:7c5e6b1e7aa5 291 }
FatCookies 6:b0e160c51013 292
FatCookies 3:87a5122682fa 293 t.stop();
FatCookies 3:87a5122682fa 294 t.reset();
FatCookies 3:87a5122682fa 295 t.start();
maximusismax 0:566127ca8048 296 }