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@4:4afa448c9cce, 2016-11-03 (annotated)

Committer:
FatCookies
Date:
Thu Nov 03 13:06:16 2016 +0000
Revision:
4:4afa448c9cce
Parent:
3:87a5122682fa
Child:
6:b0e160c51013
add remote control

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 3:87a5122682fa 4 #define CAM_THRESHOLD 128
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;
maximusismax 0:566127ca8048 24
maximusismax 0:566127ca8048 25 int main() {
maximusismax 0:566127ca8048 26 TFC_Init();
FatCookies 3:87a5122682fa 27 TFC_InitServos(0.00052,0.00122,0.02);
FatCookies 4:4afa448c9cce 28 //TFC_HBRIDGE_ENABLE;
FatCookies 4:4afa448c9cce 29 //TFC_SetMotorPWM(0.3,0.3);
maximusismax 2:4b6f6fc84793 30
maximusismax 2:4b6f6fc84793 31
FatCookies 3:87a5122682fa 32 pc.baud(57600);
maximusismax 0:566127ca8048 33
FatCookies 3:87a5122682fa 34 float p_error, error;
FatCookies 3:87a5122682fa 35 float integral;
FatCookies 3:87a5122682fa 36 float measured_value, desired_value,derivative;
FatCookies 3:87a5122682fa 37 float output;
FatCookies 3:87a5122682fa 38 //tunable variables
FatCookies 3:87a5122682fa 39 float Kp, Ki,Kd;
FatCookies 4:4afa448c9cce 40 Kp=0.8;
FatCookies 4:4afa448c9cce 41 Ki=0.6;
FatCookies 4:4afa448c9cce 42 Kd=0.0;
FatCookies 3:87a5122682fa 43 myled = 0;// Test
FatCookies 3:87a5122682fa 44 float dt=0;
FatCookies 3:87a5122682fa 45 p_error=0;
FatCookies 3:87a5122682fa 46 error=0;
FatCookies 3:87a5122682fa 47 integral=0;
FatCookies 3:87a5122682fa 48 measured_value= 0;
FatCookies 3:87a5122682fa 49 desired_value=0;
FatCookies 3:87a5122682fa 50 derivative=0;
FatCookies 3:87a5122682fa 51 // measured value is a float between -1.0 and 1.0
FatCookies 3:87a5122682fa 52 // desired value is always 0 ( as in car is in the middle of the road)
maximusismax 0:566127ca8048 53
FatCookies 3:87a5122682fa 54 uint8_t i = 0;
maximusismax 0:566127ca8048 55 while(1) {
FatCookies 3:87a5122682fa 56
FatCookies 4:4afa448c9cce 57 if(curr_cmd != 0) {
FatCookies 4:4afa448c9cce 58 switch(curr_cmd) {
FatCookies 4:4afa448c9cce 59 case 'A':
FatCookies 4:4afa448c9cce 60 if(xb.cBuffer->available() >= 3) {
FatCookies 4:4afa448c9cce 61 char p = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 62 char i = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 63 char d = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 64 Kp = p/256.0f;
FatCookies 4:4afa448c9cce 65 Ki = i/256.0f;
FatCookies 4:4afa448c9cce 66 Kd = d/256.0f;
FatCookies 4:4afa448c9cce 67 pc.putc('E');
FatCookies 4:4afa448c9cce 68 pc.printf("pid change\0");
FatCookies 4:4afa448c9cce 69 curr_cmd = 0;
FatCookies 4:4afa448c9cce 70 }
FatCookies 4:4afa448c9cce 71 break;
FatCookies 4:4afa448c9cce 72
FatCookies 4:4afa448c9cce 73 case 'F':
FatCookies 4:4afa448c9cce 74 if(xb.cBuffer->available() >= 2) {
FatCookies 4:4afa448c9cce 75 char a = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 76 char b = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 77 short s = (a << 8) & b;
FatCookies 4:4afa448c9cce 78
FatCookies 4:4afa448c9cce 79 speed = s/65536.0f;
FatCookies 4:4afa448c9cce 80 pc.putc('E');
FatCookies 4:4afa448c9cce 81 pc.printf("speed change\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 4:4afa448c9cce 92 if(xb.cBuffer->available() > 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 4:4afa448c9cce 98 } else if (cmd == 'C') {
FatCookies 4:4afa448c9cce 99 TFC_SetMotorPWM(0.0,0.0);
FatCookies 4:4afa448c9cce 100 TFC_HBRIDGE_DISABLE;
FatCookies 4:4afa448c9cce 101 } else if(cmd == 'A') {
FatCookies 4:4afa448c9cce 102 curr_cmd = 'A';
FatCookies 4:4afa448c9cce 103 } else if(cmd == 'F') {
FatCookies 4:4afa448c9cce 104 curr_cmd = 'F';
FatCookies 4:4afa448c9cce 105 }
FatCookies 4:4afa448c9cce 106
FatCookies 4:4afa448c9cce 107 }
FatCookies 4:4afa448c9cce 108
maximusismax 0:566127ca8048 109 //If we have an image ready
FatCookies 3:87a5122682fa 110 if(TFC_LineScanImageReady>0) {
FatCookies 3:87a5122682fa 111 left = 0;
FatCookies 3:87a5122682fa 112 right = 0;
FatCookies 4:4afa448c9cce 113 for(i = 63; i > 0; i--) {
FatCookies 3:87a5122682fa 114 curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
FatCookies 3:87a5122682fa 115 if(curr_left < CAM_THRESHOLD) {
FatCookies 3:87a5122682fa 116 left = i;
FatCookies 3:87a5122682fa 117 break;
FatCookies 3:87a5122682fa 118 }
FatCookies 3:87a5122682fa 119 }
FatCookies 3:87a5122682fa 120
FatCookies 3:87a5122682fa 121 for(i = 64; i < 128; i++) {
FatCookies 3:87a5122682fa 122 curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
FatCookies 3:87a5122682fa 123 if(curr_right < CAM_THRESHOLD) {
FatCookies 3:87a5122682fa 124 right = i;
FatCookies 3:87a5122682fa 125 break;
maximusismax 0:566127ca8048 126 }
FatCookies 3:87a5122682fa 127 }
FatCookies 3:87a5122682fa 128
FatCookies 3:87a5122682fa 129 pc.putc('H');
FatCookies 3:87a5122682fa 130 for(i = 0; i < 128; i++) {
FatCookies 3:87a5122682fa 131 pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);
FatCookies 3:87a5122682fa 132 }
FatCookies 3:87a5122682fa 133
FatCookies 3:87a5122682fa 134 measured_value = (64 - ((left+right)/2))/64.f;
FatCookies 3:87a5122682fa 135
FatCookies 3:87a5122682fa 136 t.start();
FatCookies 3:87a5122682fa 137 dt=t.read();
FatCookies 3:87a5122682fa 138 error = desired_value - measured_value;
FatCookies 3:87a5122682fa 139 integral=integral+error*dt;
FatCookies 3:87a5122682fa 140 derivative=(error-p_error)/dt;
FatCookies 3:87a5122682fa 141 output=Kp*error+Ki*integral+Kd*derivative;
FatCookies 3:87a5122682fa 142 p_error=error;
FatCookies 3:87a5122682fa 143
FatCookies 3:87a5122682fa 144 if((-1.0<=output)&&(output<=1.0))
FatCookies 3:87a5122682fa 145 {
FatCookies 3:87a5122682fa 146 TFC_SetServo(0,output);
FatCookies 3:87a5122682fa 147
FatCookies 3:87a5122682fa 148 }
FatCookies 3:87a5122682fa 149 else
FatCookies 3:87a5122682fa 150 {
FatCookies 3:87a5122682fa 151 while(1){
FatCookies 4:4afa448c9cce 152 pc.putc('E');
FatCookies 4:4afa448c9cce 153 pc.printf("pid has borked :( \0");
FatCookies 4:4afa448c9cce 154 wait(0.2f);
FatCookies 3:87a5122682fa 155 }
FatCookies 3:87a5122682fa 156 }
FatCookies 3:87a5122682fa 157
FatCookies 3:87a5122682fa 158 t.stop();
FatCookies 3:87a5122682fa 159 t.reset();
FatCookies 3:87a5122682fa 160 t.start();
FatCookies 3:87a5122682fa 161
FatCookies 3:87a5122682fa 162
FatCookies 3:87a5122682fa 163
FatCookies 3:87a5122682fa 164
FatCookies 3:87a5122682fa 165 //Reset image ready flag
FatCookies 3:87a5122682fa 166 TFC_LineScanImageReady=0;
FatCookies 3:87a5122682fa 167 wait(0.1f);
FatCookies 3:87a5122682fa 168 }
maximusismax 0:566127ca8048 169 }
maximusismax 0:566127ca8048 170 }