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@24:15264aee54d1, 2016-12-13 (annotated)

Committer:
lh14g13
Date:
Tue Dec 13 10:07:30 2016 +0000
Revision:
24:15264aee54d1
Parent:
20:ed954836d028 
added a croner speed variable i think?

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