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@17:6ae90788cc2b, 2016-12-02 (annotated)

Committer:
FatCookies
Date:
Fri Dec 02 14:36:37 2016 +0000
Revision:
17:6ae90788cc2b
Parent:
16:81cdffd8c5d5
Child:
18:0095a3a8f8e4
cleaned up stoff

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