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@3:87a5122682fa, 2016-11-02 (annotated)

Committer:
FatCookies
Date:
Wed Nov 02 13:13:13 2016 +0000
Revision:
3:87a5122682fa
Parent:
2:4b6f6fc84793
Child:
4:4afa448c9cce
init pid example using camera input

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 3:87a5122682fa 3 #define CAM_THRESHOLD 128
FatCookies 3:87a5122682fa 4
maximusismax 0:566127ca8048 5
maximusismax 0:566127ca8048 6 DigitalOut myled(LED1);
FatCookies 3:87a5122682fa 7 //Serial pc(USBTX,USBRX);
FatCookies 3:87a5122682fa 8 Serial pc(PTD3,PTD2);
FatCookies 3:87a5122682fa 9
FatCookies 3:87a5122682fa 10 char curr_line[128];
FatCookies 3:87a5122682fa 11 uint8_t curr_left;
FatCookies 3:87a5122682fa 12 uint8_t curr_right;
FatCookies 3:87a5122682fa 13
FatCookies 3:87a5122682fa 14 uint8_t right;
FatCookies 3:87a5122682fa 15 uint8_t left;
FatCookies 3:87a5122682fa 16
FatCookies 3:87a5122682fa 17 Timer t;
maximusismax 0:566127ca8048 18
maximusismax 0:566127ca8048 19 int main() {
maximusismax 0:566127ca8048 20 TFC_Init();
FatCookies 3:87a5122682fa 21 TFC_InitServos(0.00052,0.00122,0.02);
FatCookies 3:87a5122682fa 22 TFC_HBRIDGE_ENABLE;
FatCookies 3:87a5122682fa 23 TFC_SetMotorPWM(0.3,0.3);
maximusismax 2:4b6f6fc84793 24
maximusismax 2:4b6f6fc84793 25
FatCookies 3:87a5122682fa 26 pc.baud(57600);
maximusismax 0:566127ca8048 27
FatCookies 3:87a5122682fa 28 float p_error, error;
FatCookies 3:87a5122682fa 29 float integral;
FatCookies 3:87a5122682fa 30 float measured_value, desired_value,derivative;
FatCookies 3:87a5122682fa 31 float output;
FatCookies 3:87a5122682fa 32 //tunable variables
FatCookies 3:87a5122682fa 33 float Kp, Ki,Kd;
FatCookies 3:87a5122682fa 34 Kp=0.5;
FatCookies 3:87a5122682fa 35 Ki=0.1;
FatCookies 3:87a5122682fa 36 Kd=0;
FatCookies 3:87a5122682fa 37 myled = 0;// Test
FatCookies 3:87a5122682fa 38 float dt=0;
FatCookies 3:87a5122682fa 39 p_error=0;
FatCookies 3:87a5122682fa 40 error=0;
FatCookies 3:87a5122682fa 41 integral=0;
FatCookies 3:87a5122682fa 42 measured_value= 0;
FatCookies 3:87a5122682fa 43 desired_value=0;
FatCookies 3:87a5122682fa 44 derivative=0;
FatCookies 3:87a5122682fa 45 // measured value is a float between -1.0 and 1.0
FatCookies 3:87a5122682fa 46 // desired value is always 0 ( as in car is in the middle of the road)
maximusismax 0:566127ca8048 47
FatCookies 3:87a5122682fa 48 uint8_t i = 0;
maximusismax 0:566127ca8048 49 while(1) {
FatCookies 3:87a5122682fa 50
maximusismax 0:566127ca8048 51 //If we have an image ready
FatCookies 3:87a5122682fa 52 if(TFC_LineScanImageReady>0) {
FatCookies 3:87a5122682fa 53 left = 0;
FatCookies 3:87a5122682fa 54 right = 0;
FatCookies 3:87a5122682fa 55 for(i = 63; i >= 0; i--) {
FatCookies 3:87a5122682fa 56 curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
FatCookies 3:87a5122682fa 57 if(curr_left < CAM_THRESHOLD) {
FatCookies 3:87a5122682fa 58 left = i;
FatCookies 3:87a5122682fa 59 break;
FatCookies 3:87a5122682fa 60 }
FatCookies 3:87a5122682fa 61 }
FatCookies 3:87a5122682fa 62
FatCookies 3:87a5122682fa 63 for(i = 64; i < 128; i++) {
FatCookies 3:87a5122682fa 64 curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
FatCookies 3:87a5122682fa 65 if(curr_right < CAM_THRESHOLD) {
FatCookies 3:87a5122682fa 66 right = i;
FatCookies 3:87a5122682fa 67 break;
maximusismax 0:566127ca8048 68 }
FatCookies 3:87a5122682fa 69 }
FatCookies 3:87a5122682fa 70
FatCookies 3:87a5122682fa 71 pc.putc('H');
FatCookies 3:87a5122682fa 72 for(i = 0; i < 128; i++) {
FatCookies 3:87a5122682fa 73 pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);
FatCookies 3:87a5122682fa 74 }
FatCookies 3:87a5122682fa 75
FatCookies 3:87a5122682fa 76 measured_value = (64 - ((left+right)/2))/64.f;
FatCookies 3:87a5122682fa 77
FatCookies 3:87a5122682fa 78 t.start();
FatCookies 3:87a5122682fa 79 dt=t.read();
FatCookies 3:87a5122682fa 80 error = desired_value - measured_value;
FatCookies 3:87a5122682fa 81 integral=integral+error*dt;
FatCookies 3:87a5122682fa 82 derivative=(error-p_error)/dt;
FatCookies 3:87a5122682fa 83 output=Kp*error+Ki*integral+Kd*derivative;
FatCookies 3:87a5122682fa 84 p_error=error;
FatCookies 3:87a5122682fa 85
FatCookies 3:87a5122682fa 86 if((-1.0<=output)&&(output<=1.0))
FatCookies 3:87a5122682fa 87 {
FatCookies 3:87a5122682fa 88 TFC_SetServo(0,output);
FatCookies 3:87a5122682fa 89
FatCookies 3:87a5122682fa 90 }
FatCookies 3:87a5122682fa 91 else
FatCookies 3:87a5122682fa 92 {
FatCookies 3:87a5122682fa 93 while(1){
FatCookies 3:87a5122682fa 94 myled = 1;
FatCookies 3:87a5122682fa 95 wait(0.2);
FatCookies 3:87a5122682fa 96 myled = 0;
FatCookies 3:87a5122682fa 97 wait(0.2);
FatCookies 3:87a5122682fa 98 integral=0;
FatCookies 3:87a5122682fa 99 }
FatCookies 3:87a5122682fa 100 }
FatCookies 3:87a5122682fa 101
FatCookies 3:87a5122682fa 102 t.stop();
FatCookies 3:87a5122682fa 103 t.reset();
FatCookies 3:87a5122682fa 104 t.start();
FatCookies 3:87a5122682fa 105
FatCookies 3:87a5122682fa 106
FatCookies 3:87a5122682fa 107
FatCookies 3:87a5122682fa 108
FatCookies 3:87a5122682fa 109 //Reset image ready flag
FatCookies 3:87a5122682fa 110 TFC_LineScanImageReady=0;
FatCookies 3:87a5122682fa 111 wait(0.1f);
FatCookies 3:87a5122682fa 112 }
maximusismax 0:566127ca8048 113 }
maximusismax 0:566127ca8048 114 }