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@52:af17b1a330f4, 2017-04-28 (annotated)

Committer:
lh14g13
Date:
Fri Apr 28 07:35:37 2017 +0000
Revision:
52:af17b1a330f4
Parent:
51:6a84fbc404c8
Child:
54:78854efeb1df
added more stuff to the change to torque system,; no longer does measure amount to WL just set torque to 0

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