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@18:0095a3a8f8e4, 2016-12-06 (annotated)

Committer:
lh14g13
Date:
Tue Dec 06 14:15:55 2016 +0000
Revision:
18:0095a3a8f8e4
Parent:
17:6ae90788cc2b
Child:
19:65f0b6febc23
added test functions;

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