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@7:ad893fc41b95, 2016-11-14 (annotated)

Committer:
FatCookies
Date:
Mon Nov 14 11:11:13 2016 +0000
Revision:
7:ad893fc41b95
Parent:
6:b0e160c51013
Child:
8:7c5e6b1e7aa5
added basic start/stop line detection

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maximusismax 0:566127ca8048 1 #include "mbed.h"
maximusismax 0:566127ca8048 2 #include "TFC.h"
FatCookies 4:4afa448c9cce 3 #include "XBEE.h"
FatCookies 6:b0e160c51013 4 #define CAM_THRESHOLD 109
FatCookies 3:87a5122682fa 5
maximusismax 0:566127ca8048 6
maximusismax 0:566127ca8048 7 DigitalOut myled(LED1);
FatCookies 3:87a5122682fa 8 //Serial pc(USBTX,USBRX);
FatCookies 3:87a5122682fa 9 Serial pc(PTD3,PTD2);
FatCookies 4:4afa448c9cce 10 XBEE xb(&pc);
FatCookies 3:87a5122682fa 11
FatCookies 3:87a5122682fa 12 char curr_line[128];
FatCookies 3:87a5122682fa 13 uint8_t curr_left;
FatCookies 3:87a5122682fa 14 uint8_t curr_right;
FatCookies 3:87a5122682fa 15
FatCookies 3:87a5122682fa 16 uint8_t right;
FatCookies 3:87a5122682fa 17 uint8_t left;
FatCookies 3:87a5122682fa 18
FatCookies 3:87a5122682fa 19 Timer t;
FatCookies 4:4afa448c9cce 20
FatCookies 4:4afa448c9cce 21 char curr_cmd = 0;
FatCookies 4:4afa448c9cce 22
FatCookies 4:4afa448c9cce 23 float speed = 0.3;
FatCookies 6:b0e160c51013 24 int frame_counter = 0;
maximusismax 0:566127ca8048 25
FatCookies 7:ad893fc41b95 26 int startstop = 0;
FatCookies 7:ad893fc41b95 27 bool seen = false;
FatCookies 7:ad893fc41b95 28
maximusismax 0:566127ca8048 29 int main() {
maximusismax 0:566127ca8048 30 TFC_Init();
FatCookies 3:87a5122682fa 31 TFC_InitServos(0.00052,0.00122,0.02);
FatCookies 4:4afa448c9cce 32 //TFC_HBRIDGE_ENABLE;
FatCookies 4:4afa448c9cce 33 //TFC_SetMotorPWM(0.3,0.3);
maximusismax 2:4b6f6fc84793 34
maximusismax 2:4b6f6fc84793 35
FatCookies 3:87a5122682fa 36 pc.baud(57600);
maximusismax 0:566127ca8048 37
FatCookies 3:87a5122682fa 38 float p_error, error;
FatCookies 3:87a5122682fa 39 float integral;
FatCookies 3:87a5122682fa 40 float measured_value, desired_value,derivative;
FatCookies 3:87a5122682fa 41 float output;
FatCookies 3:87a5122682fa 42 //tunable variables
FatCookies 3:87a5122682fa 43 float Kp, Ki,Kd;
FatCookies 6:b0e160c51013 44 Kp=125/25.0f;
FatCookies 6:b0e160c51013 45 Ki=12.0f/25.0f;
FatCookies 4:4afa448c9cce 46 Kd=0.0;
FatCookies 3:87a5122682fa 47 myled = 0;// Test
FatCookies 3:87a5122682fa 48 float dt=0;
FatCookies 3:87a5122682fa 49 p_error=0;
FatCookies 3:87a5122682fa 50 error=0;
FatCookies 3:87a5122682fa 51 integral=0;
FatCookies 3:87a5122682fa 52 measured_value= 0;
FatCookies 3:87a5122682fa 53 desired_value=0;
FatCookies 3:87a5122682fa 54 derivative=0;
FatCookies 3:87a5122682fa 55 // measured value is a float between -1.0 and 1.0
FatCookies 3:87a5122682fa 56 // desired value is always 0 ( as in car is in the middle of the road)
maximusismax 0:566127ca8048 57
FatCookies 3:87a5122682fa 58 uint8_t i = 0;
maximusismax 0:566127ca8048 59 while(1) {
FatCookies 3:87a5122682fa 60
FatCookies 4:4afa448c9cce 61 if(curr_cmd != 0) {
FatCookies 4:4afa448c9cce 62 switch(curr_cmd) {
FatCookies 4:4afa448c9cce 63 case 'A':
FatCookies 4:4afa448c9cce 64 if(xb.cBuffer->available() >= 3) {
FatCookies 4:4afa448c9cce 65 char p = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 66 char i = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 67 char d = xb.cBuffer->read();
FatCookies 6:b0e160c51013 68 Kp = p/25.0f;
FatCookies 6:b0e160c51013 69 Ki = i/25.0f;
FatCookies 6:b0e160c51013 70 Kd = d/25.0f;
FatCookies 4:4afa448c9cce 71 pc.putc('E');
FatCookies 6:b0e160c51013 72 pc.printf("pid change");
FatCookies 6:b0e160c51013 73 pc.putc(0);
FatCookies 4:4afa448c9cce 74 curr_cmd = 0;
FatCookies 4:4afa448c9cce 75 }
FatCookies 4:4afa448c9cce 76 break;
FatCookies 4:4afa448c9cce 77
FatCookies 4:4afa448c9cce 78 case 'F':
FatCookies 6:b0e160c51013 79 if(xb.cBuffer->available() >= 1) {
FatCookies 4:4afa448c9cce 80 char a = xb.cBuffer->read();
FatCookies 6:b0e160c51013 81 speed = a/256.0f;
FatCookies 7:ad893fc41b95 82 TFC_SetMotorPWM(speed,speed);
FatCookies 4:4afa448c9cce 83 pc.putc('E');
FatCookies 6:b0e160c51013 84 pc.printf("s = %u %f",a, speed);
FatCookies 6:b0e160c51013 85 pc.putc(0);
FatCookies 4:4afa448c9cce 86 curr_cmd = 0;
FatCookies 4:4afa448c9cce 87
FatCookies 4:4afa448c9cce 88 }
FatCookies 4:4afa448c9cce 89 break;
FatCookies 4:4afa448c9cce 90
FatCookies 4:4afa448c9cce 91 default:
FatCookies 4:4afa448c9cce 92 break;
FatCookies 4:4afa448c9cce 93 }
FatCookies 4:4afa448c9cce 94 }
FatCookies 4:4afa448c9cce 95
FatCookies 6:b0e160c51013 96 if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
FatCookies 4:4afa448c9cce 97 char cmd = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 98 if(cmd == 'D') {
FatCookies 4:4afa448c9cce 99 TFC_InitServos(0.00052,0.00122,0.02);
FatCookies 4:4afa448c9cce 100 TFC_HBRIDGE_ENABLE;
FatCookies 4:4afa448c9cce 101 TFC_SetMotorPWM(speed,speed);
FatCookies 6:b0e160c51013 102 integral = 0;
FatCookies 6:b0e160c51013 103
FatCookies 4:4afa448c9cce 104 } else if (cmd == 'C') {
FatCookies 4:4afa448c9cce 105 TFC_SetMotorPWM(0.0,0.0);
FatCookies 4:4afa448c9cce 106 TFC_HBRIDGE_DISABLE;
FatCookies 4:4afa448c9cce 107 } else if(cmd == 'A') {
FatCookies 4:4afa448c9cce 108 curr_cmd = 'A';
FatCookies 4:4afa448c9cce 109 } else if(cmd == 'F') {
FatCookies 4:4afa448c9cce 110 curr_cmd = 'F';
FatCookies 4:4afa448c9cce 111 }
FatCookies 4:4afa448c9cce 112
FatCookies 4:4afa448c9cce 113 }
FatCookies 4:4afa448c9cce 114
maximusismax 0:566127ca8048 115 //If we have an image ready
FatCookies 3:87a5122682fa 116 if(TFC_LineScanImageReady>0) {
FatCookies 6:b0e160c51013 117 //left = 0;
FatCookies 6:b0e160c51013 118 //right = 0;
FatCookies 6:b0e160c51013 119 int diff = 0;
FatCookies 6:b0e160c51013 120 int prev = -1;
FatCookies 4:4afa448c9cce 121 for(i = 63; i > 0; i--) {
FatCookies 3:87a5122682fa 122 curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
FatCookies 6:b0e160c51013 123 /* if(curr_left < CAM_THRESHOLD) {
FatCookies 3:87a5122682fa 124 left = i;
FatCookies 3:87a5122682fa 125 break;
FatCookies 3:87a5122682fa 126 }
FatCookies 6:b0e160c51013 127 */
FatCookies 6:b0e160c51013 128
FatCookies 6:b0e160c51013 129 diff = prev - curr_left;
FatCookies 7:ad893fc41b95 130 if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
FatCookies 6:b0e160c51013 131 left = i;
FatCookies 6:b0e160c51013 132 break;
FatCookies 6:b0e160c51013 133 }
FatCookies 6:b0e160c51013 134
FatCookies 6:b0e160c51013 135 prev = curr_left;
FatCookies 3:87a5122682fa 136 }
FatCookies 3:87a5122682fa 137
FatCookies 6:b0e160c51013 138 prev = -1;
FatCookies 3:87a5122682fa 139 for(i = 64; i < 128; i++) {
FatCookies 3:87a5122682fa 140 curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
FatCookies 6:b0e160c51013 141 /*
FatCookies 3:87a5122682fa 142 if(curr_right < CAM_THRESHOLD) {
FatCookies 3:87a5122682fa 143 right = i;
FatCookies 3:87a5122682fa 144 break;
maximusismax 0:566127ca8048 145 }
FatCookies 6:b0e160c51013 146 */
FatCookies 6:b0e160c51013 147
FatCookies 6:b0e160c51013 148 int diff = prev - curr_right;
FatCookies 7:ad893fc41b95 149 if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
FatCookies 6:b0e160c51013 150 right = i;
FatCookies 6:b0e160c51013 151 break;
FatCookies 6:b0e160c51013 152 }
FatCookies 6:b0e160c51013 153
FatCookies 6:b0e160c51013 154 prev = curr_right;
FatCookies 3:87a5122682fa 155 }
FatCookies 3:87a5122682fa 156
FatCookies 7:ad893fc41b95 157 if((frame_counter % 3) == 0) {
FatCookies 3:87a5122682fa 158 pc.putc('H');
FatCookies 3:87a5122682fa 159 for(i = 0; i < 128; i++) {
FatCookies 3:87a5122682fa 160 pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);
FatCookies 3:87a5122682fa 161 }
FatCookies 7:ad893fc41b95 162 }
FatCookies 7:ad893fc41b95 163 frame_counter++;
FatCookies 3:87a5122682fa 164
FatCookies 3:87a5122682fa 165 measured_value = (64 - ((left+right)/2))/64.f;
FatCookies 3:87a5122682fa 166
FatCookies 7:ad893fc41b95 167 if(right - left < 60) {
FatCookies 7:ad893fc41b95 168 pc.putc('E');
FatCookies 7:ad893fc41b95 169 pc.printf("START STOP!! &d",startstop);
FatCookies 7:ad893fc41b95 170 pc.putc(0);
FatCookies 7:ad893fc41b95 171
FatCookies 7:ad893fc41b95 172 if(seen) {
FatCookies 7:ad893fc41b95 173 seen = false;
FatCookies 7:ad893fc41b95 174 } else {
FatCookies 7:ad893fc41b95 175 startstop++;
FatCookies 7:ad893fc41b95 176 seen = true;
FatCookies 7:ad893fc41b95 177 }
FatCookies 7:ad893fc41b95 178
FatCookies 7:ad893fc41b95 179 if(startstop >= 5) {
FatCookies 7:ad893fc41b95 180 TFC_SetMotorPWM(0.0,0.0);
FatCookies 7:ad893fc41b95 181 TFC_HBRIDGE_DISABLE;
FatCookies 7:ad893fc41b95 182 startstop = 0;
FatCookies 7:ad893fc41b95 183 }
FatCookies 7:ad893fc41b95 184
FatCookies 7:ad893fc41b95 185 }
FatCookies 6:b0e160c51013 186
FatCookies 3:87a5122682fa 187 t.start();
FatCookies 3:87a5122682fa 188 dt=t.read();
FatCookies 3:87a5122682fa 189 error = desired_value - measured_value;
FatCookies 3:87a5122682fa 190 integral=integral+error*dt;
FatCookies 3:87a5122682fa 191 derivative=(error-p_error)/dt;
FatCookies 3:87a5122682fa 192 output=Kp*error+Ki*integral+Kd*derivative;
FatCookies 3:87a5122682fa 193 p_error=error;
FatCookies 3:87a5122682fa 194
FatCookies 6:b0e160c51013 195
FatCookies 6:b0e160c51013 196 if(integral > 1.0f) {
FatCookies 6:b0e160c51013 197 integral = 1.0f;
FatCookies 6:b0e160c51013 198 }
FatCookies 6:b0e160c51013 199 if(integral < -1.0f) {
FatCookies 6:b0e160c51013 200 integral = -1.0f;
FatCookies 6:b0e160c51013 201 }
FatCookies 6:b0e160c51013 202
FatCookies 6:b0e160c51013 203
FatCookies 3:87a5122682fa 204 if((-1.0<=output)&&(output<=1.0))
FatCookies 3:87a5122682fa 205 {
FatCookies 3:87a5122682fa 206 TFC_SetServo(0,output);
FatCookies 3:87a5122682fa 207
FatCookies 6:b0e160c51013 208 }
FatCookies 3:87a5122682fa 209 else
FatCookies 6:b0e160c51013 210 {
FatCookies 6:b0e160c51013 211
FatCookies 6:b0e160c51013 212 pc.putc('E');
FatCookies 6:b0e160c51013 213 pc.printf("pid unhappy\0");
FatCookies 6:b0e160c51013 214 pc.putc('E');
FatCookies 6:b0e160c51013 215 pc.printf("out = %f p_err = %f\0",output, p_error);
FatCookies 6:b0e160c51013 216 TFC_InitServos(0.00052,0.00122,0.02);
FatCookies 6:b0e160c51013 217 //output, error, p_error, integral, derivative = 0;
FatCookies 6:b0e160c51013 218
FatCookies 6:b0e160c51013 219 if(output >= 1.0f) {
FatCookies 6:b0e160c51013 220 TFC_SetServo(0,0.9f);
FatCookies 6:b0e160c51013 221 output = 1.0f;
FatCookies 6:b0e160c51013 222 } else {
FatCookies 6:b0e160c51013 223 TFC_SetServo(0,-0.9f);
FatCookies 6:b0e160c51013 224 output = -1.0f;
FatCookies 6:b0e160c51013 225 }
FatCookies 6:b0e160c51013 226
FatCookies 6:b0e160c51013 227
FatCookies 6:b0e160c51013 228
FatCookies 6:b0e160c51013 229
FatCookies 6:b0e160c51013 230
FatCookies 3:87a5122682fa 231 }
FatCookies 3:87a5122682fa 232
FatCookies 3:87a5122682fa 233 t.stop();
FatCookies 3:87a5122682fa 234 t.reset();
FatCookies 3:87a5122682fa 235 t.start();
FatCookies 3:87a5122682fa 236
FatCookies 3:87a5122682fa 237
FatCookies 3:87a5122682fa 238
FatCookies 3:87a5122682fa 239
FatCookies 3:87a5122682fa 240 //Reset image ready flag
FatCookies 3:87a5122682fa 241 TFC_LineScanImageReady=0;
FatCookies 7:ad893fc41b95 242 wait(0.02f);
FatCookies 3:87a5122682fa 243 }
maximusismax 0:566127ca8048 244 }
maximusismax 0:566127ca8048 245 }