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@16:81cdffd8c5d5, 2016-12-02 (annotated)

Committer:
FatCookies
Date:
Fri Dec 02 11:06:17 2016 +0000
Revision:
16:81cdffd8c5d5
Parent:
15:ccde02f96449
Child:
17:6ae90788cc2b
fixed camera "state" variables for two cameras

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
maximusismax 8:7c5e6b1e7aa5 19 #define BAUD_RATE 57600
FatCookies 6:b0e160c51013 20 #define CAM_THRESHOLD 109
maximusismax 8:7c5e6b1e7aa5 21 #define CAM_DIFF 10
FatCookies 14:13085e161dd1 22 #define WHEEL_RADIUS 0.025f
maximusismax 8:7c5e6b1e7aa5 23
FatCookies 10:1bd0224093e4 24 #define RIGHT_MOTOR_COMPENSATION_RATIO 1.1586276
FatCookies 10:1bd0224093e4 25
FatCookies 3:87a5122682fa 26 Serial pc(PTD3,PTD2);
FatCookies 4:4afa448c9cce 27 XBEE xb(&pc);
FatCookies 3:87a5122682fa 28
maximusismax 8:7c5e6b1e7aa5 29 //Woo global variables!
maximusismax 8:7c5e6b1e7aa5 30 bool onTrack;
FatCookies 3:87a5122682fa 31 char curr_line[128];
FatCookies 3:87a5122682fa 32 uint8_t curr_left;
FatCookies 3:87a5122682fa 33 uint8_t curr_right;
FatCookies 3:87a5122682fa 34 uint8_t right;
FatCookies 3:87a5122682fa 35 uint8_t left;
FatCookies 16:81cdffd8c5d5 36 uint8_t farRight;
FatCookies 16:81cdffd8c5d5 37 uint8_t farLeft;
maximusismax 8:7c5e6b1e7aa5 38 int diff;
maximusismax 8:7c5e6b1e7aa5 39 int prev;
maximusismax 8:7c5e6b1e7aa5 40 int i = 0;
FatCookies 13:4e77264f254a 41 float measuredValBuffer[64];
maximusismax 8:7c5e6b1e7aa5 42 uint8_t valBufferIndex;
maximusismax 8:7c5e6b1e7aa5 43
FatCookies 15:ccde02f96449 44 //Communication Variables
FatCookies 15:ccde02f96449 45 uint8_t sendCam = 0;
FatCookies 16:81cdffd8c5d5 46 int frame_counter = 0;
FatCookies 16:81cdffd8c5d5 47 char curr_cmd = 0; // Current comms command
FatCookies 15:ccde02f96449 48
maximusismax 8:7c5e6b1e7aa5 49 //Some PID variables
FatCookies 12:da96e2f87465 50 pid_instance servo_pid;
FatCookies 13:4e77264f254a 51 pid_instance right_motor_pid;
FatCookies 13:4e77264f254a 52 pid_instance left_motor_pid;
FatCookies 3:87a5122682fa 53 Timer t;
FatCookies 4:4afa448c9cce 54
FatCookies 12:da96e2f87465 55
FatCookies 9:aa2ce38dec6b 56 //Speed Sensors variables
FatCookies 9:aa2ce38dec6b 57 InterruptIn leftHallSensor(D0);
FatCookies 9:aa2ce38dec6b 58 InterruptIn rightHallSensor(D2);
FatCookies 9:aa2ce38dec6b 59 Timer t1;
FatCookies 9:aa2ce38dec6b 60 Timer t2;
FatCookies 9:aa2ce38dec6b 61 volatile float Time_L,Time_R;
FatCookies 9:aa2ce38dec6b 62 float wL, wR;
FatCookies 9:aa2ce38dec6b 63 void GetTime_L();
FatCookies 9:aa2ce38dec6b 64 void GetTime_R();
FatCookies 9:aa2ce38dec6b 65 inline void initSpeedSensors();
FatCookies 9:aa2ce38dec6b 66
FatCookies 16:81cdffd8c5d5 67
FatCookies 4:4afa448c9cce 68
FatCookies 4:4afa448c9cce 69 float speed = 0.3;
FatCookies 16:81cdffd8c5d5 70
maximusismax 8:7c5e6b1e7aa5 71
maximusismax 8:7c5e6b1e7aa5 72 //Hacky start/stop signal detection
maximusismax 8:7c5e6b1e7aa5 73 int startstop = 0;
FatCookies 7:ad893fc41b95 74 bool seen = false;
maximusismax 8:7c5e6b1e7aa5 75
maximusismax 8:7c5e6b1e7aa5 76 void sendString(const char *format, ...);
maximusismax 8:7c5e6b1e7aa5 77 void initVariables();
maximusismax 8:7c5e6b1e7aa5 78 inline void handleComms();
FatCookies 16:81cdffd8c5d5 79 inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &left, uint8_t &right);
maximusismax 8:7c5e6b1e7aa5 80 inline void PIDController();
maximusismax 8:7c5e6b1e7aa5 81 inline void sendImage();
FatCookies 13:4e77264f254a 82 inline void sendSpeeds();
FatCookies 13:4e77264f254a 83 void handlePID(pid_instance *pid);
FatCookies 13:4e77264f254a 84 void initPID(pid_instance* pid);
FatCookies 13:4e77264f254a 85 inline void handleStartStop();
FatCookies 13:4e77264f254a 86 inline void recordMeasuredVal();
FatCookies 13:4e77264f254a 87 void sendBattery();
FatCookies 15:ccde02f96449 88 inline float getLineEntropy();
FatCookies 15:ccde02f96449 89
maximusismax 8:7c5e6b1e7aa5 90
maximusismax 0:566127ca8048 91 int main() {
maximusismax 8:7c5e6b1e7aa5 92 //Set up TFC driver stuff
maximusismax 0:566127ca8048 93 TFC_Init();
FatCookies 3:87a5122682fa 94 TFC_InitServos(0.00052,0.00122,0.02);
FatCookies 13:4e77264f254a 95
maximusismax 8:7c5e6b1e7aa5 96 //Setup baud rate for serial link, do not change!
maximusismax 8:7c5e6b1e7aa5 97 pc.baud(BAUD_RATE);
maximusismax 0:566127ca8048 98
maximusismax 8:7c5e6b1e7aa5 99 //Initialise/reset PID variables
maximusismax 8:7c5e6b1e7aa5 100 initVariables();
FatCookies 9:aa2ce38dec6b 101 initSpeedSensors();
FatCookies 12:da96e2f87465 102
FatCookies 13:4e77264f254a 103
maximusismax 0:566127ca8048 104 while(1) {
FatCookies 3:87a5122682fa 105
maximusismax 8:7c5e6b1e7aa5 106 handleComms();
maximusismax 8:7c5e6b1e7aa5 107
maximusismax 8:7c5e6b1e7aa5 108 //If we have an image ready
FatCookies 13:4e77264f254a 109 if(TFC_LineScanImageReady>0) {
FatCookies 13:4e77264f254a 110 /* Find the bounds of the track and calculate how close we are to
FatCookies 13:4e77264f254a 111 * the centre */
FatCookies 16:81cdffd8c5d5 112 servo_pid.measured_value = findCentreValue(TFC_LineScanImage0, left, right);
FatCookies 13:4e77264f254a 113 recordMeasuredVal();
maximusismax 8:7c5e6b1e7aa5 114
FatCookies 13:4e77264f254a 115 // Read the angular velocity of both wheels
FatCookies 13:4e77264f254a 116 wL=Get_Speed(Time_L);
FatCookies 13:4e77264f254a 117 wR=Get_Speed(Time_R);
FatCookies 13:4e77264f254a 118
FatCookies 13:4e77264f254a 119 // Run the PID controllers and adjust steering/motor accordingly
maximusismax 8:7c5e6b1e7aa5 120 PIDController();
maximusismax 8:7c5e6b1e7aa5 121
FatCookies 13:4e77264f254a 122 // Send the line scan image over serial
maximusismax 8:7c5e6b1e7aa5 123 sendImage();
maximusismax 8:7c5e6b1e7aa5 124
FatCookies 13:4e77264f254a 125 // Send the wheel speeds over serial
FatCookies 13:4e77264f254a 126 sendSpeeds();
FatCookies 13:4e77264f254a 127
FatCookies 13:4e77264f254a 128 // Check if car is at the stop line
FatCookies 13:4e77264f254a 129 //handleStartStop();
maximusismax 8:7c5e6b1e7aa5 130
FatCookies 15:ccde02f96449 131
maximusismax 8:7c5e6b1e7aa5 132 //Reset image ready flag
maximusismax 8:7c5e6b1e7aa5 133 TFC_LineScanImageReady=0;
maximusismax 8:7c5e6b1e7aa5 134 }
maximusismax 8:7c5e6b1e7aa5 135 }
maximusismax 8:7c5e6b1e7aa5 136 }
maximusismax 8:7c5e6b1e7aa5 137
FatCookies 13:4e77264f254a 138 void sendBattery() {
FatCookies 13:4e77264f254a 139
FatCookies 13:4e77264f254a 140 if(frame_counter % 256 == 0) {
FatCookies 13:4e77264f254a 141 float level = TFC_ReadBatteryVoltage() * 6.25;
FatCookies 13:4e77264f254a 142 union {
FatCookies 13:4e77264f254a 143 float a;
FatCookies 13:4e77264f254a 144 unsigned char bytes[4];
FatCookies 13:4e77264f254a 145 } thing;
FatCookies 13:4e77264f254a 146
FatCookies 13:4e77264f254a 147 pc.putc('J');
FatCookies 13:4e77264f254a 148 thing.a = level;
FatCookies 13:4e77264f254a 149 pc.putc(thing.bytes[0]);
FatCookies 13:4e77264f254a 150 pc.putc(thing.bytes[1]);
FatCookies 13:4e77264f254a 151 pc.putc(thing.bytes[2]);
FatCookies 13:4e77264f254a 152 pc.putc(thing.bytes[3]);
FatCookies 13:4e77264f254a 153 }
FatCookies 13:4e77264f254a 154 }
FatCookies 13:4e77264f254a 155
maximusismax 8:7c5e6b1e7aa5 156 void sendString(const char *format, ...) {
maximusismax 8:7c5e6b1e7aa5 157 va_list arg;
maximusismax 8:7c5e6b1e7aa5 158
maximusismax 8:7c5e6b1e7aa5 159 pc.putc('E');
maximusismax 8:7c5e6b1e7aa5 160 va_start (arg, format);
maximusismax 8:7c5e6b1e7aa5 161 pc.vprintf(format,arg);
maximusismax 8:7c5e6b1e7aa5 162 va_end (arg);
maximusismax 8:7c5e6b1e7aa5 163 pc.putc(0);
maximusismax 8:7c5e6b1e7aa5 164 }
maximusismax 8:7c5e6b1e7aa5 165
FatCookies 13:4e77264f254a 166 void initPID(pid_instance* pid, float Kp, float Ki, float Kd) {
FatCookies 13:4e77264f254a 167 pid->Kp = Kd;
FatCookies 13:4e77264f254a 168 pid->Ki = Ki;
FatCookies 13:4e77264f254a 169 pid->Kd = Kd;
FatCookies 13:4e77264f254a 170 pid->dt = 0;
FatCookies 13:4e77264f254a 171 pid->p_error = 0;
FatCookies 13:4e77264f254a 172 pid->pid_error = 0;
FatCookies 13:4e77264f254a 173 pid->integral = 0;
FatCookies 13:4e77264f254a 174 pid->measured_value = 0;
FatCookies 13:4e77264f254a 175 pid->desired_value = 0;
FatCookies 13:4e77264f254a 176 pid->derivative = 0;
FatCookies 13:4e77264f254a 177 }
FatCookies 13:4e77264f254a 178
maximusismax 8:7c5e6b1e7aa5 179 void initVariables() {
FatCookies 13:4e77264f254a 180 // Initialise three PID controllers for the servo and each wheel.
FatCookies 13:4e77264f254a 181 initPID(&servo_pid, 2.2f, 0.6f, 0.f);
FatCookies 13:4e77264f254a 182 initPID(&left_motor_pid, 1.0f, 0.f, 0.f);
FatCookies 13:4e77264f254a 183 initPID(&right_motor_pid, 1.0f, 0.f, 0.f);
maximusismax 8:7c5e6b1e7aa5 184
maximusismax 8:7c5e6b1e7aa5 185 valBufferIndex = 0;
FatCookies 13:4e77264f254a 186 }
FatCookies 13:4e77264f254a 187
FatCookies 13:4e77264f254a 188 int turning = 0;
FatCookies 13:4e77264f254a 189 int keepTurning = 0;
FatCookies 13:4e77264f254a 190 bool slow = false;
FatCookies 13:4e77264f254a 191 inline void recordMeasuredVal() {
FatCookies 12:da96e2f87465 192
FatCookies 16:81cdffd8c5d5 193 float aheadForward = findCentreValue(TFC_LineScanImage1, farLeft, farRight);
FatCookies 15:ccde02f96449 194
FatCookies 13:4e77264f254a 195 measuredValBuffer[frame_counter % 64] = servo_pid.measured_value;
FatCookies 13:4e77264f254a 196
FatCookies 13:4e77264f254a 197 int count = 0;
FatCookies 13:4e77264f254a 198 for(i = 0; i < 10; i++) {
FatCookies 13:4e77264f254a 199 float val = abs(measuredValBuffer[(frame_counter - i) % 64]);
FatCookies 13:4e77264f254a 200 if(val > 0.09) {
FatCookies 13:4e77264f254a 201 count++;
FatCookies 13:4e77264f254a 202 }
FatCookies 13:4e77264f254a 203 }
FatCookies 12:da96e2f87465 204
FatCookies 15:ccde02f96449 205 if(!turning && abs(aheadForward) > 0.11f){
FatCookies 15:ccde02f96449 206 TFC_SetMotorPWM(0.4,0.4);
FatCookies 15:ccde02f96449 207 }
FatCookies 15:ccde02f96449 208
FatCookies 14:13085e161dd1 209 if(turning) {
FatCookies 15:ccde02f96449 210 dutyCycleCorner(0.4,servo_pid.output);
FatCookies 14:13085e161dd1 211 //sensorCorner(left_motor_pid.desired_value, right_motor_pid.desired_value , servo_pid.output, 50);
FatCookies 14:13085e161dd1 212 }
FatCookies 14:13085e161dd1 213
FatCookies 13:4e77264f254a 214 if(abs(servo_pid.measured_value) > 0.11f){
FatCookies 15:ccde02f96449 215 if(!turning) {
FatCookies 13:4e77264f254a 216 turning = 1;
FatCookies 13:4e77264f254a 217 } else {
FatCookies 13:4e77264f254a 218 turning++;
FatCookies 13:4e77264f254a 219 }
FatCookies 13:4e77264f254a 220
FatCookies 13:4e77264f254a 221 } else {
FatCookies 13:4e77264f254a 222 if(turning) {
FatCookies 13:4e77264f254a 223 if(keepTurning == 0 || count > 6) {
FatCookies 13:4e77264f254a 224 keepTurning++;
FatCookies 13:4e77264f254a 225 } else {
FatCookies 15:ccde02f96449 226 //sendString("stop turning turned=%d",turning);
FatCookies 13:4e77264f254a 227 keepTurning = 0;
FatCookies 13:4e77264f254a 228 turning = 0;
FatCookies 13:4e77264f254a 229 TFC_SetMotorPWM(speed,speed);
FatCookies 13:4e77264f254a 230 }
FatCookies 13:4e77264f254a 231
FatCookies 13:4e77264f254a 232 }
FatCookies 13:4e77264f254a 233 }
FatCookies 13:4e77264f254a 234
maximusismax 8:7c5e6b1e7aa5 235 }
maximusismax 8:7c5e6b1e7aa5 236
maximusismax 8:7c5e6b1e7aa5 237 inline void sendImage() {
maximusismax 8:7c5e6b1e7aa5 238 //Only send 1/3 of camera frames to GUI program
maximusismax 8:7c5e6b1e7aa5 239 if((frame_counter % 3) == 0) {
maximusismax 8:7c5e6b1e7aa5 240 pc.putc('H');
FatCookies 15:ccde02f96449 241 if(sendCam == 0) {
FatCookies 15:ccde02f96449 242 for(i = 0; i < 128; i++) {
FatCookies 15:ccde02f96449 243 pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);
FatCookies 15:ccde02f96449 244 }
FatCookies 15:ccde02f96449 245 } else {
FatCookies 15:ccde02f96449 246 for(i = 0; i < 128; i++) {
FatCookies 15:ccde02f96449 247 pc.putc((int8_t)(TFC_LineScanImage1[i] >> 4) & 0xFF);
FatCookies 15:ccde02f96449 248 }
FatCookies 15:ccde02f96449 249 }
FatCookies 13:4e77264f254a 250 sendBattery();
FatCookies 13:4e77264f254a 251 }
FatCookies 13:4e77264f254a 252
FatCookies 13:4e77264f254a 253 frame_counter++;
FatCookies 13:4e77264f254a 254 }
FatCookies 13:4e77264f254a 255
FatCookies 15:ccde02f96449 256 inline float getLineEntropy() {
FatCookies 15:ccde02f96449 257 float entropy = 0;
FatCookies 15:ccde02f96449 258 float last = (int8_t)(TFC_LineScanImage0[0] >> 4) & 0xFF;
FatCookies 15:ccde02f96449 259 for(int i = 1; i < 128; i++) {
FatCookies 15:ccde02f96449 260 entropy += abs(last - ((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF));
FatCookies 15:ccde02f96449 261 }
FatCookies 15:ccde02f96449 262 return entropy;
FatCookies 15:ccde02f96449 263 }
FatCookies 15:ccde02f96449 264
FatCookies 13:4e77264f254a 265 inline void sendSpeeds() {
FatCookies 13:4e77264f254a 266 union {
FatCookies 12:da96e2f87465 267 float a;
FatCookies 12:da96e2f87465 268 unsigned char bytes[4];
FatCookies 12:da96e2f87465 269 } thing;
FatCookies 13:4e77264f254a 270
FatCookies 15:ccde02f96449 271 float en = getLineEntropy();
FatCookies 15:ccde02f96449 272
FatCookies 15:ccde02f96449 273 if(onTrack) {
FatCookies 15:ccde02f96449 274 if(en <= 14000) {
FatCookies 15:ccde02f96449 275 onTrack = false;
FatCookies 15:ccde02f96449 276 sendString("offfffffffffffff");
FatCookies 15:ccde02f96449 277 TFC_SetMotorPWM(0.0,0.0);
FatCookies 15:ccde02f96449 278 TFC_HBRIDGE_DISABLE;
FatCookies 15:ccde02f96449 279 }
FatCookies 15:ccde02f96449 280 } else {
FatCookies 15:ccde02f96449 281 if(en > 14000) {
FatCookies 15:ccde02f96449 282 onTrack = true;
FatCookies 15:ccde02f96449 283 sendString("ON TRACK");
FatCookies 15:ccde02f96449 284 }
FatCookies 15:ccde02f96449 285 }
FatCookies 15:ccde02f96449 286
FatCookies 14:13085e161dd1 287
FatCookies 13:4e77264f254a 288 pc.putc('B');
FatCookies 15:ccde02f96449 289 thing.a = en;//wL * WHEEL_RADIUS;
FatCookies 12:da96e2f87465 290 pc.putc(thing.bytes[0]);
FatCookies 12:da96e2f87465 291 pc.putc(thing.bytes[1]);
FatCookies 12:da96e2f87465 292 pc.putc(thing.bytes[2]);
FatCookies 12:da96e2f87465 293 pc.putc(thing.bytes[3]);
FatCookies 15:ccde02f96449 294 thing.a = en; //wR * WHEEL_RADIUS;
FatCookies 13:4e77264f254a 295 pc.putc(thing.bytes[0]);
FatCookies 13:4e77264f254a 296 pc.putc(thing.bytes[1]);
FatCookies 13:4e77264f254a 297 pc.putc(thing.bytes[2]);
FatCookies 13:4e77264f254a 298 pc.putc(thing.bytes[3]);
maximusismax 8:7c5e6b1e7aa5 299 }
maximusismax 8:7c5e6b1e7aa5 300
FatCookies 13:4e77264f254a 301
maximusismax 8:7c5e6b1e7aa5 302 inline void handleComms() {
maximusismax 8:7c5e6b1e7aa5 303 if(curr_cmd != 0) {
FatCookies 4:4afa448c9cce 304 switch(curr_cmd) {
FatCookies 4:4afa448c9cce 305 case 'A':
FatCookies 4:4afa448c9cce 306 if(xb.cBuffer->available() >= 3) {
FatCookies 4:4afa448c9cce 307 char p = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 308 char i = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 309 char d = xb.cBuffer->read();
FatCookies 12:da96e2f87465 310 servo_pid.Kp = p/25.0f;
FatCookies 12:da96e2f87465 311 servo_pid.Ki = i/25.0f;
FatCookies 12:da96e2f87465 312 servo_pid.Kd = d/25.0f;
FatCookies 13:4e77264f254a 313 sendString("pid= Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", servo_pid.Kp, servo_pid.Ki, servo_pid.Kd, p, i, d);
maximusismax 8:7c5e6b1e7aa5 314
FatCookies 4:4afa448c9cce 315 curr_cmd = 0;
FatCookies 4:4afa448c9cce 316 }
FatCookies 4:4afa448c9cce 317 break;
FatCookies 4:4afa448c9cce 318
FatCookies 4:4afa448c9cce 319 case 'F':
FatCookies 6:b0e160c51013 320 if(xb.cBuffer->available() >= 1) {
FatCookies 4:4afa448c9cce 321 char a = xb.cBuffer->read();
FatCookies 6:b0e160c51013 322 speed = a/256.0f;
FatCookies 7:ad893fc41b95 323 TFC_SetMotorPWM(speed,speed);
FatCookies 13:4e77264f254a 324 sendString("s = %u %f",a, speed);
FatCookies 4:4afa448c9cce 325 curr_cmd = 0;
FatCookies 4:4afa448c9cce 326 }
FatCookies 4:4afa448c9cce 327 break;
FatCookies 4:4afa448c9cce 328
FatCookies 4:4afa448c9cce 329 default:
FatCookies 13:4e77264f254a 330 // Unrecognised command
FatCookies 13:4e77264f254a 331 curr_cmd = 0;
FatCookies 4:4afa448c9cce 332 break;
FatCookies 4:4afa448c9cce 333 }
FatCookies 4:4afa448c9cce 334 }
FatCookies 4:4afa448c9cce 335
FatCookies 6:b0e160c51013 336 if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
FatCookies 4:4afa448c9cce 337 char cmd = xb.cBuffer->read();
FatCookies 4:4afa448c9cce 338 if(cmd == 'D') {
FatCookies 4:4afa448c9cce 339 TFC_InitServos(0.00052,0.00122,0.02);
FatCookies 4:4afa448c9cce 340 TFC_HBRIDGE_ENABLE;
FatCookies 10:1bd0224093e4 341 TFC_SetMotorPWM(RIGHT_MOTOR_COMPENSATION_RATIO*speed,speed);
FatCookies 12:da96e2f87465 342 servo_pid.integral = 0;
FatCookies 12:da96e2f87465 343
FatCookies 6:b0e160c51013 344
FatCookies 4:4afa448c9cce 345 } else if (cmd == 'C') {
FatCookies 4:4afa448c9cce 346 TFC_SetMotorPWM(0.0,0.0);
FatCookies 4:4afa448c9cce 347 TFC_HBRIDGE_DISABLE;
FatCookies 4:4afa448c9cce 348 } else if(cmd == 'A') {
FatCookies 4:4afa448c9cce 349 curr_cmd = 'A';
FatCookies 4:4afa448c9cce 350 } else if(cmd == 'F') {
FatCookies 4:4afa448c9cce 351 curr_cmd = 'F';
FatCookies 15:ccde02f96449 352 } else if(cmd == 'K') {
FatCookies 15:ccde02f96449 353 sendCam = ~sendCam;
FatCookies 4:4afa448c9cce 354 }
FatCookies 4:4afa448c9cce 355
FatCookies 4:4afa448c9cce 356 }
maximusismax 8:7c5e6b1e7aa5 357 }
FatCookies 15:ccde02f96449 358
FatCookies 16:81cdffd8c5d5 359 inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &l, uint8_t &r) {
maximusismax 8:7c5e6b1e7aa5 360
maximusismax 8:7c5e6b1e7aa5 361 diff = 0;
maximusismax 8:7c5e6b1e7aa5 362 prev = -1;
maximusismax 8:7c5e6b1e7aa5 363 for(i = 63; i > 0; i--) {
FatCookies 15:ccde02f96449 364 curr_left = (int8_t)(cam_data[i] >> 4) & 0xFF;
maximusismax 8:7c5e6b1e7aa5 365 diff = prev - curr_left;
maximusismax 8:7c5e6b1e7aa5 366 if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
FatCookies 16:81cdffd8c5d5 367 l = i;
maximusismax 8:7c5e6b1e7aa5 368 break;
maximusismax 8:7c5e6b1e7aa5 369 }
maximusismax 8:7c5e6b1e7aa5 370 prev = curr_left;
maximusismax 8:7c5e6b1e7aa5 371 }
maximusismax 8:7c5e6b1e7aa5 372
maximusismax 8:7c5e6b1e7aa5 373 prev = -1;
maximusismax 8:7c5e6b1e7aa5 374 for(i = 64; i < 128; i++) {
FatCookies 15:ccde02f96449 375 curr_right = (int8_t)(cam_data[i] >> 4) & 0xFF;
maximusismax 8:7c5e6b1e7aa5 376 int diff = prev - curr_right;
maximusismax 8:7c5e6b1e7aa5 377 if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
FatCookies 16:81cdffd8c5d5 378 r = i;
maximusismax 8:7c5e6b1e7aa5 379 break;
maximusismax 8:7c5e6b1e7aa5 380 }
maximusismax 8:7c5e6b1e7aa5 381 prev = curr_right;
maximusismax 8:7c5e6b1e7aa5 382 }
maximusismax 8:7c5e6b1e7aa5 383
FatCookies 15:ccde02f96449 384 //Calculate how left/right we are
FatCookies 16:81cdffd8c5d5 385 return (64 - ((l+r)/2))/64.f;
maximusismax 8:7c5e6b1e7aa5 386 }
FatCookies 3:87a5122682fa 387
FatCookies 16:81cdffd8c5d5 388
FatCookies 12:da96e2f87465 389 void handlePID(pid_instance *pid) {
FatCookies 12:da96e2f87465 390 pid->dt = t.read();
FatCookies 12:da96e2f87465 391 pid->pid_error = pid->desired_value - pid->measured_value;
FatCookies 12:da96e2f87465 392 pid->integral = pid->integral + pid->pid_error * pid->dt;
FatCookies 12:da96e2f87465 393 pid->derivative = (pid->pid_error - pid->p_error) / pid->dt;
FatCookies 12:da96e2f87465 394 pid->output = pid->Kp * pid->pid_error + pid->Ki * pid->integral + pid->Kd * pid->derivative;
FatCookies 12:da96e2f87465 395 pid->p_error = pid->pid_error;
FatCookies 12:da96e2f87465 396
FatCookies 12:da96e2f87465 397 if(pid->integral > 1.0f) {
FatCookies 12:da96e2f87465 398 pid->integral = 1.0f;
FatCookies 12:da96e2f87465 399 }
FatCookies 12:da96e2f87465 400 if(pid->integral < -1.0f) {
FatCookies 12:da96e2f87465 401 pid->integral = -1.0f;
FatCookies 12:da96e2f87465 402 }
FatCookies 12:da96e2f87465 403 }
FatCookies 12:da96e2f87465 404
maximusismax 8:7c5e6b1e7aa5 405 inline void PIDController() {
FatCookies 13:4e77264f254a 406 // update motor measurements
FatCookies 13:4e77264f254a 407 left_motor_pid.measured_value = wL;
FatCookies 13:4e77264f254a 408 right_motor_pid.measured_value = wR;
FatCookies 13:4e77264f254a 409
maximusismax 8:7c5e6b1e7aa5 410 //PID Stuff!
FatCookies 3:87a5122682fa 411 t.start();
FatCookies 12:da96e2f87465 412 handlePID(&servo_pid);
FatCookies 13:4e77264f254a 413 handlePID(&left_motor_pid);
FatCookies 13:4e77264f254a 414 handlePID(&right_motor_pid);
FatCookies 12:da96e2f87465 415
FatCookies 12:da96e2f87465 416 if((-1.0 <= servo_pid.output) && (servo_pid.output <= 1.0))
FatCookies 6:b0e160c51013 417 {
FatCookies 12:da96e2f87465 418 TFC_SetServo(0, servo_pid.output);
maximusismax 8:7c5e6b1e7aa5 419 }
maximusismax 8:7c5e6b1e7aa5 420 else //Unhappy PID state
maximusismax 8:7c5e6b1e7aa5 421 {
FatCookies 15:ccde02f96449 422 //sendString("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
maximusismax 8:7c5e6b1e7aa5 423 TFC_InitServos(0.00052, 0.00122, 0.02);
FatCookies 12:da96e2f87465 424 if(servo_pid.output >= 1.0f) {
maximusismax 8:7c5e6b1e7aa5 425 TFC_SetServo(0, 0.9f);
FatCookies 12:da96e2f87465 426 servo_pid.output = 1.0f;
maximusismax 8:7c5e6b1e7aa5 427 } else {
maximusismax 8:7c5e6b1e7aa5 428 TFC_SetServo(0, -0.9f);
FatCookies 12:da96e2f87465 429 servo_pid.output = -1.0f;
maximusismax 8:7c5e6b1e7aa5 430 }
FatCookies 13:4e77264f254a 431 }
FatCookies 12:da96e2f87465 432
FatCookies 6:b0e160c51013 433
FatCookies 3:87a5122682fa 434 t.stop();
FatCookies 3:87a5122682fa 435 t.reset();
FatCookies 3:87a5122682fa 436 t.start();
FatCookies 9:aa2ce38dec6b 437 }
FatCookies 9:aa2ce38dec6b 438
FatCookies 13:4e77264f254a 439 inline void handleStartStop() {
FatCookies 13:4e77264f254a 440 //Hacky way to detect the start/stop signal
FatCookies 13:4e77264f254a 441 if(right - left < 60) {
FatCookies 13:4e77264f254a 442 sendString("START STOP!! &d",startstop);
FatCookies 13:4e77264f254a 443
FatCookies 13:4e77264f254a 444 if(seen) {
FatCookies 13:4e77264f254a 445 seen = false;
FatCookies 13:4e77264f254a 446 } else {
FatCookies 13:4e77264f254a 447 startstop++;
FatCookies 13:4e77264f254a 448 seen = true;
FatCookies 13:4e77264f254a 449 }
FatCookies 13:4e77264f254a 450 //If we've done 5 laps, stop the car
FatCookies 13:4e77264f254a 451 if(startstop >= 1) {
FatCookies 13:4e77264f254a 452 TFC_SetMotorPWM(0.f,0.f);
FatCookies 13:4e77264f254a 453 TFC_HBRIDGE_DISABLE;
FatCookies 13:4e77264f254a 454 startstop = 0;
FatCookies 13:4e77264f254a 455 }
FatCookies 13:4e77264f254a 456 }
FatCookies 13:4e77264f254a 457 }
FatCookies 9:aa2ce38dec6b 458
FatCookies 9:aa2ce38dec6b 459
FatCookies 9:aa2ce38dec6b 460 inline void initSpeedSensors() {
FatCookies 9:aa2ce38dec6b 461 t1.start();
FatCookies 9:aa2ce38dec6b 462 t2.start();
FatCookies 9:aa2ce38dec6b 463
FatCookies 9:aa2ce38dec6b 464 //Left and Right are defined looking at the rear of the car, in the direction the camera points at.
FatCookies 9:aa2ce38dec6b 465 leftHallSensor.rise(&GetTime_L);
FatCookies 9:aa2ce38dec6b 466 rightHallSensor.rise(&GetTime_R);
FatCookies 9:aa2ce38dec6b 467 }
FatCookies 9:aa2ce38dec6b 468
FatCookies 9:aa2ce38dec6b 469 void GetTime_L(){
FatCookies 9:aa2ce38dec6b 470 Time_L=t1.read_us();
FatCookies 9:aa2ce38dec6b 471 t1.reset();
FatCookies 9:aa2ce38dec6b 472 }
FatCookies 9:aa2ce38dec6b 473
FatCookies 9:aa2ce38dec6b 474 void GetTime_R(){
FatCookies 9:aa2ce38dec6b 475 Time_R=t2.read_us();
FatCookies 9:aa2ce38dec6b 476 t2.reset();
maximusismax 0:566127ca8048 477 }