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@6:b0e160c51013, 2016-11-09 (annotated)

Committer:
FatCookies
Date:
Wed Nov 09 17:54:38 2016 +0000
Revision:
6:b0e160c51013
Parent:
4:4afa448c9cce
Child:
7:ad893fc41b95
changed thresholding to differencing

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