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@30:6c047af9f0cc, 2017-01-11 (annotated)

Committer:
lh14g13
Date:
Wed Jan 11 11:18:03 2017 +0000
Revision:
30:6c047af9f0cc
Parent:
29:b5b31256572b 
added changes to the motor library

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
FatCookies 27:627d67e3b9b0 35 Timer lapTimer;
FatCookies 16:81cdffd8c5d5 36
FatCookies 21:0b69fada7c5f 37
maximusismax 0:566127ca8048 38 int main() {
maximusismax 8:7c5e6b1e7aa5 39 //Set up TFC driver stuff
maximusismax 0:566127ca8048 40 TFC_Init();
FatCookies 17:6ae90788cc2b 41 ALIGN_SERVO;
FatCookies 13:4e77264f254a 42
FatCookies 17:6ae90788cc2b 43 #if USE_COMMS
FatCookies 17:6ae90788cc2b 44 //Setup baud rate for serial link, do not change!
FatCookies 17:6ae90788cc2b 45 pc.baud(BAUD_RATE);
FatCookies 17:6ae90788cc2b 46 #endif
maximusismax 0:566127ca8048 47
maximusismax 8:7c5e6b1e7aa5 48 //Initialise/reset PID variables
maximusismax 8:7c5e6b1e7aa5 49 initVariables();
FatCookies 9:aa2ce38dec6b 50 initSpeedSensors();
FatCookies 12:da96e2f87465 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 21:0b69fada7c5f 64 // Check if car is at the stop line
FatCookies 21:0b69fada7c5f 65 handleStartStop();
FatCookies 21:0b69fada7c5f 66
FatCookies 27:627d67e3b9b0 67 #if USE_COMMS
FatCookies 27:627d67e3b9b0 68 // Send the line scan image over serial
FatCookies 27:627d67e3b9b0 69 sendImage();
FatCookies 27:627d67e3b9b0 70 #endif
FatCookies 21:0b69fada7c5f 71
FatCookies 21:0b69fada7c5f 72
FatCookies 21:0b69fada7c5f 73 //Reset image ready flag
FatCookies 21:0b69fada7c5f 74 TFC_LineScanImageReady=0;
lh14g13 19:65f0b6febc23 75
FatCookies 17:6ae90788cc2b 76 // Slow down, adjust PID values and enable differential before corners.
lh14g13 30:6c047af9f0cc 77 handleCornering();
FatCookies 17:6ae90788cc2b 78
FatCookies 13:4e77264f254a 79 // Run the PID controllers and adjust steering/motor accordingly
maximusismax 8:7c5e6b1e7aa5 80 PIDController();
maximusismax 8:7c5e6b1e7aa5 81
FatCookies 21:0b69fada7c5f 82
FatCookies 21:0b69fada7c5f 83
FatCookies 21:0b69fada7c5f 84
FatCookies 17:6ae90788cc2b 85 #if USE_COMMS
FatCookies 17:6ae90788cc2b 86 // Send the wheel speeds over serial
FatCookies 17:6ae90788cc2b 87 sendSpeeds();
FatCookies 17:6ae90788cc2b 88 #endif
FatCookies 13:4e77264f254a 89
FatCookies 21:0b69fada7c5f 90
maximusismax 8:7c5e6b1e7aa5 91
FatCookies 15:ccde02f96449 92
FatCookies 21:0b69fada7c5f 93
maximusismax 8:7c5e6b1e7aa5 94 }
maximusismax 8:7c5e6b1e7aa5 95 }
maximusismax 8:7c5e6b1e7aa5 96 }
maximusismax 8:7c5e6b1e7aa5 97
FatCookies 17:6ae90788cc2b 98 void initVariables() {
FatCookies 17:6ae90788cc2b 99 // Initialise three PID controllers for the servo and each wheel.
FatCookies 17:6ae90788cc2b 100 initPID(&servo_pid, 2.2f, 0.6f, 0.f);
FatCookies 21:0b69fada7c5f 101 initPID(&left_motor_pid, 0.01f, 0.f, 0.f);
FatCookies 21:0b69fada7c5f 102 initPID(&right_motor_pid, 0.01f, 0.f, 0.f);
FatCookies 17:6ae90788cc2b 103
lh14g13 19:65f0b6febc23 104 right_motor_pid.desired_value=0;
lh14g13 19:65f0b6febc23 105 left_motor_pid.desired_value=0;
lh14g13 19:65f0b6febc23 106
lh14g13 19:65f0b6febc23 107 // intialise the maximum speed interms of the angular speed.
FatCookies 17:6ae90788cc2b 108 valBufferIndex = 0;
lh14g13 19:65f0b6febc23 109 speed = 50;
FatCookies 13:4e77264f254a 110
FatCookies 17:6ae90788cc2b 111 //Start stop
FatCookies 17:6ae90788cc2b 112 startstop = 0;
FatCookies 17:6ae90788cc2b 113 seen = false;
FatCookies 17:6ae90788cc2b 114
FatCookies 17:6ae90788cc2b 115 // Turning
FatCookies 17:6ae90788cc2b 116 turning = 0;
FatCookies 17:6ae90788cc2b 117 keepTurning = 0;
FatCookies 17:6ae90788cc2b 118 slow = false;
FatCookies 20:ed954836d028 119
FatCookies 20:ed954836d028 120 wL = 0;
FatCookies 20:ed954836d028 121 wR = 0;
FatCookies 20:ed954836d028 122 prevL = 0;
FatCookies 20:ed954836d028 123 prevR = 0;
lh14g13 19:65f0b6febc23 124
maximusismax 8:7c5e6b1e7aa5 125 }
maximusismax 8:7c5e6b1e7aa5 126
FatCookies 13:4e77264f254a 127 void initPID(pid_instance* pid, float Kp, float Ki, float Kd) {
FatCookies 17:6ae90788cc2b 128 pid->Kp = Kp;
FatCookies 13:4e77264f254a 129 pid->Ki = Ki;
FatCookies 13:4e77264f254a 130 pid->Kd = Kd;
FatCookies 13:4e77264f254a 131 pid->dt = 0;
FatCookies 13:4e77264f254a 132 pid->p_error = 0;
FatCookies 13:4e77264f254a 133 pid->pid_error = 0;
FatCookies 13:4e77264f254a 134 pid->integral = 0;
FatCookies 13:4e77264f254a 135 pid->measured_value = 0;
FatCookies 13:4e77264f254a 136 pid->desired_value = 0;
FatCookies 13:4e77264f254a 137 pid->derivative = 0;
FatCookies 13:4e77264f254a 138 }
FatCookies 13:4e77264f254a 139
FatCookies 29:b5b31256572b 140 bool leftSeen;
FatCookies 29:b5b31256572b 141 bool rightSeen;
FatCookies 29:b5b31256572b 142
FatCookies 17:6ae90788cc2b 143 inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &l, uint8_t &r) {
FatCookies 17:6ae90788cc2b 144
FatCookies 17:6ae90788cc2b 145 diff = 0;
FatCookies 17:6ae90788cc2b 146 prev = -1;
FatCookies 29:b5b31256572b 147
FatCookies 29:b5b31256572b 148 leftSeen = false;
FatCookies 29:b5b31256572b 149 rightSeen = false;
FatCookies 29:b5b31256572b 150
FatCookies 17:6ae90788cc2b 151 for(i = 63; i > 0; i--) {
oj3g13 26:f3d770f3eda1 152 curr_left = (uint8_t)(cam_data[i] >> 4) & 0xFF;
FatCookies 17:6ae90788cc2b 153 diff = prev - curr_left;
FatCookies 17:6ae90788cc2b 154 if(abs(diff) >= CAM_DIFF && curr_left <= CAM_THRESHOLD && prev != -1) {
FatCookies 17:6ae90788cc2b 155 l = i;
FatCookies 29:b5b31256572b 156 leftSeen = true;
FatCookies 17:6ae90788cc2b 157 break;
FatCookies 17:6ae90788cc2b 158 }
FatCookies 17:6ae90788cc2b 159 prev = curr_left;
FatCookies 17:6ae90788cc2b 160 }
maximusismax 8:7c5e6b1e7aa5 161
FatCookies 29:b5b31256572b 162
FatCookies 29:b5b31256572b 163
FatCookies 17:6ae90788cc2b 164 prev = -1;
FatCookies 17:6ae90788cc2b 165 for(i = 64; i < 128; i++) {
oj3g13 26:f3d770f3eda1 166 curr_right = (uint8_t)(cam_data[i] >> 4) & 0xFF;
FatCookies 17:6ae90788cc2b 167 int diff = prev - curr_right;
FatCookies 17:6ae90788cc2b 168 if(abs(diff) >= CAM_DIFF && curr_right <= CAM_THRESHOLD && prev != -1) {
FatCookies 17:6ae90788cc2b 169 r = i;
FatCookies 29:b5b31256572b 170 rightSeen = true;
FatCookies 17:6ae90788cc2b 171 break;
FatCookies 17:6ae90788cc2b 172 }
FatCookies 17:6ae90788cc2b 173 prev = curr_right;
FatCookies 17:6ae90788cc2b 174 }
FatCookies 17:6ae90788cc2b 175
FatCookies 29:b5b31256572b 176 if(!rightSeen && !leftSeen) {
FatCookies 29:b5b31256572b 177 sendString("lost edges");
FatCookies 29:b5b31256572b 178 ALIGN_SERVO;
FatCookies 29:b5b31256572b 179 servo_pid.integral = 0;
FatCookies 29:b5b31256572b 180 }
FatCookies 29:b5b31256572b 181
FatCookies 17:6ae90788cc2b 182 //Calculate how left/right we are
FatCookies 17:6ae90788cc2b 183 return (64 - ((l+r)/2))/64.f;
FatCookies 13:4e77264f254a 184 }
FatCookies 13:4e77264f254a 185
FatCookies 17:6ae90788cc2b 186 inline void handleCornering() {
FatCookies 12:da96e2f87465 187
FatCookies 17:6ae90788cc2b 188 float lookaheadMeasuredValue = findCentreValue(LOOKAHEAD_CAMERA, farLeft, farRight);
FatCookies 15:ccde02f96449 189
FatCookies 13:4e77264f254a 190 measuredValBuffer[frame_counter % 64] = servo_pid.measured_value;
FatCookies 13:4e77264f254a 191
FatCookies 13:4e77264f254a 192 int count = 0;
FatCookies 13:4e77264f254a 193 for(i = 0; i < 10; i++) {
FatCookies 13:4e77264f254a 194 float val = abs(measuredValBuffer[(frame_counter - i) % 64]);
FatCookies 13:4e77264f254a 195 if(val > 0.09) {
FatCookies 13:4e77264f254a 196 count++;
FatCookies 13:4e77264f254a 197 }
FatCookies 13:4e77264f254a 198 }
FatCookies 12:da96e2f87465 199
FatCookies 29:b5b31256572b 200 /*if(!turning && abs(lookaheadMeasuredValue) > 0.11f){
FatCookies 15:ccde02f96449 201 TFC_SetMotorPWM(0.4,0.4);
FatCookies 15:ccde02f96449 202 }
FatCookies 29:b5b31256572b 203 */
FatCookies 15:ccde02f96449 204
lh14g13 30:6c047af9f0cc 205 if(true) {
lh14g13 18:0095a3a8f8e4 206
lh14g13 18:0095a3a8f8e4 207 //default
lh14g13 18:0095a3a8f8e4 208 //TFC_SetMotorPWM(0.4,0.4);
lh14g13 18:0095a3a8f8e4 209
lh14g13 19:65f0b6febc23 210 //dutyCycleCorner(speed,servo_pid.output);
lh14g13 18:0095a3a8f8e4 211
lh14g13 18:0095a3a8f8e4 212 //may want to have just to set cornering speed at different if going to be slowing down for cornering.
lh14g13 18:0095a3a8f8e4 213 //dutyCycleCorner(float cornerspeed, servo_pid.output);
lh14g13 18:0095a3a8f8e4 214 //dutyCycleCorner(speed, servo_pid.output);
lh14g13 18:0095a3a8f8e4 215
FatCookies 29:b5b31256572b 216 sensorCorner(left_motor_pid.desired_value, right_motor_pid.desired_value , servo_pid.output, speed);
FatCookies 14:13085e161dd1 217 }
FatCookies 14:13085e161dd1 218
FatCookies 29:b5b31256572b 219 /*
FatCookies 13:4e77264f254a 220 if(abs(servo_pid.measured_value) > 0.11f){
FatCookies 15:ccde02f96449 221 if(!turning) {
FatCookies 13:4e77264f254a 222 turning = 1;
FatCookies 13:4e77264f254a 223 } else {
FatCookies 13:4e77264f254a 224 turning++;
FatCookies 13:4e77264f254a 225 }
FatCookies 13:4e77264f254a 226
FatCookies 13:4e77264f254a 227 } else {
FatCookies 13:4e77264f254a 228 if(turning) {
FatCookies 13:4e77264f254a 229 if(keepTurning == 0 || count > 6) {
FatCookies 13:4e77264f254a 230 keepTurning++;
FatCookies 13:4e77264f254a 231 } else {
FatCookies 15:ccde02f96449 232 //sendString("stop turning turned=%d",turning);
FatCookies 13:4e77264f254a 233 keepTurning = 0;
FatCookies 13:4e77264f254a 234 turning = 0;
FatCookies 13:4e77264f254a 235 TFC_SetMotorPWM(speed,speed);
FatCookies 13:4e77264f254a 236 }
FatCookies 13:4e77264f254a 237
FatCookies 13:4e77264f254a 238 }
FatCookies 13:4e77264f254a 239 }
FatCookies 29:b5b31256572b 240 */
FatCookies 13:4e77264f254a 241
maximusismax 8:7c5e6b1e7aa5 242 }
maximusismax 8:7c5e6b1e7aa5 243
FatCookies 17:6ae90788cc2b 244 inline float getLineEntropy() {
FatCookies 17:6ae90788cc2b 245 float entropy = 0;
FatCookies 17:6ae90788cc2b 246 float last = (int8_t)(CLOSE_CAMERA[0] >> 4) & 0xFF;
FatCookies 17:6ae90788cc2b 247 for(int i = 1; i < 128; i++) {
FatCookies 17:6ae90788cc2b 248 entropy += abs(last - ((int8_t)(CLOSE_CAMERA[i] >> 4) & 0xFF));
FatCookies 17:6ae90788cc2b 249 }
FatCookies 17:6ae90788cc2b 250 return entropy;
FatCookies 17:6ae90788cc2b 251 }
FatCookies 17:6ae90788cc2b 252
FatCookies 17:6ae90788cc2b 253 void handlePID(pid_instance *pid) {
FatCookies 17:6ae90788cc2b 254 pid->dt = t.read();
FatCookies 17:6ae90788cc2b 255 pid->pid_error = pid->desired_value - pid->measured_value;
FatCookies 17:6ae90788cc2b 256 pid->integral = pid->integral + pid->pid_error * pid->dt;
FatCookies 17:6ae90788cc2b 257 pid->derivative = (pid->pid_error - pid->p_error) / pid->dt;
FatCookies 17:6ae90788cc2b 258 pid->output = pid->Kp * pid->pid_error + pid->Ki * pid->integral + pid->Kd * pid->derivative;
FatCookies 17:6ae90788cc2b 259 pid->p_error = pid->pid_error;
FatCookies 17:6ae90788cc2b 260
FatCookies 17:6ae90788cc2b 261 if(pid->integral > 1.0f) {
FatCookies 17:6ae90788cc2b 262 pid->integral = 1.0f;
FatCookies 17:6ae90788cc2b 263 }
FatCookies 27:627d67e3b9b0 264 if(pid->integral < -1.0f ) {
FatCookies 17:6ae90788cc2b 265 pid->integral = -1.0f;
FatCookies 17:6ae90788cc2b 266 }
FatCookies 17:6ae90788cc2b 267 }
FatCookies 17:6ae90788cc2b 268
lh14g13 19:65f0b6febc23 269
FatCookies 17:6ae90788cc2b 270 inline void PIDController() {
FatCookies 27:627d67e3b9b0 271 // update motor measurements
lh14g13 19:65f0b6febc23 272 // Read the angular velocity of both wheels
FatCookies 27:627d67e3b9b0 273
FatCookies 27:627d67e3b9b0 274 prevL = wL;
FatCookies 27:627d67e3b9b0 275 prevR = wR;
FatCookies 27:627d67e3b9b0 276
lh14g13 19:65f0b6febc23 277 wL=Get_Speed(Time_L);
lh14g13 19:65f0b6febc23 278 wR=Get_Speed(Time_R);
FatCookies 20:ed954836d028 279
FatCookies 20:ed954836d028 280 // Check if left wheel is slipping/giving an abnormal reading and ignore reading
FatCookies 20:ed954836d028 281 if(wL - prevL < 1.2/0.025) {
FatCookies 20:ed954836d028 282 left_motor_pid.measured_value = wL;
FatCookies 20:ed954836d028 283 }
FatCookies 20:ed954836d028 284 if(wR - prevR < 1.2/0.025) {
FatCookies 20:ed954836d028 285 right_motor_pid.measured_value = wR;
FatCookies 20:ed954836d028 286 }
FatCookies 17:6ae90788cc2b 287
FatCookies 21:0b69fada7c5f 288
FatCookies 17:6ae90788cc2b 289 //PID Stuff!
FatCookies 17:6ae90788cc2b 290 t.start();
FatCookies 17:6ae90788cc2b 291 handlePID(&servo_pid);
FatCookies 17:6ae90788cc2b 292 handlePID(&left_motor_pid);
FatCookies 17:6ae90788cc2b 293 handlePID(&right_motor_pid);
FatCookies 17:6ae90788cc2b 294
FatCookies 17:6ae90788cc2b 295 if((-1.0 <= servo_pid.output) && (servo_pid.output <= 1.0))
FatCookies 17:6ae90788cc2b 296 {
FatCookies 17:6ae90788cc2b 297 TFC_SetServo(0, servo_pid.output);
FatCookies 17:6ae90788cc2b 298 }
FatCookies 17:6ae90788cc2b 299 else //Unhappy PID state
FatCookies 17:6ae90788cc2b 300 {
FatCookies 17:6ae90788cc2b 301 //sendString("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
FatCookies 17:6ae90788cc2b 302 ALIGN_SERVO;
FatCookies 17:6ae90788cc2b 303 if(servo_pid.output >= 1.0f) {
FatCookies 17:6ae90788cc2b 304 TFC_SetServo(0, 0.9f);
FatCookies 17:6ae90788cc2b 305 servo_pid.output = 1.0f;
FatCookies 17:6ae90788cc2b 306 } else {
FatCookies 17:6ae90788cc2b 307 TFC_SetServo(0, -0.9f);
FatCookies 17:6ae90788cc2b 308 servo_pid.output = -1.0f;
FatCookies 17:6ae90788cc2b 309 }
FatCookies 17:6ae90788cc2b 310 }
FatCookies 17:6ae90788cc2b 311
FatCookies 17:6ae90788cc2b 312
FatCookies 20:ed954836d028 313 if(left_motor_pid.output > 1.0f) {
FatCookies 20:ed954836d028 314 left_motor_pid.output = 1.0f;
lh14g13 19:65f0b6febc23 315 }
FatCookies 29:b5b31256572b 316 if(left_motor_pid.output < 0.0f) {
FatCookies 27:627d67e3b9b0 317 left_motor_pid.output = 0.0f;
lh14g13 19:65f0b6febc23 318 }
lh14g13 19:65f0b6febc23 319
FatCookies 20:ed954836d028 320 if(right_motor_pid.output > 1.0f) {
FatCookies 20:ed954836d028 321 right_motor_pid.output = 1.0f;
FatCookies 20:ed954836d028 322 }
FatCookies 29:b5b31256572b 323 if(right_motor_pid.output < 0.0f) {
FatCookies 27:627d67e3b9b0 324 right_motor_pid.output = 0.0f;
FatCookies 20:ed954836d028 325 }
lh14g13 19:65f0b6febc23 326
FatCookies 20:ed954836d028 327 TFC_SetMotorPWM(left_motor_pid.output,right_motor_pid.output);
lh14g13 19:65f0b6febc23 328
FatCookies 21:0b69fada7c5f 329
FatCookies 17:6ae90788cc2b 330 t.stop();
FatCookies 17:6ae90788cc2b 331 t.reset();
FatCookies 17:6ae90788cc2b 332 t.start();
FatCookies 17:6ae90788cc2b 333 }
FatCookies 17:6ae90788cc2b 334
FatCookies 17:6ae90788cc2b 335 inline void handleStartStop() {
maximusismax 22:973b95478663 336
maximusismax 22:973b95478663 337 //v1:
FatCookies 17:6ae90788cc2b 338 //Hacky way to detect the start/stop signal
oj3g13 26:f3d770f3eda1 339 /*if(right - left < 60) {
maximusismax 22:973b95478663 340 sendString("START STOP!!");
FatCookies 21:0b69fada7c5f 341
maximusismax 22:973b95478663 342 TFC_SetMotorPWM(0.f,0.f);
maximusismax 22:973b95478663 343 TFC_HBRIDGE_DISABLE;
maximusismax 22:973b95478663 344 startstop = 0;
oj3g13 26:f3d770f3eda1 345 }*/
FatCookies 21:0b69fada7c5f 346
maximusismax 22:973b95478663 347 //----------------------------START/STOP v2-----------------------------
maximusismax 22:973b95478663 348 //New method plan:
maximusismax 22:973b95478663 349 //Start at the centre of the image
maximusismax 22:973b95478663 350 //Look for 2 transitions (B->W OR W->B) - ignoring whether the centre pixel was black or white
maximusismax 22:973b95478663 351 //this should efficiently detect whether the marker is visible, and shouldn't give too many false positives
maximusismax 22:973b95478663 352 //May need to fiddle with the track width initial check, or disbale it entirely.
maximusismax 22:973b95478663 353 //NB: May want to incorporate this into findCentreValue(), it looks like they will be operating in similar ways on the exact same data...
maximusismax 22:973b95478663 354 /*
maximusismax 22:973b95478663 355 uint8_t lastPixel, currentPixel;
maximusismax 22:973b95478663 356 lastPixel = -1; //or 0?
maximusismax 22:973b95478663 357 bool startStopLeft = false;
maximusismax 22:973b95478663 358 bool startStopRight = false
maximusismax 22:973b95478663 359 //So:
maximusismax 22:973b95478663 360 //1. Starting at the middle, step left, looking for 2 transitions
maximusismax 22:973b95478663 361 for(int i = 63; i > 0; i--) {
maximusismax 22:973b95478663 362 currentPixel = (uint8_t)(CLOSE_CAMERA[i] >> 4) & 0xFF; //Cast to signed or unsigned? Edge detection code has signed, but it puts it in an unsigned variable...
maximusismax 22:973b95478663 363 if((lastPixel - currentPixel) > 10) { //1st transition
maximusismax 22:973b95478663 364 for (int j = i; j > 0; j--) { //Keep going until 2nd transition
maximusismax 22:973b95478663 365 if((lastPixel - currentPixel) > 10) { //2nd transition
maximusismax 22:973b95478663 366 //Set flag that 2 transitions on the left side are identified
maximusismax 22:973b95478663 367 startStopLeft = true;
maximusismax 22:973b95478663 368 //goto finishLeft;
maximusismax 22:973b95478663 369 }
maximusismax 22:973b95478663 370 }
maximusismax 22:973b95478663 371
maximusismax 22:973b95478663 372 }
maximusismax 22:973b95478663 373 startStopLeft = false;
maximusismax 22:973b95478663 374 lastPixel = currentPixel;
maximusismax 22:973b95478663 375 }
maximusismax 22:973b95478663 376 //finishLeft:
maximusismax 22:973b95478663 377 */
maximusismax 22:973b95478663 378
oj3g13 26:f3d770f3eda1 379 //v2.5:--------------
oj3g13 26:f3d770f3eda1 380
maximusismax 22:973b95478663 381 int slower = 0;
oj3g13 26:f3d770f3eda1 382 int difference = 0;
oj3g13 26:f3d770f3eda1 383 int lastPixel, currentPixel, transitionsSeen;
maximusismax 22:973b95478663 384 lastPixel = -1;
maximusismax 22:973b95478663 385 transitionsSeen = 0;
maximusismax 22:973b95478663 386 //Starting near the left edge, step right, counting transitions. If there are 4, it is the marker (what about 3?)
maximusismax 22:973b95478663 387 for(int i = 30; i < 98; i++) {
oj3g13 26:f3d770f3eda1 388 currentPixel = (int)(CLOSE_CAMERA[i] >> 4) & 0xFF;
maximusismax 22:973b95478663 389 difference = lastPixel - currentPixel;
FatCookies 27:627d67e3b9b0 390 if(abs(difference) > 10 && lastPixel != -1){ //transition seen, increment counter
maximusismax 22:973b95478663 391 transitionsSeen++;
oj3g13 26:f3d770f3eda1 392 i+=5;
maximusismax 22:973b95478663 393 }
maximusismax 22:973b95478663 394 lastPixel = currentPixel;
maximusismax 22:973b95478663 395 }
oj3g13 26:f3d770f3eda1 396 //if (slower % 1000 == 0) {
oj3g13 26:f3d770f3eda1 397 //sendString("Transitions seen: %d", transitionsSeen);
oj3g13 26:f3d770f3eda1 398 //}
oj3g13 26:f3d770f3eda1 399 if(transitionsSeen >= 5) {
maximusismax 22:973b95478663 400 //Stop the car!
oj3g13 26:f3d770f3eda1 401 sendString("Start/stop seen");
oj3g13 26:f3d770f3eda1 402 TFC_SetMotorPWM(0.f,0.f);
oj3g13 26:f3d770f3eda1 403 TFC_HBRIDGE_DISABLE;
lh14g13 30:6c047af9f0cc 404 //lapTime();
oj3g13 26:f3d770f3eda1 405 }
oj3g13 26:f3d770f3eda1 406 // slower++;
oj3g13 26:f3d770f3eda1 407
FatCookies 17:6ae90788cc2b 408 }
FatCookies 17:6ae90788cc2b 409
FatCookies 17:6ae90788cc2b 410
FatCookies 17:6ae90788cc2b 411 inline void initSpeedSensors() {
FatCookies 17:6ae90788cc2b 412 t1.start();
FatCookies 17:6ae90788cc2b 413 t2.start();
FatCookies 17:6ae90788cc2b 414
FatCookies 17:6ae90788cc2b 415 //Left and Right are defined looking at the rear of the car, in the direction the camera points at.
FatCookies 17:6ae90788cc2b 416 leftHallSensor.rise(&GetTime_L);
FatCookies 17:6ae90788cc2b 417 rightHallSensor.rise(&GetTime_R);
FatCookies 17:6ae90788cc2b 418 }
FatCookies 17:6ae90788cc2b 419
FatCookies 17:6ae90788cc2b 420 void GetTime_L(){
FatCookies 17:6ae90788cc2b 421 Time_L=t1.read_us();
FatCookies 17:6ae90788cc2b 422 t1.reset();
FatCookies 17:6ae90788cc2b 423 }
FatCookies 17:6ae90788cc2b 424
FatCookies 17:6ae90788cc2b 425 void GetTime_R(){
FatCookies 17:6ae90788cc2b 426 Time_R=t2.read_us();
FatCookies 17:6ae90788cc2b 427 t2.reset();
FatCookies 17:6ae90788cc2b 428 }
FatCookies 17:6ae90788cc2b 429
FatCookies 17:6ae90788cc2b 430 #if USE_COMMS
FatCookies 17:6ae90788cc2b 431 void sendBattery() {
FatCookies 17:6ae90788cc2b 432
FatCookies 17:6ae90788cc2b 433 if(frame_counter % 256 == 0) {
FatCookies 17:6ae90788cc2b 434 float level = TFC_ReadBatteryVoltage() * 6.25;
FatCookies 17:6ae90788cc2b 435 pc.putc('J');
FatCookies 28:613239f10ba4 436 byte_float_union._float = level;
FatCookies 27:627d67e3b9b0 437 pc.putc(byte_float_union.bytes[0]);
FatCookies 27:627d67e3b9b0 438 pc.putc(byte_float_union.bytes[1]);
FatCookies 27:627d67e3b9b0 439 pc.putc(byte_float_union.bytes[2]);
FatCookies 27:627d67e3b9b0 440 pc.putc(byte_float_union.bytes[3]);
FatCookies 17:6ae90788cc2b 441 }
FatCookies 17:6ae90788cc2b 442 }
FatCookies 17:6ae90788cc2b 443
FatCookies 17:6ae90788cc2b 444 void sendString(const char *format, ...) {
FatCookies 17:6ae90788cc2b 445 va_list arg;
FatCookies 17:6ae90788cc2b 446
FatCookies 17:6ae90788cc2b 447 pc.putc('E');
FatCookies 17:6ae90788cc2b 448 va_start (arg, format);
FatCookies 17:6ae90788cc2b 449 pc.vprintf(format,arg);
FatCookies 17:6ae90788cc2b 450 va_end (arg);
FatCookies 17:6ae90788cc2b 451 pc.putc(0);
FatCookies 17:6ae90788cc2b 452 }
FatCookies 17:6ae90788cc2b 453
maximusismax 8:7c5e6b1e7aa5 454 inline void sendImage() {
maximusismax 8:7c5e6b1e7aa5 455 //Only send 1/3 of camera frames to GUI program
maximusismax 8:7c5e6b1e7aa5 456 if((frame_counter % 3) == 0) {
maximusismax 8:7c5e6b1e7aa5 457 pc.putc('H');
FatCookies 15:ccde02f96449 458 if(sendCam == 0) {
FatCookies 15:ccde02f96449 459 for(i = 0; i < 128; i++) {
FatCookies 17:6ae90788cc2b 460 pc.putc((int8_t)(CLOSE_CAMERA[i] >> 4) & 0xFF);
FatCookies 15:ccde02f96449 461 }
FatCookies 15:ccde02f96449 462 } else {
FatCookies 15:ccde02f96449 463 for(i = 0; i < 128; i++) {
FatCookies 17:6ae90788cc2b 464 pc.putc((int8_t)(LOOKAHEAD_CAMERA[i] >> 4) & 0xFF);
FatCookies 15:ccde02f96449 465 }
FatCookies 15:ccde02f96449 466 }
FatCookies 13:4e77264f254a 467 sendBattery();
FatCookies 13:4e77264f254a 468 }
FatCookies 13:4e77264f254a 469
FatCookies 13:4e77264f254a 470 frame_counter++;
FatCookies 13:4e77264f254a 471 }
FatCookies 13:4e77264f254a 472
FatCookies 13:4e77264f254a 473 inline void sendSpeeds() {
FatCookies 17:6ae90788cc2b 474
lh14g13 19:65f0b6febc23 475 /*float en = getLineEntropy();
FatCookies 15:ccde02f96449 476
FatCookies 15:ccde02f96449 477 if(onTrack) {
FatCookies 15:ccde02f96449 478 if(en <= 14000) {
FatCookies 15:ccde02f96449 479 onTrack = false;
FatCookies 15:ccde02f96449 480 sendString("offfffffffffffff");
FatCookies 15:ccde02f96449 481 TFC_SetMotorPWM(0.0,0.0);
FatCookies 15:ccde02f96449 482 TFC_HBRIDGE_DISABLE;
FatCookies 15:ccde02f96449 483 }
FatCookies 15:ccde02f96449 484 } else {
FatCookies 15:ccde02f96449 485 if(en > 14000) {
FatCookies 15:ccde02f96449 486 onTrack = true;
FatCookies 15:ccde02f96449 487 sendString("ON TRACK");
FatCookies 15:ccde02f96449 488 }
lh14g13 19:65f0b6febc23 489 }*/
FatCookies 15:ccde02f96449 490
FatCookies 14:13085e161dd1 491
FatCookies 13:4e77264f254a 492 pc.putc('B');
FatCookies 28:613239f10ba4 493 byte_float_union._float = wL * WHEEL_RADIUS;//left_motor_pid.output; //
FatCookies 27:627d67e3b9b0 494 pc.putc(byte_float_union.bytes[0]);
FatCookies 27:627d67e3b9b0 495 pc.putc(byte_float_union.bytes[1]);
FatCookies 27:627d67e3b9b0 496 pc.putc(byte_float_union.bytes[2]);
FatCookies 27:627d67e3b9b0 497 pc.putc(byte_float_union.bytes[3]);
FatCookies 28:613239f10ba4 498 byte_float_union._float = wR * WHEEL_RADIUS; // right_motor_pid.output; //
FatCookies 27:627d67e3b9b0 499 pc.putc(byte_float_union.bytes[0]);
FatCookies 27:627d67e3b9b0 500 pc.putc(byte_float_union.bytes[1]);
FatCookies 27:627d67e3b9b0 501 pc.putc(byte_float_union.bytes[2]);
FatCookies 27:627d67e3b9b0 502 pc.putc(byte_float_union.bytes[3]);
FatCookies 27:627d67e3b9b0 503
maximusismax 8:7c5e6b1e7aa5 504 }
maximusismax 8:7c5e6b1e7aa5 505
FatCookies 13:4e77264f254a 506
maximusismax 8:7c5e6b1e7aa5 507 inline void handleComms() {
maximusismax 8:7c5e6b1e7aa5 508 if(curr_cmd != 0) {
FatCookies 4:4afa448c9cce 509 switch(curr_cmd) {
FatCookies 4:4afa448c9cce 510 case 'A':
FatCookies 20:ed954836d028 511 if(xb.cBuffer->available() >= 12) {
FatCookies 20:ed954836d028 512
FatCookies 27:627d67e3b9b0 513 byte_float_union.bytes[0] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 514 byte_float_union.bytes[1] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 515 byte_float_union.bytes[2] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 516 byte_float_union.bytes[3] = xb.cBuffer->read();
FatCookies 28:613239f10ba4 517 servo_pid.Kp = byte_float_union._float;
FatCookies 20:ed954836d028 518
FatCookies 27:627d67e3b9b0 519 byte_float_union.bytes[0] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 520 byte_float_union.bytes[1] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 521 byte_float_union.bytes[2] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 522 byte_float_union.bytes[3] = xb.cBuffer->read();
FatCookies 28:613239f10ba4 523 servo_pid.Ki = byte_float_union._float;
FatCookies 20:ed954836d028 524
FatCookies 27:627d67e3b9b0 525 byte_float_union.bytes[0] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 526 byte_float_union.bytes[1] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 527 byte_float_union.bytes[2] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 528 byte_float_union.bytes[3] = xb.cBuffer->read();
FatCookies 28:613239f10ba4 529 servo_pid.Kd = byte_float_union._float;
FatCookies 20:ed954836d028 530
FatCookies 20:ed954836d028 531 sendString("pid= Kp: %f, Ki: %f, Kd: %f", servo_pid.Kp, servo_pid.Ki, servo_pid.Kd);
maximusismax 8:7c5e6b1e7aa5 532
FatCookies 4:4afa448c9cce 533 curr_cmd = 0;
FatCookies 4:4afa448c9cce 534 }
FatCookies 4:4afa448c9cce 535 break;
FatCookies 4:4afa448c9cce 536
FatCookies 4:4afa448c9cce 537 case 'F':
FatCookies 6:b0e160c51013 538 if(xb.cBuffer->available() >= 1) {
FatCookies 4:4afa448c9cce 539 char a = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 540 speed = a;
FatCookies 13:4e77264f254a 541 sendString("s = %u %f",a, speed);
FatCookies 4:4afa448c9cce 542 curr_cmd = 0;
FatCookies 29:b5b31256572b 543 right_motor_pid.desired_value=speed;
FatCookies 29:b5b31256572b 544 left_motor_pid.desired_value=speed;
FatCookies 4:4afa448c9cce 545 }
FatCookies 4:4afa448c9cce 546 break;
FatCookies 4:4afa448c9cce 547
FatCookies 4:4afa448c9cce 548 default:
FatCookies 13:4e77264f254a 549 // Unrecognised command
FatCookies 13:4e77264f254a 550 curr_cmd = 0;
FatCookies 4:4afa448c9cce 551 break;
FatCookies 4:4afa448c9cce 552 }
FatCookies 4:4afa448c9cce 553 }
FatCookies 4:4afa448c9cce 554
FatCookies 6:b0e160c51013 555 if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
lh14g13 18:0095a3a8f8e4 556 //Start car
FatCookies 4:4afa448c9cce 557 char cmd = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 558 if(cmd == 'D') {
FatCookies 17:6ae90788cc2b 559 ALIGN_SERVO;
FatCookies 21:0b69fada7c5f 560 right_motor_pid.desired_value=speed;
FatCookies 21:0b69fada7c5f 561 left_motor_pid.desired_value=speed;
FatCookies 4:4afa448c9cce 562 TFC_HBRIDGE_ENABLE;
FatCookies 29:b5b31256572b 563
FatCookies 21:0b69fada7c5f 564
FatCookies 12:da96e2f87465 565 servo_pid.integral = 0;
FatCookies 27:627d67e3b9b0 566 lapTimer.start();
lh14g13 18:0095a3a8f8e4 567 lapNo =0;
FatCookies 6:b0e160c51013 568
FatCookies 4:4afa448c9cce 569 } else if (cmd == 'C') {
FatCookies 4:4afa448c9cce 570 TFC_SetMotorPWM(0.0,0.0);
lh14g13 19:65f0b6febc23 571 right_motor_pid.desired_value=0;
FatCookies 21:0b69fada7c5f 572 right_motor_pid.measured_value = 0;
FatCookies 21:0b69fada7c5f 573 wR = 0;
FatCookies 21:0b69fada7c5f 574 prevR = 0;
FatCookies 21:0b69fada7c5f 575
lh14g13 19:65f0b6febc23 576 left_motor_pid.desired_value=0;
FatCookies 21:0b69fada7c5f 577 left_motor_pid.measured_value = 0;
FatCookies 21:0b69fada7c5f 578 wL = 0;
FatCookies 21:0b69fada7c5f 579 prevL = 0;
FatCookies 21:0b69fada7c5f 580
FatCookies 4:4afa448c9cce 581 TFC_HBRIDGE_DISABLE;
lh14g13 18:0095a3a8f8e4 582 endTest();
FatCookies 4:4afa448c9cce 583 } else if(cmd == 'A') {
FatCookies 4:4afa448c9cce 584 curr_cmd = 'A';
FatCookies 4:4afa448c9cce 585 } else if(cmd == 'F') {
FatCookies 4:4afa448c9cce 586 curr_cmd = 'F';
FatCookies 15:ccde02f96449 587 } else if(cmd == 'K') {
FatCookies 15:ccde02f96449 588 sendCam = ~sendCam;
FatCookies 4:4afa448c9cce 589 }
FatCookies 4:4afa448c9cce 590
FatCookies 4:4afa448c9cce 591 }
maximusismax 8:7c5e6b1e7aa5 592 }
lh14g13 18:0095a3a8f8e4 593
lh14g13 18:0095a3a8f8e4 594 float testSpeed(float speed)
lh14g13 18:0095a3a8f8e4 595 {
lh14g13 18:0095a3a8f8e4 596 // search: Speed Increase
lh14g13 18:0095a3a8f8e4 597 // every time the car sees the stop start the speed of the car will increase
lh14g13 18:0095a3a8f8e4 598 // this can occur on stop start trigger.
lh14g13 18:0095a3a8f8e4 599 // may need to send the speed back to the telemetry.
lh14g13 18:0095a3a8f8e4 600 if (speed>0.4)
lh14g13 18:0095a3a8f8e4 601 {
lh14g13 18:0095a3a8f8e4 602 speed+=0.05;
lh14g13 18:0095a3a8f8e4 603 }
lh14g13 18:0095a3a8f8e4 604
lh14g13 18:0095a3a8f8e4 605 else
lh14g13 18:0095a3a8f8e4 606 {
lh14g13 18:0095a3a8f8e4 607 speed+=0.1;
lh14g13 18:0095a3a8f8e4 608
lh14g13 18:0095a3a8f8e4 609 }
lh14g13 18:0095a3a8f8e4 610
lh14g13 18:0095a3a8f8e4 611
lh14g13 18:0095a3a8f8e4 612 sendString("s = %f", speed);
lh14g13 18:0095a3a8f8e4 613 return speed;
lh14g13 18:0095a3a8f8e4 614
lh14g13 18:0095a3a8f8e4 615 }
lh14g13 18:0095a3a8f8e4 616
lh14g13 18:0095a3a8f8e4 617
lh14g13 18:0095a3a8f8e4 618
lh14g13 18:0095a3a8f8e4 619 int lapTime()
lh14g13 19:65f0b6febc23 620 {
lh14g13 18:0095a3a8f8e4 621 // function which sends the lap time back to the telemetry.
FatCookies 27:627d67e3b9b0 622 float newTime= lapTimer.read();
lh14g13 18:0095a3a8f8e4 623 lapNo += 1;
lh14g13 18:0095a3a8f8e4 624 float lapTime= newTime-oldTime;
lh14g13 18:0095a3a8f8e4 625 float avgTime= newTime/lapNo;
lh14g13 18:0095a3a8f8e4 626
lh14g13 19:65f0b6febc23 627 sendString("For lap number: %d Lap Time: %f Avergae time: %f \n\r", lapNo,lapTime,avgTime);
lh14g13 18:0095a3a8f8e4 628
lh14g13 30:6c047af9f0cc 629
lh14g13 18:0095a3a8f8e4 630 return 0;
lh14g13 18:0095a3a8f8e4 631 }
lh14g13 18:0095a3a8f8e4 632
lh14g13 18:0095a3a8f8e4 633
lh14g13 18:0095a3a8f8e4 634 void endTest()
lh14g13 18:0095a3a8f8e4 635 {// 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 636
FatCookies 27:627d67e3b9b0 637 float time= lapTimer.read();
lh14g13 18:0095a3a8f8e4 638
lh14g13 19:65f0b6febc23 639 sendString("Laps done: %d Time elapsed: %f Average time: %f \n\r",lapNo, time,float (time/lapNo));
FatCookies 27:627d67e3b9b0 640 lapTimer.stop();
lh14g13 18:0095a3a8f8e4 641
lh14g13 18:0095a3a8f8e4 642
lh14g13 19:65f0b6febc23 643 }
lh14g13 18:0095a3a8f8e4 644
lh14g13 18:0095a3a8f8e4 645
lh14g13 18:0095a3a8f8e4 646
lh14g13 18:0095a3a8f8e4 647
lh14g13 18:0095a3a8f8e4 648
FatCookies 17:6ae90788cc2b 649 #endif