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@15:ccde02f96449, 2016-12-01 (annotated)

Committer:
FatCookies
Date:
Thu Dec 01 16:56:17 2016 +0000
Revision:
15:ccde02f96449
Parent:
14:13085e161dd1
Child:
16:81cdffd8c5d5
added slowing down for corners using second "lookahead" camera

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