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@12:da96e2f87465, 2016-11-21 (annotated)

Committer:
FatCookies
Date:
Mon Nov 21 17:19:05 2016 +0000
Revision:
12:da96e2f87465
Parent:
11:53de69b1840b
Child:
13:4e77264f254a
i thought i updated this

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"
FatCookies 9:aa2ce38dec6b 15 #include "angular_speed.h"
FatCookies 12:da96e2f87465 16 #include "motor.h"
FatCookies 12:da96e2f87465 17 #include "main.h"
maximusismax 8:7c5e6b1e7aa5 18
maximusismax 8:7c5e6b1e7aa5 19 #define BAUD_RATE 57600
FatCookies 6:b0e160c51013 20 #define CAM_THRESHOLD 109
maximusismax 8:7c5e6b1e7aa5 21 #define CAM_DIFF 10
maximusismax 8:7c5e6b1e7aa5 22
FatCookies 10:1bd0224093e4 23 #define RIGHT_MOTOR_COMPENSATION_RATIO 1.1586276
FatCookies 10:1bd0224093e4 24
FatCookies 3:87a5122682fa 25 //Serial pc(USBTX,USBRX);
FatCookies 3:87a5122682fa 26 Serial pc(PTD3,PTD2);
FatCookies 4:4afa448c9cce 27 XBEE xb(&pc);
FatCookies 3:87a5122682fa 28
maximusismax 8:7c5e6b1e7aa5 29 //Woo global variables!
maximusismax 8:7c5e6b1e7aa5 30 bool onTrack;
FatCookies 3:87a5122682fa 31 char curr_line[128];
FatCookies 3:87a5122682fa 32 uint8_t curr_left;
FatCookies 3:87a5122682fa 33 uint8_t curr_right;
FatCookies 3:87a5122682fa 34 uint8_t right;
FatCookies 3:87a5122682fa 35 uint8_t left;
maximusismax 8:7c5e6b1e7aa5 36 int8_t leftChange;
maximusismax 8:7c5e6b1e7aa5 37 int8_t rightChange;
maximusismax 8:7c5e6b1e7aa5 38 int diff;
maximusismax 8:7c5e6b1e7aa5 39 int prev;
maximusismax 8:7c5e6b1e7aa5 40 int i = 0;
maximusismax 8:7c5e6b1e7aa5 41 float measuredValBuffer[5];
maximusismax 8:7c5e6b1e7aa5 42 uint8_t valBufferIndex;
maximusismax 8:7c5e6b1e7aa5 43
maximusismax 8:7c5e6b1e7aa5 44 //Some PID variables
FatCookies 12:da96e2f87465 45 pid_instance servo_pid;
FatCookies 3:87a5122682fa 46 Timer t;
FatCookies 4:4afa448c9cce 47
FatCookies 12:da96e2f87465 48 float right_p;
FatCookies 12:da96e2f87465 49 float right_error;
FatCookies 12:da96e2f87465 50 float right_perror;
FatCookies 12:da96e2f87465 51 float right_measured_value, right_desired_value;
FatCookies 12:da96e2f87465 52 float right_output;
FatCookies 12:da96e2f87465 53
FatCookies 12:da96e2f87465 54 float left_p;
FatCookies 12:da96e2f87465 55 float left_error;
FatCookies 12:da96e2f87465 56 float left_perror;
FatCookies 12:da96e2f87465 57 float left_measured_value, left_desired_value;
FatCookies 12:da96e2f87465 58 float left_output;
FatCookies 12:da96e2f87465 59
FatCookies 9:aa2ce38dec6b 60 //Speed Sensors variables
FatCookies 9:aa2ce38dec6b 61 InterruptIn leftHallSensor(D0);
FatCookies 9:aa2ce38dec6b 62 InterruptIn rightHallSensor(D2);
FatCookies 9:aa2ce38dec6b 63 Timer t1;
FatCookies 9:aa2ce38dec6b 64 Timer t2;
FatCookies 9:aa2ce38dec6b 65 volatile float Time_L,Time_R;
FatCookies 9:aa2ce38dec6b 66 float wL, wR;
FatCookies 9:aa2ce38dec6b 67 void GetTime_L();
FatCookies 9:aa2ce38dec6b 68 void GetTime_R();
FatCookies 9:aa2ce38dec6b 69 inline void initSpeedSensors();
FatCookies 9:aa2ce38dec6b 70
FatCookies 9:aa2ce38dec6b 71 // Current comms command
FatCookies 4:4afa448c9cce 72 char curr_cmd = 0;
FatCookies 4:4afa448c9cce 73
FatCookies 4:4afa448c9cce 74 float speed = 0.3;
FatCookies 6:b0e160c51013 75 int frame_counter = 0;
maximusismax 8:7c5e6b1e7aa5 76
maximusismax 8:7c5e6b1e7aa5 77 //Hacky start/stop signal detection
maximusismax 8:7c5e6b1e7aa5 78 int startstop = 0;
FatCookies 7:ad893fc41b95 79 bool seen = false;
maximusismax 8:7c5e6b1e7aa5 80
maximusismax 8:7c5e6b1e7aa5 81 void sendString(const char *format, ...);
maximusismax 8:7c5e6b1e7aa5 82 void initVariables();
maximusismax 8:7c5e6b1e7aa5 83 inline void handleComms();
maximusismax 8:7c5e6b1e7aa5 84 inline float findCentreValue();
maximusismax 8:7c5e6b1e7aa5 85 inline void PIDController();
maximusismax 8:7c5e6b1e7aa5 86 inline void sendImage();
maximusismax 8:7c5e6b1e7aa5 87
maximusismax 0:566127ca8048 88 int main() {
maximusismax 8:7c5e6b1e7aa5 89 //Set up TFC driver stuff
maximusismax 0:566127ca8048 90 TFC_Init();
FatCookies 3:87a5122682fa 91 TFC_InitServos(0.00052,0.00122,0.02);
maximusismax 8:7c5e6b1e7aa5 92 //Old things to make the car move at run-time
maximusismax 8:7c5e6b1e7aa5 93 //Should tie these to a button for the actual races
FatCookies 4:4afa448c9cce 94 //TFC_HBRIDGE_ENABLE;
FatCookies 4:4afa448c9cce 95 //TFC_SetMotorPWM(0.3,0.3);
maximusismax 2:4b6f6fc84793 96
maximusismax 8:7c5e6b1e7aa5 97 //Setup baud rate for serial link, do not change!
maximusismax 8:7c5e6b1e7aa5 98 pc.baud(BAUD_RATE);
maximusismax 0:566127ca8048 99
maximusismax 8:7c5e6b1e7aa5 100 //Initialise/reset PID variables
maximusismax 8:7c5e6b1e7aa5 101 initVariables();
FatCookies 9:aa2ce38dec6b 102 initSpeedSensors();
FatCookies 12:da96e2f87465 103
maximusismax 0:566127ca8048 104 while(1) {
FatCookies 3:87a5122682fa 105
maximusismax 8:7c5e6b1e7aa5 106 handleComms();
maximusismax 8:7c5e6b1e7aa5 107
maximusismax 8:7c5e6b1e7aa5 108 //If we have an image ready
maximusismax 8:7c5e6b1e7aa5 109 if(TFC_LineScanImageReady>0) {
FatCookies 12:da96e2f87465 110 servo_pid.measured_value = findCentreValue();
maximusismax 8:7c5e6b1e7aa5 111
maximusismax 8:7c5e6b1e7aa5 112 PIDController();
maximusismax 8:7c5e6b1e7aa5 113
maximusismax 8:7c5e6b1e7aa5 114 sendImage();
maximusismax 8:7c5e6b1e7aa5 115
maximusismax 8:7c5e6b1e7aa5 116 //Hacky way to detect the start/stop signal
maximusismax 8:7c5e6b1e7aa5 117 if(right - left < 60) {
maximusismax 8:7c5e6b1e7aa5 118 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 119 pc.printf("START STOP!! &d",startstop);
maximusismax 8:7c5e6b1e7aa5 120 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 121
maximusismax 8:7c5e6b1e7aa5 122 if(seen) {
maximusismax 8:7c5e6b1e7aa5 123 seen = false;
maximusismax 8:7c5e6b1e7aa5 124 } else {
maximusismax 8:7c5e6b1e7aa5 125 startstop++;
maximusismax 8:7c5e6b1e7aa5 126 seen = true;
maximusismax 8:7c5e6b1e7aa5 127 }
maximusismax 8:7c5e6b1e7aa5 128 //If we've done 5 laps, stop the car
maximusismax 8:7c5e6b1e7aa5 129 if(startstop >= 1) {
maximusismax 8:7c5e6b1e7aa5 130 TFC_SetMotorPWM(0.0,0.0);
maximusismax 8:7c5e6b1e7aa5 131 TFC_HBRIDGE_DISABLE;
maximusismax 8:7c5e6b1e7aa5 132 startstop = 0;
maximusismax 8:7c5e6b1e7aa5 133 }
maximusismax 8:7c5e6b1e7aa5 134 }
maximusismax 8:7c5e6b1e7aa5 135
maximusismax 8:7c5e6b1e7aa5 136 //Reset image ready flag
maximusismax 8:7c5e6b1e7aa5 137 TFC_LineScanImageReady=0;
maximusismax 8:7c5e6b1e7aa5 138 }
maximusismax 8:7c5e6b1e7aa5 139 }
maximusismax 8:7c5e6b1e7aa5 140 }
maximusismax 8:7c5e6b1e7aa5 141
maximusismax 8:7c5e6b1e7aa5 142 void sendString(const char *format, ...) {
maximusismax 8:7c5e6b1e7aa5 143 va_list arg;
maximusismax 8:7c5e6b1e7aa5 144
maximusismax 8:7c5e6b1e7aa5 145 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 146 va_start (arg, format);
maximusismax 8:7c5e6b1e7aa5 147 pc.vprintf(format,arg);
maximusismax 8:7c5e6b1e7aa5 148 va_end (arg);
maximusismax 8:7c5e6b1e7aa5 149 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 150 }
maximusismax 8:7c5e6b1e7aa5 151
maximusismax 8:7c5e6b1e7aa5 152 void initVariables() {
maximusismax 8:7c5e6b1e7aa5 153 //Tunable PID variables
FatCookies 12:da96e2f87465 154 servo_pid.Kp = 125 / 25.0f;
FatCookies 12:da96e2f87465 155 servo_pid.Ki = 12.0f / 25.0f;
FatCookies 12:da96e2f87465 156 servo_pid.Kd = 0.0f;
FatCookies 12:da96e2f87465 157 servo_pid.dt = 0;
FatCookies 12:da96e2f87465 158 servo_pid.p_error = 0;
FatCookies 12:da96e2f87465 159 servo_pid.pid_error = 0;
FatCookies 12:da96e2f87465 160 servo_pid.integral = 0;
FatCookies 12:da96e2f87465 161 servo_pid.measured_value = 0;
FatCookies 12:da96e2f87465 162 servo_pid.desired_value = 0;
FatCookies 12:da96e2f87465 163 servo_pid.derivative = 0;
maximusismax 8:7c5e6b1e7aa5 164
maximusismax 8:7c5e6b1e7aa5 165 valBufferIndex = 0;
FatCookies 12:da96e2f87465 166
FatCookies 12:da96e2f87465 167 // motor p controller init
FatCookies 12:da96e2f87465 168 right_p = 1.0;
FatCookies 12:da96e2f87465 169 right_error = 0.0;
FatCookies 12:da96e2f87465 170 right_perror = 0.0;
FatCookies 12:da96e2f87465 171 right_measured_value = 0.0;
FatCookies 12:da96e2f87465 172 right_desired_value = 0.0;
FatCookies 12:da96e2f87465 173 right_output = 0.0;
FatCookies 12:da96e2f87465 174 left_p = 1.0;
FatCookies 12:da96e2f87465 175 left_error = 0.0;
FatCookies 12:da96e2f87465 176 left_perror = 0.0;
FatCookies 12:da96e2f87465 177 left_measured_value = 0.0;
FatCookies 12:da96e2f87465 178 left_desired_value = 0.0;
FatCookies 12:da96e2f87465 179 left_output = 0.0;
FatCookies 12:da96e2f87465 180
maximusismax 8:7c5e6b1e7aa5 181 //Measured value is a float between -1.0 and 1.0 (from left to right)
maximusismax 8:7c5e6b1e7aa5 182 //Desired value is always 0.0 (as in, car is in the middle of the road)
maximusismax 8:7c5e6b1e7aa5 183 }
maximusismax 8:7c5e6b1e7aa5 184
maximusismax 8:7c5e6b1e7aa5 185 inline void sendImage() {
maximusismax 8:7c5e6b1e7aa5 186 //Only send 1/3 of camera frames to GUI program
maximusismax 8:7c5e6b1e7aa5 187 if((frame_counter % 3) == 0) {
maximusismax 8:7c5e6b1e7aa5 188 pc.putc('H');
maximusismax 8:7c5e6b1e7aa5 189 for(i = 0; i < 128; i++) {
maximusismax 8:7c5e6b1e7aa5 190 pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);
maximusismax 8:7c5e6b1e7aa5 191 }
FatCookies 9:aa2ce38dec6b 192
FatCookies 12:da96e2f87465 193 wL = wL/3.0f;
FatCookies 12:da96e2f87465 194 wR= wR/3.0f;
FatCookies 12:da96e2f87465 195 sendString("wL = %f, wR = %f",wL,wR);
FatCookies 12:da96e2f87465 196 wL = 0;
FatCookies 12:da96e2f87465 197 wR = 0;
FatCookies 11:53de69b1840b 198
FatCookies 12:da96e2f87465 199 union {
FatCookies 12:da96e2f87465 200 float a;
FatCookies 12:da96e2f87465 201 unsigned char bytes[4];
FatCookies 12:da96e2f87465 202 } thing;
FatCookies 12:da96e2f87465 203 thing.a = TFC_ReadBatteryVoltage();
FatCookies 12:da96e2f87465 204 pc.putc('J');
FatCookies 12:da96e2f87465 205 pc.putc(thing.bytes[0]);
FatCookies 12:da96e2f87465 206 pc.putc(thing.bytes[1]);
FatCookies 12:da96e2f87465 207 pc.putc(thing.bytes[2]);
FatCookies 12:da96e2f87465 208 pc.putc(thing.bytes[3]);
FatCookies 12:da96e2f87465 209 }
FatCookies 12:da96e2f87465 210 wL+=Get_Speed(Time_L);
FatCookies 12:da96e2f87465 211 wR+=Get_Speed(Time_R);
FatCookies 12:da96e2f87465 212 frame_counter++;
maximusismax 8:7c5e6b1e7aa5 213 }
maximusismax 8:7c5e6b1e7aa5 214
maximusismax 8:7c5e6b1e7aa5 215 inline void handleComms() {
maximusismax 8:7c5e6b1e7aa5 216 if(curr_cmd != 0) {
FatCookies 4:4afa448c9cce 217 switch(curr_cmd) {
FatCookies 4:4afa448c9cce 218 case 'A':
FatCookies 4:4afa448c9cce 219 if(xb.cBuffer->available() >= 3) {
FatCookies 4:4afa448c9cce 220 char p = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 221 char i = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 222 char d = xb.cBuffer->read();
FatCookies 12:da96e2f87465 223 servo_pid.Kp = p/25.0f;
FatCookies 12:da96e2f87465 224 servo_pid.Ki = i/25.0f;
FatCookies 12:da96e2f87465 225 servo_pid.Kd = d/25.0f;
FatCookies 4:4afa448c9cce 226 pc.putc('E');
FatCookies 12:da96e2f87465 227 pc.printf("pid change, Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", servo_pid.Kp, servo_pid.Ki, servo_pid.Kd, p, i, d);
FatCookies 6:b0e160c51013 228 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 229
FatCookies 4:4afa448c9cce 230 curr_cmd = 0;
FatCookies 4:4afa448c9cce 231 }
FatCookies 4:4afa448c9cce 232 break;
FatCookies 4:4afa448c9cce 233
FatCookies 4:4afa448c9cce 234 case 'F':
FatCookies 6:b0e160c51013 235 if(xb.cBuffer->available() >= 1) {
FatCookies 4:4afa448c9cce 236 char a = xb.cBuffer->read();
FatCookies 6:b0e160c51013 237 speed = a/256.0f;
FatCookies 7:ad893fc41b95 238 TFC_SetMotorPWM(speed,speed);
FatCookies 4:4afa448c9cce 239 pc.putc('E');
FatCookies 6:b0e160c51013 240 pc.printf("s = %u %f",a, speed);
FatCookies 6:b0e160c51013 241 pc.putc(0);
FatCookies 4:4afa448c9cce 242 curr_cmd = 0;
FatCookies 4:4afa448c9cce 243
FatCookies 4:4afa448c9cce 244 }
FatCookies 4:4afa448c9cce 245 break;
FatCookies 4:4afa448c9cce 246
FatCookies 4:4afa448c9cce 247 default:
FatCookies 4:4afa448c9cce 248 break;
FatCookies 4:4afa448c9cce 249 }
FatCookies 4:4afa448c9cce 250 }
FatCookies 4:4afa448c9cce 251
FatCookies 6:b0e160c51013 252 if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
FatCookies 4:4afa448c9cce 253 char cmd = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 254 if(cmd == 'D') {
FatCookies 4:4afa448c9cce 255 TFC_InitServos(0.00052,0.00122,0.02);
FatCookies 4:4afa448c9cce 256 TFC_HBRIDGE_ENABLE;
FatCookies 10:1bd0224093e4 257 TFC_SetMotorPWM(RIGHT_MOTOR_COMPENSATION_RATIO*speed,speed);
FatCookies 12:da96e2f87465 258 servo_pid.integral = 0;
FatCookies 12:da96e2f87465 259
FatCookies 6:b0e160c51013 260
FatCookies 4:4afa448c9cce 261 } else if (cmd == 'C') {
FatCookies 4:4afa448c9cce 262 TFC_SetMotorPWM(0.0,0.0);
FatCookies 4:4afa448c9cce 263 TFC_HBRIDGE_DISABLE;
FatCookies 4:4afa448c9cce 264 } else if(cmd == 'A') {
FatCookies 4:4afa448c9cce 265 curr_cmd = 'A';
FatCookies 4:4afa448c9cce 266 } else if(cmd == 'F') {
FatCookies 4:4afa448c9cce 267 curr_cmd = 'F';
FatCookies 4:4afa448c9cce 268 }
FatCookies 4:4afa448c9cce 269
FatCookies 4:4afa448c9cce 270 }
maximusismax 8:7c5e6b1e7aa5 271 }
maximusismax 8:7c5e6b1e7aa5 272 inline float findCentreValue() {
maximusismax 8:7c5e6b1e7aa5 273 float measuredValue;
maximusismax 8:7c5e6b1e7aa5 274
maximusismax 8:7c5e6b1e7aa5 275 diff = 0;
maximusismax 8:7c5e6b1e7aa5 276 prev = -1;
maximusismax 8:7c5e6b1e7aa5 277 leftChange = left;
maximusismax 8:7c5e6b1e7aa5 278 for(i = 63; i > 0; i--) {
maximusismax 8:7c5e6b1e7aa5 279 curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
maximusismax 8:7c5e6b1e7aa5 280 diff = prev - curr_left;
maximusismax 8:7c5e6b1e7aa5 281 if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
maximusismax 8:7c5e6b1e7aa5 282 left = i;
maximusismax 8:7c5e6b1e7aa5 283 break;
maximusismax 8:7c5e6b1e7aa5 284 }
maximusismax 8:7c5e6b1e7aa5 285 prev = curr_left;
maximusismax 8:7c5e6b1e7aa5 286 }
maximusismax 8:7c5e6b1e7aa5 287
maximusismax 8:7c5e6b1e7aa5 288 prev = -1;
maximusismax 8:7c5e6b1e7aa5 289 rightChange = right;
maximusismax 8:7c5e6b1e7aa5 290 for(i = 64; i < 128; i++) {
maximusismax 8:7c5e6b1e7aa5 291 curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
maximusismax 8:7c5e6b1e7aa5 292 int diff = prev - curr_right;
maximusismax 8:7c5e6b1e7aa5 293 if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
maximusismax 8:7c5e6b1e7aa5 294 right = i;
maximusismax 8:7c5e6b1e7aa5 295 break;
maximusismax 8:7c5e6b1e7aa5 296 }
maximusismax 8:7c5e6b1e7aa5 297 prev = curr_right;
maximusismax 8:7c5e6b1e7aa5 298 }
maximusismax 8:7c5e6b1e7aa5 299
maximusismax 8:7c5e6b1e7aa5 300 //Calculate how left/right we are
maximusismax 8:7c5e6b1e7aa5 301 measuredValue = (64 - ((left+right)/2))/64.f;
maximusismax 8:7c5e6b1e7aa5 302 measuredValBuffer[valBufferIndex % 5] = measuredValue;
maximusismax 8:7c5e6b1e7aa5 303 valBufferIndex++;
maximusismax 8:7c5e6b1e7aa5 304
maximusismax 8:7c5e6b1e7aa5 305 return measuredValue;
maximusismax 8:7c5e6b1e7aa5 306 }
FatCookies 3:87a5122682fa 307
FatCookies 12:da96e2f87465 308 void handlePID(pid_instance *pid) {
FatCookies 12:da96e2f87465 309 pid->dt = t.read();
FatCookies 12:da96e2f87465 310 pid->pid_error = pid->desired_value - pid->measured_value;
FatCookies 12:da96e2f87465 311 pid->integral = pid->integral + pid->pid_error * pid->dt;
FatCookies 12:da96e2f87465 312 pid->derivative = (pid->pid_error - pid->p_error) / pid->dt;
FatCookies 12:da96e2f87465 313 pid->output = pid->Kp * pid->pid_error + pid->Ki * pid->integral + pid->Kd * pid->derivative;
FatCookies 12:da96e2f87465 314 pid->p_error = pid->pid_error;
FatCookies 12:da96e2f87465 315
FatCookies 12:da96e2f87465 316 if(pid->integral > 1.0f) {
FatCookies 12:da96e2f87465 317 pid->integral = 1.0f;
FatCookies 12:da96e2f87465 318 }
FatCookies 12:da96e2f87465 319 if(pid->integral < -1.0f) {
FatCookies 12:da96e2f87465 320 pid->integral = -1.0f;
FatCookies 12:da96e2f87465 321 }
FatCookies 12:da96e2f87465 322 }
FatCookies 12:da96e2f87465 323
maximusismax 8:7c5e6b1e7aa5 324 inline void PIDController() {
maximusismax 8:7c5e6b1e7aa5 325 //PID Stuff!
FatCookies 3:87a5122682fa 326 t.start();
FatCookies 12:da96e2f87465 327 handlePID(&servo_pid);
FatCookies 12:da96e2f87465 328
FatCookies 12:da96e2f87465 329 if((-1.0 <= servo_pid.output) && (servo_pid.output <= 1.0))
FatCookies 6:b0e160c51013 330 {
FatCookies 12:da96e2f87465 331 TFC_SetServo(0, servo_pid.output);
maximusismax 8:7c5e6b1e7aa5 332 }
maximusismax 8:7c5e6b1e7aa5 333 else //Unhappy PID state
maximusismax 8:7c5e6b1e7aa5 334 {
maximusismax 8:7c5e6b1e7aa5 335 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 336 pc.printf("pid unhappy");
maximusismax 8:7c5e6b1e7aa5 337 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 338 pc.putc('E');
FatCookies 12:da96e2f87465 339 pc.printf("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
maximusismax 8:7c5e6b1e7aa5 340 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 341 TFC_InitServos(0.00052, 0.00122, 0.02);
maximusismax 8:7c5e6b1e7aa5 342 //output, pid_error, p_error, integral, derivative = 0;
maximusismax 8:7c5e6b1e7aa5 343
FatCookies 12:da96e2f87465 344 if(servo_pid.output >= 1.0f) {
maximusismax 8:7c5e6b1e7aa5 345 TFC_SetServo(0, 0.9f);
FatCookies 12:da96e2f87465 346 servo_pid.output = 1.0f;
maximusismax 8:7c5e6b1e7aa5 347 } else {
maximusismax 8:7c5e6b1e7aa5 348 TFC_SetServo(0, -0.9f);
FatCookies 12:da96e2f87465 349 servo_pid.output = -1.0f;
maximusismax 8:7c5e6b1e7aa5 350 }
maximusismax 8:7c5e6b1e7aa5 351 }
FatCookies 12:da96e2f87465 352
FatCookies 12:da96e2f87465 353 /*
FatCookies 12:da96e2f87465 354 right_error = right_desired_value - right_measured_value;
FatCookies 12:da96e2f87465 355 right_output = right_p * right_error;
FatCookies 12:da96e2f87465 356 right_perror = right_error;
FatCookies 12:da96e2f87465 357
FatCookies 12:da96e2f87465 358 left_error = left_desired_value - left_measured_value;
FatCookies 12:da96e2f87465 359 left_output = left_p * left_error;
FatCookies 12:da96e2f87465 360 left_perror = left_error;
FatCookies 12:da96e2f87465 361
FatCookies 12:da96e2f87465 362 TFC_SetMotorPWM(right_output,left_output);
FatCookies 12:da96e2f87465 363
FatCookies 12:da96e2f87465 364 dutyCycleCorner(speed,output);
FatCookies 12:da96e2f87465 365 */
FatCookies 6:b0e160c51013 366
FatCookies 3:87a5122682fa 367 t.stop();
FatCookies 3:87a5122682fa 368 t.reset();
FatCookies 3:87a5122682fa 369 t.start();
FatCookies 9:aa2ce38dec6b 370 }
FatCookies 9:aa2ce38dec6b 371
FatCookies 9:aa2ce38dec6b 372
FatCookies 9:aa2ce38dec6b 373
FatCookies 9:aa2ce38dec6b 374 inline void initSpeedSensors() {
FatCookies 9:aa2ce38dec6b 375 t1.start();
FatCookies 9:aa2ce38dec6b 376 t2.start();
FatCookies 9:aa2ce38dec6b 377
FatCookies 9:aa2ce38dec6b 378 //Left and Right are defined looking at the rear of the car, in the direction the camera points at.
FatCookies 9:aa2ce38dec6b 379 leftHallSensor.rise(&GetTime_L);
FatCookies 9:aa2ce38dec6b 380 rightHallSensor.rise(&GetTime_R);
FatCookies 9:aa2ce38dec6b 381 }
FatCookies 9:aa2ce38dec6b 382
FatCookies 9:aa2ce38dec6b 383 void GetTime_L(){
FatCookies 9:aa2ce38dec6b 384 Time_L=t1.read_us();
FatCookies 9:aa2ce38dec6b 385 t1.reset();
FatCookies 9:aa2ce38dec6b 386 }
FatCookies 9:aa2ce38dec6b 387
FatCookies 9:aa2ce38dec6b 388 void GetTime_R(){
FatCookies 9:aa2ce38dec6b 389 Time_R=t2.read_us();
FatCookies 9:aa2ce38dec6b 390 t2.reset();
maximusismax 0:566127ca8048 391 }