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@11:53de69b1840b, 2016-11-21 (annotated)

Committer:
FatCookies
Date:
Mon Nov 21 16:57:53 2016 +0000
Revision:
11:53de69b1840b
Parent:
10:1bd0224093e4
Child:
12:da96e2f87465
added hall sensor sending to jabba

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"
maximusismax 8:7c5e6b1e7aa5 16
maximusismax 8:7c5e6b1e7aa5 17 #define BAUD_RATE 57600
FatCookies 6:b0e160c51013 18 #define CAM_THRESHOLD 109
maximusismax 8:7c5e6b1e7aa5 19 #define CAM_DIFF 10
maximusismax 8:7c5e6b1e7aa5 20
FatCookies 10:1bd0224093e4 21 #define RIGHT_MOTOR_COMPENSATION_RATIO 1.1586276
FatCookies 10:1bd0224093e4 22
FatCookies 3:87a5122682fa 23 //Serial pc(USBTX,USBRX);
FatCookies 3:87a5122682fa 24 Serial pc(PTD3,PTD2);
FatCookies 4:4afa448c9cce 25 XBEE xb(&pc);
FatCookies 3:87a5122682fa 26
maximusismax 8:7c5e6b1e7aa5 27 //Woo global variables!
maximusismax 8:7c5e6b1e7aa5 28 bool onTrack;
FatCookies 3:87a5122682fa 29 char curr_line[128];
FatCookies 3:87a5122682fa 30 uint8_t curr_left;
FatCookies 3:87a5122682fa 31 uint8_t curr_right;
FatCookies 3:87a5122682fa 32 uint8_t right;
FatCookies 3:87a5122682fa 33 uint8_t left;
maximusismax 8:7c5e6b1e7aa5 34 int8_t leftChange;
maximusismax 8:7c5e6b1e7aa5 35 int8_t rightChange;
maximusismax 8:7c5e6b1e7aa5 36 int diff;
maximusismax 8:7c5e6b1e7aa5 37 int prev;
maximusismax 8:7c5e6b1e7aa5 38 int i = 0;
maximusismax 8:7c5e6b1e7aa5 39 float measuredValBuffer[5];
maximusismax 8:7c5e6b1e7aa5 40 uint8_t valBufferIndex;
maximusismax 8:7c5e6b1e7aa5 41
maximusismax 8:7c5e6b1e7aa5 42 //Some PID variables
maximusismax 8:7c5e6b1e7aa5 43 float Kp;
maximusismax 8:7c5e6b1e7aa5 44 float Ki;
maximusismax 8:7c5e6b1e7aa5 45 float Kd;
maximusismax 8:7c5e6b1e7aa5 46 float dt;
maximusismax 8:7c5e6b1e7aa5 47 float p_error;
maximusismax 8:7c5e6b1e7aa5 48 float pid_error;
maximusismax 8:7c5e6b1e7aa5 49 float integral;
maximusismax 8:7c5e6b1e7aa5 50 float measured_value, desired_value, derivative;
maximusismax 8:7c5e6b1e7aa5 51 float output;
FatCookies 3:87a5122682fa 52 Timer t;
FatCookies 4:4afa448c9cce 53
FatCookies 9:aa2ce38dec6b 54 //Speed Sensors variables
FatCookies 9:aa2ce38dec6b 55 InterruptIn leftHallSensor(D0);
FatCookies 9:aa2ce38dec6b 56 InterruptIn rightHallSensor(D2);
FatCookies 9:aa2ce38dec6b 57 Timer t1;
FatCookies 9:aa2ce38dec6b 58 Timer t2;
FatCookies 9:aa2ce38dec6b 59 volatile float Time_L,Time_R;
FatCookies 9:aa2ce38dec6b 60 float wL, wR;
FatCookies 9:aa2ce38dec6b 61 void GetTime_L();
FatCookies 9:aa2ce38dec6b 62 void GetTime_R();
FatCookies 9:aa2ce38dec6b 63 inline void initSpeedSensors();
FatCookies 9:aa2ce38dec6b 64
FatCookies 9:aa2ce38dec6b 65 // Current comms command
FatCookies 4:4afa448c9cce 66 char curr_cmd = 0;
FatCookies 4:4afa448c9cce 67
FatCookies 4:4afa448c9cce 68 float speed = 0.3;
FatCookies 6:b0e160c51013 69 int frame_counter = 0;
maximusismax 8:7c5e6b1e7aa5 70
maximusismax 8:7c5e6b1e7aa5 71 //Hacky start/stop signal detection
maximusismax 8:7c5e6b1e7aa5 72 int startstop = 0;
FatCookies 7:ad893fc41b95 73 bool seen = false;
maximusismax 8:7c5e6b1e7aa5 74
maximusismax 8:7c5e6b1e7aa5 75 void sendString(const char *format, ...);
maximusismax 8:7c5e6b1e7aa5 76 void initVariables();
maximusismax 8:7c5e6b1e7aa5 77 inline void handleComms();
maximusismax 8:7c5e6b1e7aa5 78 inline float findCentreValue();
maximusismax 8:7c5e6b1e7aa5 79 inline void PIDController();
maximusismax 8:7c5e6b1e7aa5 80 inline void sendImage();
maximusismax 8:7c5e6b1e7aa5 81
maximusismax 0:566127ca8048 82 int main() {
maximusismax 8:7c5e6b1e7aa5 83 //Set up TFC driver stuff
maximusismax 0:566127ca8048 84 TFC_Init();
FatCookies 3:87a5122682fa 85 TFC_InitServos(0.00052,0.00122,0.02);
maximusismax 8:7c5e6b1e7aa5 86 //Old things to make the car move at run-time
maximusismax 8:7c5e6b1e7aa5 87 //Should tie these to a button for the actual races
FatCookies 4:4afa448c9cce 88 //TFC_HBRIDGE_ENABLE;
FatCookies 4:4afa448c9cce 89 //TFC_SetMotorPWM(0.3,0.3);
maximusismax 2:4b6f6fc84793 90
maximusismax 8:7c5e6b1e7aa5 91 //Setup baud rate for serial link, do not change!
maximusismax 8:7c5e6b1e7aa5 92 pc.baud(BAUD_RATE);
maximusismax 0:566127ca8048 93
maximusismax 8:7c5e6b1e7aa5 94 //Initialise/reset PID variables
maximusismax 8:7c5e6b1e7aa5 95 initVariables();
FatCookies 9:aa2ce38dec6b 96 initSpeedSensors();
maximusismax 0:566127ca8048 97
maximusismax 0:566127ca8048 98 while(1) {
FatCookies 3:87a5122682fa 99
maximusismax 8:7c5e6b1e7aa5 100 handleComms();
maximusismax 8:7c5e6b1e7aa5 101
maximusismax 8:7c5e6b1e7aa5 102 //If we have an image ready
maximusismax 8:7c5e6b1e7aa5 103 if(TFC_LineScanImageReady>0) {
maximusismax 8:7c5e6b1e7aa5 104 measured_value = findCentreValue();
maximusismax 8:7c5e6b1e7aa5 105
maximusismax 8:7c5e6b1e7aa5 106 PIDController();
maximusismax 8:7c5e6b1e7aa5 107
maximusismax 8:7c5e6b1e7aa5 108 sendImage();
maximusismax 8:7c5e6b1e7aa5 109
maximusismax 8:7c5e6b1e7aa5 110 //Hacky way to detect the start/stop signal
maximusismax 8:7c5e6b1e7aa5 111 if(right - left < 60) {
maximusismax 8:7c5e6b1e7aa5 112 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 113 pc.printf("START STOP!! &d",startstop);
maximusismax 8:7c5e6b1e7aa5 114 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 115
maximusismax 8:7c5e6b1e7aa5 116 if(seen) {
maximusismax 8:7c5e6b1e7aa5 117 seen = false;
maximusismax 8:7c5e6b1e7aa5 118 } else {
maximusismax 8:7c5e6b1e7aa5 119 startstop++;
maximusismax 8:7c5e6b1e7aa5 120 seen = true;
maximusismax 8:7c5e6b1e7aa5 121 }
maximusismax 8:7c5e6b1e7aa5 122 //If we've done 5 laps, stop the car
maximusismax 8:7c5e6b1e7aa5 123 if(startstop >= 1) {
maximusismax 8:7c5e6b1e7aa5 124 TFC_SetMotorPWM(0.0,0.0);
maximusismax 8:7c5e6b1e7aa5 125 TFC_HBRIDGE_DISABLE;
maximusismax 8:7c5e6b1e7aa5 126 startstop = 0;
maximusismax 8:7c5e6b1e7aa5 127 }
maximusismax 8:7c5e6b1e7aa5 128 }
maximusismax 8:7c5e6b1e7aa5 129
maximusismax 8:7c5e6b1e7aa5 130 //Reset image ready flag
maximusismax 8:7c5e6b1e7aa5 131 TFC_LineScanImageReady=0;
maximusismax 8:7c5e6b1e7aa5 132 }
maximusismax 8:7c5e6b1e7aa5 133 }
maximusismax 8:7c5e6b1e7aa5 134 }
maximusismax 8:7c5e6b1e7aa5 135
maximusismax 8:7c5e6b1e7aa5 136 void sendString(const char *format, ...) {
maximusismax 8:7c5e6b1e7aa5 137 va_list arg;
maximusismax 8:7c5e6b1e7aa5 138
maximusismax 8:7c5e6b1e7aa5 139 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 140 va_start (arg, format);
maximusismax 8:7c5e6b1e7aa5 141 pc.vprintf(format,arg);
maximusismax 8:7c5e6b1e7aa5 142 va_end (arg);
maximusismax 8:7c5e6b1e7aa5 143 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 144 }
maximusismax 8:7c5e6b1e7aa5 145
maximusismax 8:7c5e6b1e7aa5 146 void initVariables() {
maximusismax 8:7c5e6b1e7aa5 147 //Tunable PID variables
maximusismax 8:7c5e6b1e7aa5 148 Kp = 125 / 25.0f;
maximusismax 8:7c5e6b1e7aa5 149 Ki = 12.0f / 25.0f;
maximusismax 8:7c5e6b1e7aa5 150 Kd = 0.0f;
maximusismax 8:7c5e6b1e7aa5 151 dt = 0;
maximusismax 8:7c5e6b1e7aa5 152 p_error = 0;
maximusismax 8:7c5e6b1e7aa5 153 pid_error = 0;
maximusismax 8:7c5e6b1e7aa5 154 integral = 0;
maximusismax 8:7c5e6b1e7aa5 155 measured_value = 0;
maximusismax 8:7c5e6b1e7aa5 156 desired_value = 0;
maximusismax 8:7c5e6b1e7aa5 157 derivative = 0;
maximusismax 8:7c5e6b1e7aa5 158
maximusismax 8:7c5e6b1e7aa5 159 valBufferIndex = 0;
maximusismax 8:7c5e6b1e7aa5 160 //Measured value is a float between -1.0 and 1.0 (from left to right)
maximusismax 8:7c5e6b1e7aa5 161 //Desired value is always 0.0 (as in, car is in the middle of the road)
maximusismax 8:7c5e6b1e7aa5 162 }
maximusismax 8:7c5e6b1e7aa5 163
maximusismax 8:7c5e6b1e7aa5 164 inline void sendImage() {
maximusismax 8:7c5e6b1e7aa5 165 //Only send 1/3 of camera frames to GUI program
maximusismax 8:7c5e6b1e7aa5 166 if((frame_counter % 3) == 0) {
maximusismax 8:7c5e6b1e7aa5 167 pc.putc('H');
maximusismax 8:7c5e6b1e7aa5 168 for(i = 0; i < 128; i++) {
maximusismax 8:7c5e6b1e7aa5 169 pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);
maximusismax 8:7c5e6b1e7aa5 170 }
FatCookies 9:aa2ce38dec6b 171
FatCookies 11:53de69b1840b 172
FatCookies 11:53de69b1840b 173 }
FatCookies 11:53de69b1840b 174 frame_counter++;
FatCookies 11:53de69b1840b 175
FatCookies 9:aa2ce38dec6b 176 wL=Get_Speed(Time_L);
FatCookies 9:aa2ce38dec6b 177 wR=Get_Speed(Time_R);
FatCookies 11:53de69b1840b 178
FatCookies 11:53de69b1840b 179 union {
FatCookies 11:53de69b1840b 180 float a;
FatCookies 11:53de69b1840b 181 unsigned char bytes[4];
FatCookies 11:53de69b1840b 182 } thing;
FatCookies 11:53de69b1840b 183
FatCookies 11:53de69b1840b 184 pc.putc('B');
FatCookies 11:53de69b1840b 185 thing.a = wL;
FatCookies 11:53de69b1840b 186 pc.putc(thing.bytes[0]);
FatCookies 11:53de69b1840b 187 pc.putc(thing.bytes[1]);
FatCookies 11:53de69b1840b 188 pc.putc(thing.bytes[2]);
FatCookies 11:53de69b1840b 189 pc.putc(thing.bytes[3]);
FatCookies 11:53de69b1840b 190 thing.a = wR;
FatCookies 11:53de69b1840b 191 pc.putc(thing.bytes[0]);
FatCookies 11:53de69b1840b 192 pc.putc(thing.bytes[1]);
FatCookies 11:53de69b1840b 193 pc.putc(thing.bytes[2]);
FatCookies 11:53de69b1840b 194 pc.putc(thing.bytes[3]);
maximusismax 8:7c5e6b1e7aa5 195 }
maximusismax 8:7c5e6b1e7aa5 196
maximusismax 8:7c5e6b1e7aa5 197 inline void handleComms() {
maximusismax 8:7c5e6b1e7aa5 198 if(curr_cmd != 0) {
FatCookies 4:4afa448c9cce 199 switch(curr_cmd) {
FatCookies 4:4afa448c9cce 200 case 'A':
FatCookies 4:4afa448c9cce 201 if(xb.cBuffer->available() >= 3) {
FatCookies 4:4afa448c9cce 202 char p = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 203 char i = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 204 char d = xb.cBuffer->read();
FatCookies 6:b0e160c51013 205 Kp = p/25.0f;
FatCookies 6:b0e160c51013 206 Ki = i/25.0f;
FatCookies 6:b0e160c51013 207 Kd = d/25.0f;
FatCookies 4:4afa448c9cce 208 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 209 pc.printf("pid change, Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", Kp, Ki, Kd, p, i, d);
FatCookies 6:b0e160c51013 210 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 211
FatCookies 4:4afa448c9cce 212 curr_cmd = 0;
FatCookies 4:4afa448c9cce 213 }
FatCookies 4:4afa448c9cce 214 break;
FatCookies 4:4afa448c9cce 215
FatCookies 4:4afa448c9cce 216 case 'F':
FatCookies 6:b0e160c51013 217 if(xb.cBuffer->available() >= 1) {
FatCookies 4:4afa448c9cce 218 char a = xb.cBuffer->read();
FatCookies 6:b0e160c51013 219 speed = a/256.0f;
FatCookies 7:ad893fc41b95 220 TFC_SetMotorPWM(speed,speed);
FatCookies 4:4afa448c9cce 221 pc.putc('E');
FatCookies 6:b0e160c51013 222 pc.printf("s = %u %f",a, speed);
FatCookies 6:b0e160c51013 223 pc.putc(0);
FatCookies 4:4afa448c9cce 224 curr_cmd = 0;
FatCookies 4:4afa448c9cce 225
FatCookies 4:4afa448c9cce 226 }
FatCookies 4:4afa448c9cce 227 break;
FatCookies 4:4afa448c9cce 228
FatCookies 4:4afa448c9cce 229 default:
FatCookies 4:4afa448c9cce 230 break;
FatCookies 4:4afa448c9cce 231 }
FatCookies 4:4afa448c9cce 232 }
FatCookies 4:4afa448c9cce 233
FatCookies 6:b0e160c51013 234 if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
FatCookies 4:4afa448c9cce 235 char cmd = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 236 if(cmd == 'D') {
FatCookies 4:4afa448c9cce 237 TFC_InitServos(0.00052,0.00122,0.02);
FatCookies 4:4afa448c9cce 238 TFC_HBRIDGE_ENABLE;
FatCookies 10:1bd0224093e4 239 TFC_SetMotorPWM(RIGHT_MOTOR_COMPENSATION_RATIO*speed,speed);
FatCookies 6:b0e160c51013 240 integral = 0;
FatCookies 6:b0e160c51013 241
FatCookies 4:4afa448c9cce 242 } else if (cmd == 'C') {
FatCookies 4:4afa448c9cce 243 TFC_SetMotorPWM(0.0,0.0);
FatCookies 4:4afa448c9cce 244 TFC_HBRIDGE_DISABLE;
FatCookies 4:4afa448c9cce 245 } else if(cmd == 'A') {
FatCookies 4:4afa448c9cce 246 curr_cmd = 'A';
FatCookies 4:4afa448c9cce 247 } else if(cmd == 'F') {
FatCookies 4:4afa448c9cce 248 curr_cmd = 'F';
FatCookies 4:4afa448c9cce 249 }
FatCookies 4:4afa448c9cce 250
FatCookies 4:4afa448c9cce 251 }
maximusismax 8:7c5e6b1e7aa5 252 }
maximusismax 8:7c5e6b1e7aa5 253 inline float findCentreValue() {
maximusismax 8:7c5e6b1e7aa5 254 float measuredValue;
maximusismax 8:7c5e6b1e7aa5 255
maximusismax 8:7c5e6b1e7aa5 256 diff = 0;
maximusismax 8:7c5e6b1e7aa5 257 prev = -1;
maximusismax 8:7c5e6b1e7aa5 258 leftChange = left;
maximusismax 8:7c5e6b1e7aa5 259 for(i = 63; i > 0; i--) {
maximusismax 8:7c5e6b1e7aa5 260 curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
maximusismax 8:7c5e6b1e7aa5 261 diff = prev - curr_left;
maximusismax 8:7c5e6b1e7aa5 262 if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
maximusismax 8:7c5e6b1e7aa5 263 left = i;
maximusismax 8:7c5e6b1e7aa5 264 break;
maximusismax 8:7c5e6b1e7aa5 265 }
maximusismax 8:7c5e6b1e7aa5 266 prev = curr_left;
maximusismax 8:7c5e6b1e7aa5 267 }
maximusismax 8:7c5e6b1e7aa5 268
maximusismax 8:7c5e6b1e7aa5 269 prev = -1;
maximusismax 8:7c5e6b1e7aa5 270 rightChange = right;
maximusismax 8:7c5e6b1e7aa5 271 for(i = 64; i < 128; i++) {
maximusismax 8:7c5e6b1e7aa5 272 curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
maximusismax 8:7c5e6b1e7aa5 273 int diff = prev - curr_right;
maximusismax 8:7c5e6b1e7aa5 274 if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
maximusismax 8:7c5e6b1e7aa5 275 right = i;
maximusismax 8:7c5e6b1e7aa5 276 break;
maximusismax 8:7c5e6b1e7aa5 277 }
maximusismax 8:7c5e6b1e7aa5 278 prev = curr_right;
maximusismax 8:7c5e6b1e7aa5 279 }
maximusismax 8:7c5e6b1e7aa5 280
maximusismax 8:7c5e6b1e7aa5 281 //Calculate how left/right we are
maximusismax 8:7c5e6b1e7aa5 282 measuredValue = (64 - ((left+right)/2))/64.f;
maximusismax 8:7c5e6b1e7aa5 283 measuredValBuffer[valBufferIndex % 5] = measuredValue;
maximusismax 8:7c5e6b1e7aa5 284 valBufferIndex++;
maximusismax 8:7c5e6b1e7aa5 285
maximusismax 8:7c5e6b1e7aa5 286 return measuredValue;
maximusismax 8:7c5e6b1e7aa5 287 }
FatCookies 3:87a5122682fa 288
maximusismax 8:7c5e6b1e7aa5 289 inline void PIDController() {
maximusismax 8:7c5e6b1e7aa5 290 //PID Stuff!
FatCookies 3:87a5122682fa 291 t.start();
maximusismax 8:7c5e6b1e7aa5 292 dt = t.read();
maximusismax 8:7c5e6b1e7aa5 293 pid_error = desired_value - measured_value;
maximusismax 8:7c5e6b1e7aa5 294 integral = integral + pid_error * dt;
maximusismax 8:7c5e6b1e7aa5 295 derivative = (pid_error - p_error) / dt;
maximusismax 8:7c5e6b1e7aa5 296 output = Kp * pid_error + Ki * integral + Kd * derivative;
maximusismax 8:7c5e6b1e7aa5 297 p_error = pid_error;
maximusismax 8:7c5e6b1e7aa5 298
maximusismax 8:7c5e6b1e7aa5 299 if(integral > 1.0f) {
FatCookies 6:b0e160c51013 300 integral = 1.0f;
FatCookies 6:b0e160c51013 301 }
FatCookies 6:b0e160c51013 302 if(integral < -1.0f) {
FatCookies 6:b0e160c51013 303 integral = -1.0f;
FatCookies 6:b0e160c51013 304 }
FatCookies 6:b0e160c51013 305
maximusismax 8:7c5e6b1e7aa5 306 if((-1.0 <= output) && (output <= 1.0))
FatCookies 6:b0e160c51013 307 {
maximusismax 8:7c5e6b1e7aa5 308 TFC_SetServo(0, output);
maximusismax 8:7c5e6b1e7aa5 309 }
maximusismax 8:7c5e6b1e7aa5 310 else //Unhappy PID state
maximusismax 8:7c5e6b1e7aa5 311 {
maximusismax 8:7c5e6b1e7aa5 312 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 313 pc.printf("pid unhappy");
maximusismax 8:7c5e6b1e7aa5 314 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 315 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 316 pc.printf("out = %f p_err = %f", output, p_error);
maximusismax 8:7c5e6b1e7aa5 317 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 318 TFC_InitServos(0.00052, 0.00122, 0.02);
maximusismax 8:7c5e6b1e7aa5 319 //output, pid_error, p_error, integral, derivative = 0;
maximusismax 8:7c5e6b1e7aa5 320
maximusismax 8:7c5e6b1e7aa5 321 if(output >= 1.0f) {
maximusismax 8:7c5e6b1e7aa5 322 TFC_SetServo(0, 0.9f);
maximusismax 8:7c5e6b1e7aa5 323 output = 1.0f;
maximusismax 8:7c5e6b1e7aa5 324 } else {
maximusismax 8:7c5e6b1e7aa5 325 TFC_SetServo(0, -0.9f);
maximusismax 8:7c5e6b1e7aa5 326 output = -1.0f;
maximusismax 8:7c5e6b1e7aa5 327 }
maximusismax 8:7c5e6b1e7aa5 328 }
FatCookies 6:b0e160c51013 329
FatCookies 3:87a5122682fa 330 t.stop();
FatCookies 3:87a5122682fa 331 t.reset();
FatCookies 3:87a5122682fa 332 t.start();
FatCookies 9:aa2ce38dec6b 333 }
FatCookies 9:aa2ce38dec6b 334
FatCookies 9:aa2ce38dec6b 335
FatCookies 9:aa2ce38dec6b 336
FatCookies 9:aa2ce38dec6b 337 inline void initSpeedSensors() {
FatCookies 9:aa2ce38dec6b 338 t1.start();
FatCookies 9:aa2ce38dec6b 339 t2.start();
FatCookies 9:aa2ce38dec6b 340
FatCookies 9:aa2ce38dec6b 341 //Left and Right are defined looking at the rear of the car, in the direction the camera points at.
FatCookies 9:aa2ce38dec6b 342 leftHallSensor.rise(&GetTime_L);
FatCookies 9:aa2ce38dec6b 343 rightHallSensor.rise(&GetTime_R);
FatCookies 9:aa2ce38dec6b 344 }
FatCookies 9:aa2ce38dec6b 345
FatCookies 9:aa2ce38dec6b 346 void GetTime_L(){
FatCookies 9:aa2ce38dec6b 347 Time_L=t1.read_us();
FatCookies 9:aa2ce38dec6b 348 t1.reset();
FatCookies 9:aa2ce38dec6b 349 }
FatCookies 9:aa2ce38dec6b 350
FatCookies 9:aa2ce38dec6b 351 void GetTime_R(){
FatCookies 9:aa2ce38dec6b 352 Time_R=t2.read_us();
FatCookies 9:aa2ce38dec6b 353 t2.reset();
maximusismax 0:566127ca8048 354 }