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@22:973b95478663, 2016-12-12 (annotated)

Committer:
maximusismax
Date:
Mon Dec 12 09:50:39 2016 +0000
Revision:
22:973b95478663
Parent:
21:0b69fada7c5f
Child:
26:f3d770f3eda1
Testing out some new ways to do start/stop marker detection. Nothing working yet though...

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