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@43:649473c5a12b, 2017-03-20 (annotated)

Committer:
FatCookies
Date:
Mon Mar 20 11:55:20 2017 +0000
Revision:
43:649473c5a12b
Parent:
41:d74878640739
Child:
44:1884ffec9a57
running the car without comms fixes

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