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@39:7b28ee39185d, 2017-01-13 (annotated)

Committer:
lh14g13
Date:
Fri Jan 13 10:21:07 2017 +0000
Revision:
39:7b28ee39185d
Parent:
37:3baddde964db
Child:
40:10e8e80af7da
changed tune variable to ed_tune.; added in ability to tune the ED from the telemetry

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 32:6829684f8c4d 34 //testing timer for laptimes
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.
FatCookies 21:0b69fada7c5f 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.
oj3g13 34:3de7a19ccea3 100 initPID(&servo_pid, 0.f, 0.f, 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;
maximusismax 8:7c5e6b1e7aa5 124 }
maximusismax 8:7c5e6b1e7aa5 125
FatCookies 13:4e77264f254a 126 void initPID(pid_instance* pid, float Kp, float Ki, float Kd) {
FatCookies 17:6ae90788cc2b 127 pid->Kp = Kp;
FatCookies 13:4e77264f254a 128 pid->Ki = Ki;
FatCookies 13:4e77264f254a 129 pid->Kd = Kd;
FatCookies 13:4e77264f254a 130 pid->dt = 0;
FatCookies 13:4e77264f254a 131 pid->p_error = 0;
FatCookies 13:4e77264f254a 132 pid->pid_error = 0;
FatCookies 13:4e77264f254a 133 pid->integral = 0;
FatCookies 13:4e77264f254a 134 pid->measured_value = 0;
FatCookies 13:4e77264f254a 135 pid->desired_value = 0;
FatCookies 13:4e77264f254a 136 pid->derivative = 0;
FatCookies 13:4e77264f254a 137 }
FatCookies 13:4e77264f254a 138
FatCookies 29:b5b31256572b 139 bool leftSeen;
FatCookies 29:b5b31256572b 140 bool rightSeen;
maximusismax 31:1a06c9e1985e 141
maximusismax 31:1a06c9e1985e 142 //Function which calcuates how far to the left/right of the centre of the track the car is
maximusismax 31:1a06c9e1985e 143 //Takes data from either camera, and passes some variables by reference, which will hold
maximusismax 31:1a06c9e1985e 144 //the indices holding the locations of the left and right edges of the track
FatCookies 17:6ae90788cc2b 145 inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &l, uint8_t &r) {
FatCookies 17:6ae90788cc2b 146
maximusismax 31:1a06c9e1985e 147 diff = 0; //Holds difference in intensity between consecutive pixels
maximusismax 31:1a06c9e1985e 148 prev = -1; //Holds index of last inspected pixel
FatCookies 29:b5b31256572b 149
maximusismax 31:1a06c9e1985e 150 //Used for crossroads navigation, holds info on which edges of the track are observed
FatCookies 29:b5b31256572b 151 leftSeen = false;
FatCookies 29:b5b31256572b 152 rightSeen = false;
maximusismax 31:1a06c9e1985e 153
maximusismax 31:1a06c9e1985e 154 //Starting in the middle index, step left, inspecting the the edge of the track
oj3g13 34:3de7a19ccea3 155 for(i = 63; i > 2; i--) {
oj3g13 26:f3d770f3eda1 156 curr_left = (uint8_t)(cam_data[i] >> 4) & 0xFF;
FatCookies 17:6ae90788cc2b 157 diff = prev - curr_left;
maximusismax 31:1a06c9e1985e 158 //Check incorporates a combination of looking at the difference in intensities
maximusismax 31:1a06c9e1985e 159 //and whether the pixels intensity is less than a threshold, corresponding to the black edge
FatCookies 17:6ae90788cc2b 160 if(abs(diff) >= CAM_DIFF && curr_left <= CAM_THRESHOLD && prev != -1) {
maximusismax 31:1a06c9e1985e 161 l = i; //Record the index where the edge is observed
FatCookies 29:b5b31256572b 162 leftSeen = true;
FatCookies 17:6ae90788cc2b 163 break;
FatCookies 17:6ae90788cc2b 164 }
maximusismax 31:1a06c9e1985e 165 prev = curr_left; //Update previous value for the loop
FatCookies 17:6ae90788cc2b 166 }
maximusismax 8:7c5e6b1e7aa5 167
FatCookies 17:6ae90788cc2b 168 prev = -1;
maximusismax 31:1a06c9e1985e 169 //As before, start in the middle but this time step rightwards in the image
oj3g13 34:3de7a19ccea3 170 for(i = 64; i < 126; i++) {
oj3g13 26:f3d770f3eda1 171 curr_right = (uint8_t)(cam_data[i] >> 4) & 0xFF;
FatCookies 17:6ae90788cc2b 172 int diff = prev - curr_right;
FatCookies 17:6ae90788cc2b 173 if(abs(diff) >= CAM_DIFF && curr_right <= CAM_THRESHOLD && prev != -1) {
FatCookies 17:6ae90788cc2b 174 r = i;
FatCookies 29:b5b31256572b 175 rightSeen = true;
FatCookies 17:6ae90788cc2b 176 break;
FatCookies 17:6ae90788cc2b 177 }
FatCookies 17:6ae90788cc2b 178 prev = curr_right;
FatCookies 17:6ae90788cc2b 179 }
FatCookies 17:6ae90788cc2b 180
maximusismax 31:1a06c9e1985e 181 //If both edges are not visible, we are likely in a crossroads
FatCookies 29:b5b31256572b 182 if(!rightSeen && !leftSeen) {
FatCookies 29:b5b31256572b 183 sendString("lost edges");
maximusismax 31:1a06c9e1985e 184 ALIGN_SERVO; //Straighten wheels so we go straight through the crossroads
maximusismax 31:1a06c9e1985e 185 servo_pid.integral = 0;
oj3g13 34:3de7a19ccea3 186 l=0;r=128;
FatCookies 29:b5b31256572b 187 }
FatCookies 29:b5b31256572b 188
maximusismax 31:1a06c9e1985e 189 //Calculate how left/right from the centre line we are
FatCookies 17:6ae90788cc2b 190 return (64 - ((l+r)/2))/64.f;
FatCookies 13:4e77264f254a 191 }
FatCookies 13:4e77264f254a 192
maximusismax 31:1a06c9e1985e 193 //Unused function currently
maximusismax 31:1a06c9e1985e 194 //Was used to establish whether we are in a corner, by inspecting a buffer of
maximusismax 31:1a06c9e1985e 195 //centre line values
FatCookies 17:6ae90788cc2b 196 inline void handleCornering() {
FatCookies 12:da96e2f87465 197
maximusismax 31:1a06c9e1985e 198 //Get current value of how left/right of centre line we are on the track
FatCookies 17:6ae90788cc2b 199 float lookaheadMeasuredValue = findCentreValue(LOOKAHEAD_CAMERA, farLeft, farRight);
FatCookies 15:ccde02f96449 200
FatCookies 13:4e77264f254a 201 measuredValBuffer[frame_counter % 64] = servo_pid.measured_value;
FatCookies 13:4e77264f254a 202
FatCookies 13:4e77264f254a 203 int count = 0;
FatCookies 13:4e77264f254a 204 for(i = 0; i < 10; i++) {
maximusismax 31:1a06c9e1985e 205 //Step through the buffer, using modulus operator
FatCookies 13:4e77264f254a 206 float val = abs(measuredValBuffer[(frame_counter - i) % 64]);
maximusismax 31:1a06c9e1985e 207 if(val > 0.09) { //If the value exceeds a certain value (obtained experimentally), we are in a corner
FatCookies 13:4e77264f254a 208 count++;
FatCookies 13:4e77264f254a 209 }
FatCookies 13:4e77264f254a 210 }
FatCookies 12:da96e2f87465 211
FatCookies 29:b5b31256572b 212 /*if(!turning && abs(lookaheadMeasuredValue) > 0.11f){
FatCookies 15:ccde02f96449 213 TFC_SetMotorPWM(0.4,0.4);
FatCookies 15:ccde02f96449 214 }
FatCookies 29:b5b31256572b 215 */
FatCookies 15:ccde02f96449 216
FatCookies 29:b5b31256572b 217 if(false) {
lh14g13 18:0095a3a8f8e4 218
lh14g13 18:0095a3a8f8e4 219 //default
lh14g13 18:0095a3a8f8e4 220 //TFC_SetMotorPWM(0.4,0.4);
lh14g13 18:0095a3a8f8e4 221
lh14g13 19:65f0b6febc23 222 //dutyCycleCorner(speed,servo_pid.output);
lh14g13 18:0095a3a8f8e4 223
lh14g13 32:6829684f8c4d 224
lh14g13 18:0095a3a8f8e4 225 //dutyCycleCorner(float cornerspeed, servo_pid.output);
lh14g13 18:0095a3a8f8e4 226 //dutyCycleCorner(speed, servo_pid.output);
lh14g13 32:6829684f8c4d 227 // This activates the electronic differential so that it runs whilst cornering.
lh14g13 32:6829684f8c4d 228 // this changes the desired desired speed of each of the wheels according to the angle of the servo.
lh14g13 39:7b28ee39185d 229 sensorCorner(left_motor_pid.desired_value, right_motor_pid.desired_value , servo_pid.output, speed,ed_tune);
FatCookies 14:13085e161dd1 230 }
FatCookies 14:13085e161dd1 231
FatCookies 29:b5b31256572b 232 /*
FatCookies 13:4e77264f254a 233 if(abs(servo_pid.measured_value) > 0.11f){
FatCookies 15:ccde02f96449 234 if(!turning) {
FatCookies 13:4e77264f254a 235 turning = 1;
FatCookies 13:4e77264f254a 236 } else {
FatCookies 13:4e77264f254a 237 turning++;
FatCookies 13:4e77264f254a 238 }
FatCookies 13:4e77264f254a 239
FatCookies 13:4e77264f254a 240 } else {
FatCookies 13:4e77264f254a 241 if(turning) {
FatCookies 13:4e77264f254a 242 if(keepTurning == 0 || count > 6) {
FatCookies 13:4e77264f254a 243 keepTurning++;
FatCookies 13:4e77264f254a 244 } else {
FatCookies 15:ccde02f96449 245 //sendString("stop turning turned=%d",turning);
FatCookies 13:4e77264f254a 246 keepTurning = 0;
lh14g13 33:0fc789c09694 247 turning = 0;
lh14g13 33:0fc789c09694 248 left_motor_pid.desired_value=speed;
lh14g13 33:0fc789c09694 249 right_motot_pid.desired_value=speed;
FatCookies 13:4e77264f254a 250 TFC_SetMotorPWM(speed,speed);
FatCookies 13:4e77264f254a 251 }
FatCookies 13:4e77264f254a 252
FatCookies 13:4e77264f254a 253 }
FatCookies 13:4e77264f254a 254 }
FatCookies 29:b5b31256572b 255 */
FatCookies 13:4e77264f254a 256
maximusismax 8:7c5e6b1e7aa5 257 }
maximusismax 8:7c5e6b1e7aa5 258
maximusismax 31:1a06c9e1985e 259 //Unused function currently
maximusismax 31:1a06c9e1985e 260 //Was used to estimate whether the stop marker was seen
FatCookies 17:6ae90788cc2b 261 inline float getLineEntropy() {
FatCookies 17:6ae90788cc2b 262 float entropy = 0;
FatCookies 17:6ae90788cc2b 263 float last = (int8_t)(CLOSE_CAMERA[0] >> 4) & 0xFF;
FatCookies 17:6ae90788cc2b 264 for(int i = 1; i < 128; i++) {
FatCookies 17:6ae90788cc2b 265 entropy += abs(last - ((int8_t)(CLOSE_CAMERA[i] >> 4) & 0xFF));
FatCookies 17:6ae90788cc2b 266 }
FatCookies 17:6ae90788cc2b 267 return entropy;
FatCookies 17:6ae90788cc2b 268 }
FatCookies 17:6ae90788cc2b 269
FatCookies 17:6ae90788cc2b 270 void handlePID(pid_instance *pid) {
FatCookies 17:6ae90788cc2b 271 pid->dt = t.read();
FatCookies 17:6ae90788cc2b 272 pid->pid_error = pid->desired_value - pid->measured_value;
FatCookies 17:6ae90788cc2b 273 pid->integral = pid->integral + pid->pid_error * pid->dt;
FatCookies 17:6ae90788cc2b 274 pid->derivative = (pid->pid_error - pid->p_error) / pid->dt;
FatCookies 17:6ae90788cc2b 275 pid->output = pid->Kp * pid->pid_error + pid->Ki * pid->integral + pid->Kd * pid->derivative;
FatCookies 17:6ae90788cc2b 276 pid->p_error = pid->pid_error;
FatCookies 17:6ae90788cc2b 277
FatCookies 17:6ae90788cc2b 278 if(pid->integral > 1.0f) {
maximusismax 31:1a06c9e1985e 279 pid->integral = 1.0f;
FatCookies 17:6ae90788cc2b 280 }
FatCookies 27:627d67e3b9b0 281 if(pid->integral < -1.0f ) {
FatCookies 17:6ae90788cc2b 282 pid->integral = -1.0f;
FatCookies 17:6ae90788cc2b 283 }
FatCookies 17:6ae90788cc2b 284 }
FatCookies 17:6ae90788cc2b 285
lh14g13 19:65f0b6febc23 286
FatCookies 17:6ae90788cc2b 287 inline void PIDController() {
FatCookies 27:627d67e3b9b0 288 // update motor measurements
lh14g13 19:65f0b6febc23 289 // Read the angular velocity of both wheels
FatCookies 27:627d67e3b9b0 290
FatCookies 27:627d67e3b9b0 291 prevL = wL;
FatCookies 27:627d67e3b9b0 292 prevR = wR;
FatCookies 27:627d67e3b9b0 293
lh14g13 19:65f0b6febc23 294 wL=Get_Speed(Time_L);
lh14g13 19:65f0b6febc23 295 wR=Get_Speed(Time_R);
FatCookies 20:ed954836d028 296
FatCookies 20:ed954836d028 297 // Check if left wheel is slipping/giving an abnormal reading and ignore reading
maximusismax 31:1a06c9e1985e 298 if(wL - prevL < 1.2/0.025) { //<3 magic numbers: 48....?
FatCookies 20:ed954836d028 299 left_motor_pid.measured_value = wL;
FatCookies 20:ed954836d028 300 }
FatCookies 20:ed954836d028 301 if(wR - prevR < 1.2/0.025) {
FatCookies 20:ed954836d028 302 right_motor_pid.measured_value = wR;
FatCookies 20:ed954836d028 303 }
FatCookies 17:6ae90788cc2b 304
FatCookies 21:0b69fada7c5f 305
FatCookies 17:6ae90788cc2b 306 //PID Stuff!
FatCookies 17:6ae90788cc2b 307 t.start();
FatCookies 17:6ae90788cc2b 308 handlePID(&servo_pid);
oj3g13 35:e23354abf352 309 //enables the ED
lh14g13 39:7b28ee39185d 310 sensorCorner(left_motor_pid.desired_value,right_motor_pid.desired_value, servo_pid.output,speed,ed_tune);
FatCookies 17:6ae90788cc2b 311 handlePID(&left_motor_pid);
FatCookies 17:6ae90788cc2b 312 handlePID(&right_motor_pid);
FatCookies 17:6ae90788cc2b 313
FatCookies 17:6ae90788cc2b 314 if((-1.0 <= servo_pid.output) && (servo_pid.output <= 1.0))
FatCookies 17:6ae90788cc2b 315 {
FatCookies 17:6ae90788cc2b 316 TFC_SetServo(0, servo_pid.output);
FatCookies 17:6ae90788cc2b 317 }
FatCookies 17:6ae90788cc2b 318 else //Unhappy PID state
FatCookies 17:6ae90788cc2b 319 {
FatCookies 17:6ae90788cc2b 320 //sendString("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
maximusismax 31:1a06c9e1985e 321 //ALIGN_SERVO;
maximusismax 31:1a06c9e1985e 322 //Could cause the car to be travelling along one side of the track rather than in the middle
FatCookies 17:6ae90788cc2b 323 if(servo_pid.output >= 1.0f) {
FatCookies 17:6ae90788cc2b 324 TFC_SetServo(0, 0.9f);
FatCookies 17:6ae90788cc2b 325 servo_pid.output = 1.0f;
FatCookies 17:6ae90788cc2b 326 } else {
FatCookies 17:6ae90788cc2b 327 TFC_SetServo(0, -0.9f);
FatCookies 17:6ae90788cc2b 328 servo_pid.output = -1.0f;
FatCookies 17:6ae90788cc2b 329 }
FatCookies 17:6ae90788cc2b 330 }
FatCookies 17:6ae90788cc2b 331
FatCookies 17:6ae90788cc2b 332
FatCookies 20:ed954836d028 333 if(left_motor_pid.output > 1.0f) {
FatCookies 20:ed954836d028 334 left_motor_pid.output = 1.0f;
lh14g13 19:65f0b6febc23 335 }
FatCookies 29:b5b31256572b 336 if(left_motor_pid.output < 0.0f) {
FatCookies 27:627d67e3b9b0 337 left_motor_pid.output = 0.0f;
lh14g13 19:65f0b6febc23 338 }
lh14g13 19:65f0b6febc23 339
FatCookies 20:ed954836d028 340 if(right_motor_pid.output > 1.0f) {
FatCookies 20:ed954836d028 341 right_motor_pid.output = 1.0f;
FatCookies 20:ed954836d028 342 }
FatCookies 29:b5b31256572b 343 if(right_motor_pid.output < 0.0f) {
FatCookies 27:627d67e3b9b0 344 right_motor_pid.output = 0.0f;
FatCookies 20:ed954836d028 345 }
lh14g13 19:65f0b6febc23 346
FatCookies 20:ed954836d028 347 TFC_SetMotorPWM(left_motor_pid.output,right_motor_pid.output);
lh14g13 19:65f0b6febc23 348
FatCookies 21:0b69fada7c5f 349
FatCookies 17:6ae90788cc2b 350 t.stop();
FatCookies 17:6ae90788cc2b 351 t.reset();
FatCookies 17:6ae90788cc2b 352 t.start();
FatCookies 17:6ae90788cc2b 353 }
FatCookies 17:6ae90788cc2b 354
FatCookies 17:6ae90788cc2b 355 inline void handleStartStop() {
maximusismax 31:1a06c9e1985e 356
maximusismax 31:1a06c9e1985e 357 //Function to detect the NXP cup stop marker
maximusismax 22:973b95478663 358
maximusismax 31:1a06c9e1985e 359 int slower = 0; //Only send a string every few frames
maximusismax 31:1a06c9e1985e 360 int difference = 0; //Holds the difference between intensities of consecutive pixels
oj3g13 26:f3d770f3eda1 361 int lastPixel, currentPixel, transitionsSeen;
maximusismax 22:973b95478663 362 lastPixel = -1;
maximusismax 22:973b95478663 363 transitionsSeen = 0;
maximusismax 31:1a06c9e1985e 364 //Starting near the left edge, step right, counting transitions.
maximusismax 31:1a06c9e1985e 365 //If there are several (exact value varies, best established experimentally), it is the marker
maximusismax 22:973b95478663 366 for(int i = 30; i < 98; i++) {
oj3g13 26:f3d770f3eda1 367 currentPixel = (int)(CLOSE_CAMERA[i] >> 4) & 0xFF;
maximusismax 22:973b95478663 368 difference = lastPixel - currentPixel;
FatCookies 27:627d67e3b9b0 369 if(abs(difference) > 10 && lastPixel != -1){ //transition seen, increment counter
maximusismax 22:973b95478663 370 transitionsSeen++;
oj3g13 26:f3d770f3eda1 371 i+=5;
maximusismax 22:973b95478663 372 }
maximusismax 22:973b95478663 373 lastPixel = currentPixel;
maximusismax 22:973b95478663 374 }
maximusismax 31:1a06c9e1985e 375 //Was used to send an indication that the marker was seen, useful for debugging
oj3g13 26:f3d770f3eda1 376 //if (slower % 1000 == 0) {
oj3g13 26:f3d770f3eda1 377 //sendString("Transitions seen: %d", transitionsSeen);
oj3g13 26:f3d770f3eda1 378 //}
maximusismax 31:1a06c9e1985e 379 //slower++;
oj3g13 26:f3d770f3eda1 380 if(transitionsSeen >= 5) {
maximusismax 22:973b95478663 381 //Stop the car!
oj3g13 26:f3d770f3eda1 382 sendString("Start/stop seen");
oj3g13 26:f3d770f3eda1 383 TFC_SetMotorPWM(0.f,0.f);
oj3g13 26:f3d770f3eda1 384 TFC_HBRIDGE_DISABLE;
FatCookies 27:627d67e3b9b0 385 lapTime();
oj3g13 26:f3d770f3eda1 386 }
FatCookies 17:6ae90788cc2b 387 }
FatCookies 17:6ae90788cc2b 388
FatCookies 17:6ae90788cc2b 389
FatCookies 17:6ae90788cc2b 390 inline void initSpeedSensors() {
FatCookies 17:6ae90788cc2b 391 t1.start();
FatCookies 17:6ae90788cc2b 392 t2.start();
FatCookies 17:6ae90788cc2b 393
FatCookies 17:6ae90788cc2b 394 //Left and Right are defined looking at the rear of the car, in the direction the camera points at.
FatCookies 17:6ae90788cc2b 395 leftHallSensor.rise(&GetTime_L);
FatCookies 17:6ae90788cc2b 396 rightHallSensor.rise(&GetTime_R);
FatCookies 17:6ae90788cc2b 397 }
FatCookies 17:6ae90788cc2b 398
FatCookies 17:6ae90788cc2b 399 void GetTime_L(){
FatCookies 17:6ae90788cc2b 400 Time_L=t1.read_us();
FatCookies 17:6ae90788cc2b 401 t1.reset();
FatCookies 17:6ae90788cc2b 402 }
FatCookies 17:6ae90788cc2b 403
FatCookies 17:6ae90788cc2b 404 void GetTime_R(){
FatCookies 17:6ae90788cc2b 405 Time_R=t2.read_us();
FatCookies 17:6ae90788cc2b 406 t2.reset();
FatCookies 17:6ae90788cc2b 407 }
FatCookies 17:6ae90788cc2b 408
FatCookies 17:6ae90788cc2b 409 #if USE_COMMS
FatCookies 17:6ae90788cc2b 410 void sendBattery() {
FatCookies 17:6ae90788cc2b 411
FatCookies 17:6ae90788cc2b 412 if(frame_counter % 256 == 0) {
FatCookies 17:6ae90788cc2b 413 float level = TFC_ReadBatteryVoltage() * 6.25;
FatCookies 17:6ae90788cc2b 414 pc.putc('J');
FatCookies 28:613239f10ba4 415 byte_float_union._float = level;
FatCookies 27:627d67e3b9b0 416 pc.putc(byte_float_union.bytes[0]);
FatCookies 27:627d67e3b9b0 417 pc.putc(byte_float_union.bytes[1]);
FatCookies 27:627d67e3b9b0 418 pc.putc(byte_float_union.bytes[2]);
FatCookies 27:627d67e3b9b0 419 pc.putc(byte_float_union.bytes[3]);
FatCookies 17:6ae90788cc2b 420 }
FatCookies 17:6ae90788cc2b 421 }
FatCookies 17:6ae90788cc2b 422
FatCookies 17:6ae90788cc2b 423 void sendString(const char *format, ...) {
FatCookies 17:6ae90788cc2b 424 va_list arg;
FatCookies 17:6ae90788cc2b 425
FatCookies 17:6ae90788cc2b 426 pc.putc('E');
FatCookies 17:6ae90788cc2b 427 va_start (arg, format);
FatCookies 17:6ae90788cc2b 428 pc.vprintf(format,arg);
FatCookies 17:6ae90788cc2b 429 va_end (arg);
FatCookies 17:6ae90788cc2b 430 pc.putc(0);
FatCookies 17:6ae90788cc2b 431 }
FatCookies 17:6ae90788cc2b 432
maximusismax 8:7c5e6b1e7aa5 433 inline void sendImage() {
maximusismax 8:7c5e6b1e7aa5 434 //Only send 1/3 of camera frames to GUI program
maximusismax 8:7c5e6b1e7aa5 435 if((frame_counter % 3) == 0) {
maximusismax 8:7c5e6b1e7aa5 436 pc.putc('H');
FatCookies 15:ccde02f96449 437 if(sendCam == 0) {
FatCookies 15:ccde02f96449 438 for(i = 0; i < 128; i++) {
FatCookies 17:6ae90788cc2b 439 pc.putc((int8_t)(CLOSE_CAMERA[i] >> 4) & 0xFF);
FatCookies 15:ccde02f96449 440 }
FatCookies 15:ccde02f96449 441 } else {
FatCookies 15:ccde02f96449 442 for(i = 0; i < 128; i++) {
FatCookies 17:6ae90788cc2b 443 pc.putc((int8_t)(LOOKAHEAD_CAMERA[i] >> 4) & 0xFF);
FatCookies 15:ccde02f96449 444 }
FatCookies 15:ccde02f96449 445 }
FatCookies 13:4e77264f254a 446 sendBattery();
FatCookies 13:4e77264f254a 447 }
FatCookies 13:4e77264f254a 448
FatCookies 13:4e77264f254a 449 frame_counter++;
FatCookies 13:4e77264f254a 450 }
FatCookies 13:4e77264f254a 451
FatCookies 13:4e77264f254a 452 inline void sendSpeeds() {
FatCookies 17:6ae90788cc2b 453
lh14g13 19:65f0b6febc23 454 /*float en = getLineEntropy();
FatCookies 15:ccde02f96449 455
FatCookies 15:ccde02f96449 456 if(onTrack) {
FatCookies 15:ccde02f96449 457 if(en <= 14000) {
FatCookies 15:ccde02f96449 458 onTrack = false;
FatCookies 15:ccde02f96449 459 sendString("offfffffffffffff");
FatCookies 15:ccde02f96449 460 TFC_SetMotorPWM(0.0,0.0);
FatCookies 15:ccde02f96449 461 TFC_HBRIDGE_DISABLE;
FatCookies 15:ccde02f96449 462 }
FatCookies 15:ccde02f96449 463 } else {
FatCookies 15:ccde02f96449 464 if(en > 14000) {
FatCookies 15:ccde02f96449 465 onTrack = true;
FatCookies 15:ccde02f96449 466 sendString("ON TRACK");
FatCookies 15:ccde02f96449 467 }
lh14g13 19:65f0b6febc23 468 }*/
FatCookies 15:ccde02f96449 469
FatCookies 14:13085e161dd1 470
FatCookies 13:4e77264f254a 471 pc.putc('B');
FatCookies 28:613239f10ba4 472 byte_float_union._float = wL * WHEEL_RADIUS;//left_motor_pid.output; //
FatCookies 27:627d67e3b9b0 473 pc.putc(byte_float_union.bytes[0]);
FatCookies 27:627d67e3b9b0 474 pc.putc(byte_float_union.bytes[1]);
FatCookies 27:627d67e3b9b0 475 pc.putc(byte_float_union.bytes[2]);
FatCookies 27:627d67e3b9b0 476 pc.putc(byte_float_union.bytes[3]);
FatCookies 28:613239f10ba4 477 byte_float_union._float = wR * WHEEL_RADIUS; // right_motor_pid.output; //
FatCookies 27:627d67e3b9b0 478 pc.putc(byte_float_union.bytes[0]);
FatCookies 27:627d67e3b9b0 479 pc.putc(byte_float_union.bytes[1]);
FatCookies 27:627d67e3b9b0 480 pc.putc(byte_float_union.bytes[2]);
FatCookies 27:627d67e3b9b0 481 pc.putc(byte_float_union.bytes[3]);
FatCookies 27:627d67e3b9b0 482
maximusismax 8:7c5e6b1e7aa5 483 }
maximusismax 8:7c5e6b1e7aa5 484
FatCookies 13:4e77264f254a 485
maximusismax 8:7c5e6b1e7aa5 486 inline void handleComms() {
maximusismax 8:7c5e6b1e7aa5 487 if(curr_cmd != 0) {
FatCookies 4:4afa448c9cce 488 switch(curr_cmd) {
FatCookies 4:4afa448c9cce 489 case 'A':
FatCookies 20:ed954836d028 490 if(xb.cBuffer->available() >= 12) {
FatCookies 20:ed954836d028 491
FatCookies 27:627d67e3b9b0 492 byte_float_union.bytes[0] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 493 byte_float_union.bytes[1] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 494 byte_float_union.bytes[2] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 495 byte_float_union.bytes[3] = xb.cBuffer->read();
FatCookies 28:613239f10ba4 496 servo_pid.Kp = byte_float_union._float;
FatCookies 20:ed954836d028 497
FatCookies 27:627d67e3b9b0 498 byte_float_union.bytes[0] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 499 byte_float_union.bytes[1] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 500 byte_float_union.bytes[2] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 501 byte_float_union.bytes[3] = xb.cBuffer->read();
FatCookies 28:613239f10ba4 502 servo_pid.Ki = byte_float_union._float;
FatCookies 20:ed954836d028 503
FatCookies 27:627d67e3b9b0 504 byte_float_union.bytes[0] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 505 byte_float_union.bytes[1] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 506 byte_float_union.bytes[2] = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 507 byte_float_union.bytes[3] = xb.cBuffer->read();
FatCookies 28:613239f10ba4 508 servo_pid.Kd = byte_float_union._float;
FatCookies 20:ed954836d028 509
FatCookies 20:ed954836d028 510 sendString("pid= Kp: %f, Ki: %f, Kd: %f", servo_pid.Kp, servo_pid.Ki, servo_pid.Kd);
maximusismax 8:7c5e6b1e7aa5 511
FatCookies 4:4afa448c9cce 512 curr_cmd = 0;
FatCookies 4:4afa448c9cce 513 }
FatCookies 4:4afa448c9cce 514 break;
FatCookies 4:4afa448c9cce 515
FatCookies 4:4afa448c9cce 516 case 'F':
FatCookies 6:b0e160c51013 517 if(xb.cBuffer->available() >= 1) {
FatCookies 4:4afa448c9cce 518 char a = xb.cBuffer->read();
FatCookies 27:627d67e3b9b0 519 speed = a;
FatCookies 13:4e77264f254a 520 sendString("s = %u %f",a, speed);
FatCookies 4:4afa448c9cce 521 curr_cmd = 0;
FatCookies 29:b5b31256572b 522 right_motor_pid.desired_value=speed;
FatCookies 29:b5b31256572b 523 left_motor_pid.desired_value=speed;
FatCookies 4:4afa448c9cce 524 }
FatCookies 4:4afa448c9cce 525 break;
lh14g13 39:7b28ee39185d 526
lh14g13 39:7b28ee39185d 527 case 'L':
lh14g13 39:7b28ee39185d 528 if(xb.cBuffer->available() >= 4) {
lh14g13 39:7b28ee39185d 529 byte_float_union.bytes[0] = xb.cBuffer->read();
lh14g13 39:7b28ee39185d 530 byte_float_union.bytes[1] = xb.cBuffer->read();
lh14g13 39:7b28ee39185d 531 byte_float_union.bytes[2] = xb.cBuffer->read();
lh14g13 39:7b28ee39185d 532 byte_float_union.bytes[3] = xb.cBuffer->read();
lh14g13 39:7b28ee39185d 533 ed_tune = byte_float_union._float;
lh14g13 39:7b28ee39185d 534 curr_cmd = 0;
lh14g13 39:7b28ee39185d 535 }
lh14g13 39:7b28ee39185d 536 break;
FatCookies 4:4afa448c9cce 537
FatCookies 4:4afa448c9cce 538 default:
FatCookies 13:4e77264f254a 539 // Unrecognised command
FatCookies 13:4e77264f254a 540 curr_cmd = 0;
FatCookies 4:4afa448c9cce 541 break;
FatCookies 4:4afa448c9cce 542 }
FatCookies 4:4afa448c9cce 543 }
FatCookies 4:4afa448c9cce 544
FatCookies 6:b0e160c51013 545 if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
lh14g13 18:0095a3a8f8e4 546 //Start car
FatCookies 4:4afa448c9cce 547 char cmd = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 548 if(cmd == 'D') {
FatCookies 17:6ae90788cc2b 549 ALIGN_SERVO;
FatCookies 21:0b69fada7c5f 550 right_motor_pid.desired_value=speed;
FatCookies 21:0b69fada7c5f 551 left_motor_pid.desired_value=speed;
FatCookies 4:4afa448c9cce 552 TFC_HBRIDGE_ENABLE;
FatCookies 29:b5b31256572b 553
FatCookies 21:0b69fada7c5f 554
FatCookies 12:da96e2f87465 555 servo_pid.integral = 0;
FatCookies 27:627d67e3b9b0 556 lapTimer.start();
lh14g13 18:0095a3a8f8e4 557 lapNo =0;
FatCookies 6:b0e160c51013 558
FatCookies 4:4afa448c9cce 559 } else if (cmd == 'C') {
FatCookies 4:4afa448c9cce 560 TFC_SetMotorPWM(0.0,0.0);
lh14g13 19:65f0b6febc23 561 right_motor_pid.desired_value=0;
FatCookies 21:0b69fada7c5f 562 right_motor_pid.measured_value = 0;
FatCookies 21:0b69fada7c5f 563 wR = 0;
FatCookies 21:0b69fada7c5f 564 prevR = 0;
FatCookies 21:0b69fada7c5f 565
lh14g13 19:65f0b6febc23 566 left_motor_pid.desired_value=0;
FatCookies 21:0b69fada7c5f 567 left_motor_pid.measured_value = 0;
FatCookies 21:0b69fada7c5f 568 wL = 0;
FatCookies 21:0b69fada7c5f 569 prevL = 0;
FatCookies 21:0b69fada7c5f 570
FatCookies 4:4afa448c9cce 571 TFC_HBRIDGE_DISABLE;
lh14g13 18:0095a3a8f8e4 572 endTest();
FatCookies 4:4afa448c9cce 573 } else if(cmd == 'A') {
FatCookies 4:4afa448c9cce 574 curr_cmd = 'A';
FatCookies 4:4afa448c9cce 575 } else if(cmd == 'F') {
FatCookies 4:4afa448c9cce 576 curr_cmd = 'F';
FatCookies 15:ccde02f96449 577 } else if(cmd == 'K') {
FatCookies 15:ccde02f96449 578 sendCam = ~sendCam;
lh14g13 39:7b28ee39185d 579 } else if(cmd == 'L') {
lh14g13 39:7b28ee39185d 580 curr_cmd = 'L'
FatCookies 4:4afa448c9cce 581 }
FatCookies 4:4afa448c9cce 582
FatCookies 4:4afa448c9cce 583 }
maximusismax 8:7c5e6b1e7aa5 584 }
lh14g13 18:0095a3a8f8e4 585
lh14g13 18:0095a3a8f8e4 586 float testSpeed(float speed)
lh14g13 18:0095a3a8f8e4 587 {
lh14g13 18:0095a3a8f8e4 588 // search: Speed Increase
lh14g13 18:0095a3a8f8e4 589 // every time the car sees the stop start the speed of the car will increase
lh14g13 18:0095a3a8f8e4 590 // this can occur on stop start trigger.
lh14g13 18:0095a3a8f8e4 591 // may need to send the speed back to the telemetry.
lh14g13 18:0095a3a8f8e4 592 if (speed>0.4)
lh14g13 18:0095a3a8f8e4 593 {
lh14g13 18:0095a3a8f8e4 594 speed+=0.05;
lh14g13 18:0095a3a8f8e4 595 }
lh14g13 18:0095a3a8f8e4 596
lh14g13 18:0095a3a8f8e4 597 else
lh14g13 18:0095a3a8f8e4 598 {
lh14g13 18:0095a3a8f8e4 599 speed+=0.1;
lh14g13 18:0095a3a8f8e4 600
lh14g13 18:0095a3a8f8e4 601 }
lh14g13 18:0095a3a8f8e4 602
lh14g13 18:0095a3a8f8e4 603
lh14g13 18:0095a3a8f8e4 604 sendString("s = %f", speed);
lh14g13 18:0095a3a8f8e4 605 return speed;
lh14g13 18:0095a3a8f8e4 606
lh14g13 18:0095a3a8f8e4 607 }
lh14g13 18:0095a3a8f8e4 608
lh14g13 18:0095a3a8f8e4 609
lh14g13 18:0095a3a8f8e4 610
lh14g13 18:0095a3a8f8e4 611 int lapTime()
lh14g13 19:65f0b6febc23 612 {
lh14g13 18:0095a3a8f8e4 613 // function which sends the lap time back to the telemetry.
lh14g13 32:6829684f8c4d 614 //reads the timer
FatCookies 27:627d67e3b9b0 615 float newTime= lapTimer.read();
lh14g13 18:0095a3a8f8e4 616 lapNo += 1;
lh14g13 18:0095a3a8f8e4 617 float lapTime= newTime-oldTime;
lh14g13 18:0095a3a8f8e4 618 float avgTime= newTime/lapNo;
lh14g13 32:6829684f8c4d 619 // this calulates the average lap time and the lap time.
lh14g13 32:6829684f8c4d 620 //then sends the information back to the UI.
lh14g13 19:65f0b6febc23 621 sendString("For lap number: %d Lap Time: %f Avergae time: %f \n\r", lapNo,lapTime,avgTime);
lh14g13 18:0095a3a8f8e4 622
lh14g13 32:6829684f8c4d 623
lh14g13 18:0095a3a8f8e4 624 return 0;
lh14g13 18:0095a3a8f8e4 625 }
lh14g13 18:0095a3a8f8e4 626
lh14g13 18:0095a3a8f8e4 627
lh14g13 18:0095a3a8f8e4 628 void endTest()
lh14g13 18:0095a3a8f8e4 629 {// 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 630
FatCookies 27:627d67e3b9b0 631 float time= lapTimer.read();
lh14g13 18:0095a3a8f8e4 632
lh14g13 19:65f0b6febc23 633 sendString("Laps done: %d Time elapsed: %f Average time: %f \n\r",lapNo, time,float (time/lapNo));
FatCookies 27:627d67e3b9b0 634 lapTimer.stop();
lh14g13 18:0095a3a8f8e4 635
lh14g13 18:0095a3a8f8e4 636
lh14g13 19:65f0b6febc23 637 }
lh14g13 33:0fc789c09694 638 //motor controll specific(newfunction)
lh14g13 18:0095a3a8f8e4 639
lh14g13 18:0095a3a8f8e4 640
lh14g13 18:0095a3a8f8e4 641
lh14g13 18:0095a3a8f8e4 642
FatCookies 17:6ae90788cc2b 643 #endif