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@42:4395ede5781e, 2017-01-29 (annotated)

Committer:
lh14g13
Date:
Sun Jan 29 13:16:06 2017 +0000
Revision:
42:4395ede5781e
Parent:
41:d74878640739
Child:
44:1884ffec9a57
added some comments to motor parts of the code

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