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

Committer:
FatCookies
Date:
Wed Nov 30 14:54:19 2016 +0000
Revision:
14:13085e161dd1
Parent:
13:4e77264f254a
Child:
15:ccde02f96449
integrated electronic diff

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 14:13085e161dd1 16 #include "main.h"
FatCookies 12:da96e2f87465 17 #include "motor.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
FatCookies 14:13085e161dd1 22 #define WHEEL_RADIUS 0.025f
maximusismax 8:7c5e6b1e7aa5 23
FatCookies 10:1bd0224093e4 24 #define RIGHT_MOTOR_COMPENSATION_RATIO 1.1586276
FatCookies 10:1bd0224093e4 25
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;
FatCookies 13:4e77264f254a 41 float measuredValBuffer[64];
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 13:4e77264f254a 46 pid_instance right_motor_pid;
FatCookies 13:4e77264f254a 47 pid_instance left_motor_pid;
FatCookies 3:87a5122682fa 48 Timer t;
FatCookies 4:4afa448c9cce 49
FatCookies 12:da96e2f87465 50
FatCookies 9:aa2ce38dec6b 51 //Speed Sensors variables
FatCookies 9:aa2ce38dec6b 52 InterruptIn leftHallSensor(D0);
FatCookies 9:aa2ce38dec6b 53 InterruptIn rightHallSensor(D2);
FatCookies 9:aa2ce38dec6b 54 Timer t1;
FatCookies 9:aa2ce38dec6b 55 Timer t2;
FatCookies 9:aa2ce38dec6b 56 volatile float Time_L,Time_R;
FatCookies 9:aa2ce38dec6b 57 float wL, wR;
FatCookies 9:aa2ce38dec6b 58 void GetTime_L();
FatCookies 9:aa2ce38dec6b 59 void GetTime_R();
FatCookies 9:aa2ce38dec6b 60 inline void initSpeedSensors();
FatCookies 9:aa2ce38dec6b 61
FatCookies 9:aa2ce38dec6b 62 // Current comms command
FatCookies 4:4afa448c9cce 63 char curr_cmd = 0;
FatCookies 4:4afa448c9cce 64
FatCookies 4:4afa448c9cce 65 float speed = 0.3;
FatCookies 6:b0e160c51013 66 int frame_counter = 0;
maximusismax 8:7c5e6b1e7aa5 67
maximusismax 8:7c5e6b1e7aa5 68 //Hacky start/stop signal detection
maximusismax 8:7c5e6b1e7aa5 69 int startstop = 0;
FatCookies 7:ad893fc41b95 70 bool seen = false;
maximusismax 8:7c5e6b1e7aa5 71
maximusismax 8:7c5e6b1e7aa5 72 void sendString(const char *format, ...);
maximusismax 8:7c5e6b1e7aa5 73 void initVariables();
maximusismax 8:7c5e6b1e7aa5 74 inline void handleComms();
maximusismax 8:7c5e6b1e7aa5 75 inline float findCentreValue();
maximusismax 8:7c5e6b1e7aa5 76 inline void PIDController();
maximusismax 8:7c5e6b1e7aa5 77 inline void sendImage();
FatCookies 13:4e77264f254a 78 inline void sendSpeeds();
FatCookies 13:4e77264f254a 79 void handlePID(pid_instance *pid);
FatCookies 13:4e77264f254a 80 void initPID(pid_instance* pid);
FatCookies 13:4e77264f254a 81 inline void handleStartStop();
FatCookies 13:4e77264f254a 82 inline void recordMeasuredVal();
FatCookies 13:4e77264f254a 83 void sendBattery();
maximusismax 8:7c5e6b1e7aa5 84
maximusismax 0:566127ca8048 85 int main() {
maximusismax 8:7c5e6b1e7aa5 86 //Set up TFC driver stuff
maximusismax 0:566127ca8048 87 TFC_Init();
FatCookies 3:87a5122682fa 88 TFC_InitServos(0.00052,0.00122,0.02);
FatCookies 13:4e77264f254a 89
maximusismax 8:7c5e6b1e7aa5 90 //Setup baud rate for serial link, do not change!
maximusismax 8:7c5e6b1e7aa5 91 pc.baud(BAUD_RATE);
maximusismax 0:566127ca8048 92
maximusismax 8:7c5e6b1e7aa5 93 //Initialise/reset PID variables
maximusismax 8:7c5e6b1e7aa5 94 initVariables();
FatCookies 9:aa2ce38dec6b 95 initSpeedSensors();
FatCookies 12:da96e2f87465 96
FatCookies 13:4e77264f254a 97
maximusismax 0:566127ca8048 98 while(1) {
FatCookies 3:87a5122682fa 99
maximusismax 8:7c5e6b1e7aa5 100 handleComms();
maximusismax 8:7c5e6b1e7aa5 101
maximusismax 8:7c5e6b1e7aa5 102 //If we have an image ready
FatCookies 13:4e77264f254a 103 if(TFC_LineScanImageReady>0) {
FatCookies 13:4e77264f254a 104 /* Find the bounds of the track and calculate how close we are to
FatCookies 13:4e77264f254a 105 * the centre */
FatCookies 12:da96e2f87465 106 servo_pid.measured_value = findCentreValue();
FatCookies 13:4e77264f254a 107 recordMeasuredVal();
maximusismax 8:7c5e6b1e7aa5 108
FatCookies 13:4e77264f254a 109 // Read the angular velocity of both wheels
FatCookies 13:4e77264f254a 110 wL=Get_Speed(Time_L);
FatCookies 13:4e77264f254a 111 wR=Get_Speed(Time_R);
FatCookies 13:4e77264f254a 112
FatCookies 13:4e77264f254a 113 // Run the PID controllers and adjust steering/motor accordingly
maximusismax 8:7c5e6b1e7aa5 114 PIDController();
maximusismax 8:7c5e6b1e7aa5 115
FatCookies 13:4e77264f254a 116 // Send the line scan image over serial
maximusismax 8:7c5e6b1e7aa5 117 sendImage();
maximusismax 8:7c5e6b1e7aa5 118
FatCookies 13:4e77264f254a 119 // Send the wheel speeds over serial
FatCookies 13:4e77264f254a 120 sendSpeeds();
FatCookies 13:4e77264f254a 121
FatCookies 13:4e77264f254a 122 // Check if car is at the stop line
FatCookies 13:4e77264f254a 123 //handleStartStop();
maximusismax 8:7c5e6b1e7aa5 124
maximusismax 8:7c5e6b1e7aa5 125 //Reset image ready flag
maximusismax 8:7c5e6b1e7aa5 126 TFC_LineScanImageReady=0;
maximusismax 8:7c5e6b1e7aa5 127 }
maximusismax 8:7c5e6b1e7aa5 128 }
maximusismax 8:7c5e6b1e7aa5 129 }
maximusismax 8:7c5e6b1e7aa5 130
FatCookies 13:4e77264f254a 131 void sendBattery() {
FatCookies 13:4e77264f254a 132
FatCookies 13:4e77264f254a 133 if(frame_counter % 256 == 0) {
FatCookies 13:4e77264f254a 134 float level = TFC_ReadBatteryVoltage() * 6.25;
FatCookies 13:4e77264f254a 135 union {
FatCookies 13:4e77264f254a 136 float a;
FatCookies 13:4e77264f254a 137 unsigned char bytes[4];
FatCookies 13:4e77264f254a 138 } thing;
FatCookies 13:4e77264f254a 139
FatCookies 13:4e77264f254a 140 pc.putc('J');
FatCookies 13:4e77264f254a 141 thing.a = level;
FatCookies 13:4e77264f254a 142 pc.putc(thing.bytes[0]);
FatCookies 13:4e77264f254a 143 pc.putc(thing.bytes[1]);
FatCookies 13:4e77264f254a 144 pc.putc(thing.bytes[2]);
FatCookies 13:4e77264f254a 145 pc.putc(thing.bytes[3]);
FatCookies 13:4e77264f254a 146 }
FatCookies 13:4e77264f254a 147 }
FatCookies 13:4e77264f254a 148
maximusismax 8:7c5e6b1e7aa5 149 void sendString(const char *format, ...) {
maximusismax 8:7c5e6b1e7aa5 150 va_list arg;
maximusismax 8:7c5e6b1e7aa5 151
maximusismax 8:7c5e6b1e7aa5 152 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 153 va_start (arg, format);
maximusismax 8:7c5e6b1e7aa5 154 pc.vprintf(format,arg);
maximusismax 8:7c5e6b1e7aa5 155 va_end (arg);
maximusismax 8:7c5e6b1e7aa5 156 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 157 }
maximusismax 8:7c5e6b1e7aa5 158
FatCookies 13:4e77264f254a 159 void initPID(pid_instance* pid, float Kp, float Ki, float Kd) {
FatCookies 13:4e77264f254a 160 pid->Kp = Kd;
FatCookies 13:4e77264f254a 161 pid->Ki = Ki;
FatCookies 13:4e77264f254a 162 pid->Kd = Kd;
FatCookies 13:4e77264f254a 163 pid->dt = 0;
FatCookies 13:4e77264f254a 164 pid->p_error = 0;
FatCookies 13:4e77264f254a 165 pid->pid_error = 0;
FatCookies 13:4e77264f254a 166 pid->integral = 0;
FatCookies 13:4e77264f254a 167 pid->measured_value = 0;
FatCookies 13:4e77264f254a 168 pid->desired_value = 0;
FatCookies 13:4e77264f254a 169 pid->derivative = 0;
FatCookies 13:4e77264f254a 170 }
FatCookies 13:4e77264f254a 171
maximusismax 8:7c5e6b1e7aa5 172 void initVariables() {
FatCookies 13:4e77264f254a 173 // Initialise three PID controllers for the servo and each wheel.
FatCookies 13:4e77264f254a 174 initPID(&servo_pid, 2.2f, 0.6f, 0.f);
FatCookies 13:4e77264f254a 175 initPID(&left_motor_pid, 1.0f, 0.f, 0.f);
FatCookies 13:4e77264f254a 176 initPID(&right_motor_pid, 1.0f, 0.f, 0.f);
maximusismax 8:7c5e6b1e7aa5 177
maximusismax 8:7c5e6b1e7aa5 178 valBufferIndex = 0;
FatCookies 13:4e77264f254a 179 }
FatCookies 13:4e77264f254a 180
FatCookies 13:4e77264f254a 181 int turning = 0;
FatCookies 13:4e77264f254a 182 int keepTurning = 0;
FatCookies 13:4e77264f254a 183 bool slow = false;
FatCookies 13:4e77264f254a 184 inline void recordMeasuredVal() {
FatCookies 12:da96e2f87465 185
FatCookies 13:4e77264f254a 186 measuredValBuffer[frame_counter % 64] = servo_pid.measured_value;
FatCookies 13:4e77264f254a 187
FatCookies 13:4e77264f254a 188 int count = 0;
FatCookies 13:4e77264f254a 189 for(i = 0; i < 10; i++) {
FatCookies 13:4e77264f254a 190 float val = abs(measuredValBuffer[(frame_counter - i) % 64]);
FatCookies 13:4e77264f254a 191 if(val > 0.09) {
FatCookies 13:4e77264f254a 192 count++;
FatCookies 13:4e77264f254a 193 }
FatCookies 13:4e77264f254a 194 }
FatCookies 12:da96e2f87465 195
FatCookies 14:13085e161dd1 196 if(turning) {
FatCookies 14:13085e161dd1 197 dutyCycleCorner(0.3,servo_pid.output);
FatCookies 14:13085e161dd1 198 //sensorCorner(left_motor_pid.desired_value, right_motor_pid.desired_value , servo_pid.output, 50);
FatCookies 14:13085e161dd1 199 }
FatCookies 14:13085e161dd1 200
FatCookies 13:4e77264f254a 201 if(abs(servo_pid.measured_value) > 0.11f){
FatCookies 13:4e77264f254a 202 if(!turning) {
FatCookies 13:4e77264f254a 203 sendString("start turning");
FatCookies 13:4e77264f254a 204
FatCookies 13:4e77264f254a 205 TFC_SetMotorPWM(0.2,0.2);
FatCookies 14:13085e161dd1 206
FatCookies 13:4e77264f254a 207 turning = 1;
FatCookies 13:4e77264f254a 208
FatCookies 13:4e77264f254a 209 } else {
FatCookies 13:4e77264f254a 210 turning++;
FatCookies 13:4e77264f254a 211 }
FatCookies 13:4e77264f254a 212
FatCookies 13:4e77264f254a 213 } else {
FatCookies 13:4e77264f254a 214 if(turning) {
FatCookies 13:4e77264f254a 215 if(keepTurning == 0 || count > 6) {
FatCookies 13:4e77264f254a 216 keepTurning++;
FatCookies 13:4e77264f254a 217 } else {
FatCookies 13:4e77264f254a 218 sendString("stop turning turned=%d",turning);
FatCookies 13:4e77264f254a 219 keepTurning = 0;
FatCookies 13:4e77264f254a 220 turning = 0;
FatCookies 13:4e77264f254a 221 TFC_SetMotorPWM(speed,speed);
FatCookies 13:4e77264f254a 222 }
FatCookies 13:4e77264f254a 223
FatCookies 13:4e77264f254a 224 }
FatCookies 13:4e77264f254a 225 }
FatCookies 13:4e77264f254a 226
maximusismax 8:7c5e6b1e7aa5 227 }
maximusismax 8:7c5e6b1e7aa5 228
maximusismax 8:7c5e6b1e7aa5 229 inline void sendImage() {
maximusismax 8:7c5e6b1e7aa5 230 //Only send 1/3 of camera frames to GUI program
maximusismax 8:7c5e6b1e7aa5 231 if((frame_counter % 3) == 0) {
maximusismax 8:7c5e6b1e7aa5 232 pc.putc('H');
maximusismax 8:7c5e6b1e7aa5 233 for(i = 0; i < 128; i++) {
maximusismax 8:7c5e6b1e7aa5 234 pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);
FatCookies 13:4e77264f254a 235 }
FatCookies 13:4e77264f254a 236 sendBattery();
FatCookies 13:4e77264f254a 237 }
FatCookies 13:4e77264f254a 238
FatCookies 13:4e77264f254a 239 frame_counter++;
FatCookies 13:4e77264f254a 240 }
FatCookies 13:4e77264f254a 241
FatCookies 13:4e77264f254a 242 inline void sendSpeeds() {
FatCookies 13:4e77264f254a 243 union {
FatCookies 12:da96e2f87465 244 float a;
FatCookies 12:da96e2f87465 245 unsigned char bytes[4];
FatCookies 12:da96e2f87465 246 } thing;
FatCookies 13:4e77264f254a 247
FatCookies 14:13085e161dd1 248
FatCookies 13:4e77264f254a 249 pc.putc('B');
FatCookies 14:13085e161dd1 250 thing.a = wL * WHEEL_RADIUS;
FatCookies 12:da96e2f87465 251 pc.putc(thing.bytes[0]);
FatCookies 12:da96e2f87465 252 pc.putc(thing.bytes[1]);
FatCookies 12:da96e2f87465 253 pc.putc(thing.bytes[2]);
FatCookies 12:da96e2f87465 254 pc.putc(thing.bytes[3]);
FatCookies 14:13085e161dd1 255 thing.a = wR * WHEEL_RADIUS;
FatCookies 13:4e77264f254a 256 pc.putc(thing.bytes[0]);
FatCookies 13:4e77264f254a 257 pc.putc(thing.bytes[1]);
FatCookies 13:4e77264f254a 258 pc.putc(thing.bytes[2]);
FatCookies 13:4e77264f254a 259 pc.putc(thing.bytes[3]);
maximusismax 8:7c5e6b1e7aa5 260 }
maximusismax 8:7c5e6b1e7aa5 261
FatCookies 13:4e77264f254a 262
maximusismax 8:7c5e6b1e7aa5 263 inline void handleComms() {
maximusismax 8:7c5e6b1e7aa5 264 if(curr_cmd != 0) {
FatCookies 4:4afa448c9cce 265 switch(curr_cmd) {
FatCookies 4:4afa448c9cce 266 case 'A':
FatCookies 4:4afa448c9cce 267 if(xb.cBuffer->available() >= 3) {
FatCookies 4:4afa448c9cce 268 char p = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 269 char i = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 270 char d = xb.cBuffer->read();
FatCookies 12:da96e2f87465 271 servo_pid.Kp = p/25.0f;
FatCookies 12:da96e2f87465 272 servo_pid.Ki = i/25.0f;
FatCookies 12:da96e2f87465 273 servo_pid.Kd = d/25.0f;
FatCookies 13:4e77264f254a 274 sendString("pid= Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", servo_pid.Kp, servo_pid.Ki, servo_pid.Kd, p, i, d);
maximusismax 8:7c5e6b1e7aa5 275
FatCookies 4:4afa448c9cce 276 curr_cmd = 0;
FatCookies 4:4afa448c9cce 277 }
FatCookies 4:4afa448c9cce 278 break;
FatCookies 4:4afa448c9cce 279
FatCookies 4:4afa448c9cce 280 case 'F':
FatCookies 6:b0e160c51013 281 if(xb.cBuffer->available() >= 1) {
FatCookies 4:4afa448c9cce 282 char a = xb.cBuffer->read();
FatCookies 6:b0e160c51013 283 speed = a/256.0f;
FatCookies 7:ad893fc41b95 284 TFC_SetMotorPWM(speed,speed);
FatCookies 13:4e77264f254a 285 sendString("s = %u %f",a, speed);
FatCookies 4:4afa448c9cce 286 curr_cmd = 0;
FatCookies 4:4afa448c9cce 287 }
FatCookies 4:4afa448c9cce 288 break;
FatCookies 4:4afa448c9cce 289
FatCookies 4:4afa448c9cce 290 default:
FatCookies 13:4e77264f254a 291 // Unrecognised command
FatCookies 13:4e77264f254a 292 curr_cmd = 0;
FatCookies 4:4afa448c9cce 293 break;
FatCookies 4:4afa448c9cce 294 }
FatCookies 4:4afa448c9cce 295 }
FatCookies 4:4afa448c9cce 296
FatCookies 6:b0e160c51013 297 if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
FatCookies 4:4afa448c9cce 298 char cmd = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 299 if(cmd == 'D') {
FatCookies 4:4afa448c9cce 300 TFC_InitServos(0.00052,0.00122,0.02);
FatCookies 4:4afa448c9cce 301 TFC_HBRIDGE_ENABLE;
FatCookies 10:1bd0224093e4 302 TFC_SetMotorPWM(RIGHT_MOTOR_COMPENSATION_RATIO*speed,speed);
FatCookies 12:da96e2f87465 303 servo_pid.integral = 0;
FatCookies 12:da96e2f87465 304
FatCookies 6:b0e160c51013 305
FatCookies 4:4afa448c9cce 306 } else if (cmd == 'C') {
FatCookies 4:4afa448c9cce 307 TFC_SetMotorPWM(0.0,0.0);
FatCookies 4:4afa448c9cce 308 TFC_HBRIDGE_DISABLE;
FatCookies 4:4afa448c9cce 309 } else if(cmd == 'A') {
FatCookies 4:4afa448c9cce 310 curr_cmd = 'A';
FatCookies 4:4afa448c9cce 311 } else if(cmd == 'F') {
FatCookies 4:4afa448c9cce 312 curr_cmd = 'F';
FatCookies 4:4afa448c9cce 313 }
FatCookies 4:4afa448c9cce 314
FatCookies 4:4afa448c9cce 315 }
maximusismax 8:7c5e6b1e7aa5 316 }
maximusismax 8:7c5e6b1e7aa5 317 inline float findCentreValue() {
maximusismax 8:7c5e6b1e7aa5 318
maximusismax 8:7c5e6b1e7aa5 319 diff = 0;
maximusismax 8:7c5e6b1e7aa5 320 prev = -1;
maximusismax 8:7c5e6b1e7aa5 321 leftChange = left;
maximusismax 8:7c5e6b1e7aa5 322 for(i = 63; i > 0; i--) {
maximusismax 8:7c5e6b1e7aa5 323 curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
maximusismax 8:7c5e6b1e7aa5 324 diff = prev - curr_left;
maximusismax 8:7c5e6b1e7aa5 325 if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
maximusismax 8:7c5e6b1e7aa5 326 left = i;
maximusismax 8:7c5e6b1e7aa5 327 break;
maximusismax 8:7c5e6b1e7aa5 328 }
maximusismax 8:7c5e6b1e7aa5 329 prev = curr_left;
maximusismax 8:7c5e6b1e7aa5 330 }
maximusismax 8:7c5e6b1e7aa5 331
maximusismax 8:7c5e6b1e7aa5 332 prev = -1;
maximusismax 8:7c5e6b1e7aa5 333 rightChange = right;
maximusismax 8:7c5e6b1e7aa5 334 for(i = 64; i < 128; i++) {
maximusismax 8:7c5e6b1e7aa5 335 curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
maximusismax 8:7c5e6b1e7aa5 336 int diff = prev - curr_right;
maximusismax 8:7c5e6b1e7aa5 337 if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
maximusismax 8:7c5e6b1e7aa5 338 right = i;
maximusismax 8:7c5e6b1e7aa5 339 break;
maximusismax 8:7c5e6b1e7aa5 340 }
maximusismax 8:7c5e6b1e7aa5 341 prev = curr_right;
maximusismax 8:7c5e6b1e7aa5 342 }
maximusismax 8:7c5e6b1e7aa5 343
maximusismax 8:7c5e6b1e7aa5 344 //Calculate how left/right we are
FatCookies 13:4e77264f254a 345 servo_pid.measured_value = (64 - ((left+right)/2))/64.f;
maximusismax 8:7c5e6b1e7aa5 346
FatCookies 13:4e77264f254a 347 return servo_pid.measured_value;
maximusismax 8:7c5e6b1e7aa5 348 }
FatCookies 3:87a5122682fa 349
FatCookies 12:da96e2f87465 350 void handlePID(pid_instance *pid) {
FatCookies 12:da96e2f87465 351 pid->dt = t.read();
FatCookies 12:da96e2f87465 352 pid->pid_error = pid->desired_value - pid->measured_value;
FatCookies 12:da96e2f87465 353 pid->integral = pid->integral + pid->pid_error * pid->dt;
FatCookies 12:da96e2f87465 354 pid->derivative = (pid->pid_error - pid->p_error) / pid->dt;
FatCookies 12:da96e2f87465 355 pid->output = pid->Kp * pid->pid_error + pid->Ki * pid->integral + pid->Kd * pid->derivative;
FatCookies 12:da96e2f87465 356 pid->p_error = pid->pid_error;
FatCookies 12:da96e2f87465 357
FatCookies 12:da96e2f87465 358 if(pid->integral > 1.0f) {
FatCookies 12:da96e2f87465 359 pid->integral = 1.0f;
FatCookies 12:da96e2f87465 360 }
FatCookies 12:da96e2f87465 361 if(pid->integral < -1.0f) {
FatCookies 12:da96e2f87465 362 pid->integral = -1.0f;
FatCookies 12:da96e2f87465 363 }
FatCookies 12:da96e2f87465 364 }
FatCookies 12:da96e2f87465 365
maximusismax 8:7c5e6b1e7aa5 366 inline void PIDController() {
FatCookies 13:4e77264f254a 367 // update motor measurements
FatCookies 13:4e77264f254a 368 left_motor_pid.measured_value = wL;
FatCookies 13:4e77264f254a 369 right_motor_pid.measured_value = wR;
FatCookies 13:4e77264f254a 370
maximusismax 8:7c5e6b1e7aa5 371 //PID Stuff!
FatCookies 3:87a5122682fa 372 t.start();
FatCookies 12:da96e2f87465 373 handlePID(&servo_pid);
FatCookies 13:4e77264f254a 374 handlePID(&left_motor_pid);
FatCookies 13:4e77264f254a 375 handlePID(&right_motor_pid);
FatCookies 12:da96e2f87465 376
FatCookies 12:da96e2f87465 377 if((-1.0 <= servo_pid.output) && (servo_pid.output <= 1.0))
FatCookies 6:b0e160c51013 378 {
FatCookies 12:da96e2f87465 379 TFC_SetServo(0, servo_pid.output);
maximusismax 8:7c5e6b1e7aa5 380 }
maximusismax 8:7c5e6b1e7aa5 381 else //Unhappy PID state
maximusismax 8:7c5e6b1e7aa5 382 {
FatCookies 13:4e77264f254a 383 sendString("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
maximusismax 8:7c5e6b1e7aa5 384 TFC_InitServos(0.00052, 0.00122, 0.02);
FatCookies 12:da96e2f87465 385 if(servo_pid.output >= 1.0f) {
maximusismax 8:7c5e6b1e7aa5 386 TFC_SetServo(0, 0.9f);
FatCookies 12:da96e2f87465 387 servo_pid.output = 1.0f;
maximusismax 8:7c5e6b1e7aa5 388 } else {
maximusismax 8:7c5e6b1e7aa5 389 TFC_SetServo(0, -0.9f);
FatCookies 12:da96e2f87465 390 servo_pid.output = -1.0f;
maximusismax 8:7c5e6b1e7aa5 391 }
FatCookies 13:4e77264f254a 392 }
FatCookies 12:da96e2f87465 393
FatCookies 6:b0e160c51013 394
FatCookies 3:87a5122682fa 395 t.stop();
FatCookies 3:87a5122682fa 396 t.reset();
FatCookies 3:87a5122682fa 397 t.start();
FatCookies 9:aa2ce38dec6b 398 }
FatCookies 9:aa2ce38dec6b 399
FatCookies 13:4e77264f254a 400 inline void handleStartStop() {
FatCookies 13:4e77264f254a 401 //Hacky way to detect the start/stop signal
FatCookies 13:4e77264f254a 402 if(right - left < 60) {
FatCookies 13:4e77264f254a 403 sendString("START STOP!! &d",startstop);
FatCookies 13:4e77264f254a 404
FatCookies 13:4e77264f254a 405 if(seen) {
FatCookies 13:4e77264f254a 406 seen = false;
FatCookies 13:4e77264f254a 407 } else {
FatCookies 13:4e77264f254a 408 startstop++;
FatCookies 13:4e77264f254a 409 seen = true;
FatCookies 13:4e77264f254a 410 }
FatCookies 13:4e77264f254a 411 //If we've done 5 laps, stop the car
FatCookies 13:4e77264f254a 412 if(startstop >= 1) {
FatCookies 13:4e77264f254a 413 TFC_SetMotorPWM(0.f,0.f);
FatCookies 13:4e77264f254a 414 TFC_HBRIDGE_DISABLE;
FatCookies 13:4e77264f254a 415 startstop = 0;
FatCookies 13:4e77264f254a 416 }
FatCookies 13:4e77264f254a 417 }
FatCookies 13:4e77264f254a 418 }
FatCookies 9:aa2ce38dec6b 419
FatCookies 9:aa2ce38dec6b 420
FatCookies 9:aa2ce38dec6b 421 inline void initSpeedSensors() {
FatCookies 9:aa2ce38dec6b 422 t1.start();
FatCookies 9:aa2ce38dec6b 423 t2.start();
FatCookies 9:aa2ce38dec6b 424
FatCookies 9:aa2ce38dec6b 425 //Left and Right are defined looking at the rear of the car, in the direction the camera points at.
FatCookies 9:aa2ce38dec6b 426 leftHallSensor.rise(&GetTime_L);
FatCookies 9:aa2ce38dec6b 427 rightHallSensor.rise(&GetTime_R);
FatCookies 9:aa2ce38dec6b 428 }
FatCookies 9:aa2ce38dec6b 429
FatCookies 9:aa2ce38dec6b 430 void GetTime_L(){
FatCookies 9:aa2ce38dec6b 431 Time_L=t1.read_us();
FatCookies 9:aa2ce38dec6b 432 t1.reset();
FatCookies 9:aa2ce38dec6b 433 }
FatCookies 9:aa2ce38dec6b 434
FatCookies 9:aa2ce38dec6b 435 void GetTime_R(){
FatCookies 9:aa2ce38dec6b 436 Time_R=t2.read_us();
FatCookies 9:aa2ce38dec6b 437 t2.reset();
maximusismax 0:566127ca8048 438 }