ELCT 302 / Mbed 2 deprecated Top_Fuel_Dragster

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@14:e55e80c1bd9a, 2018-04-24 (annotated)

Committer:
KDrainEE
Date:
Tue Apr 24 00:36:08 2018 +0000
Revision:
14:e55e80c1bd9a
Parent:
13:d1f37c9038de 
Saved prior to tuning (EKF focused);

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KDrainEE 0:30871514c229 1 //Works at speed 2.0, KPS 2.0e-2, KD 1.0e-4
KDrainEE 0:30871514c229 2
KDrainEE 0:30871514c229 3 //Has tolerance check, Steering doesn't
KDrainEE 0:30871514c229 4 //Values from Simulation
KDrainEE 0:30871514c229 5 //#define KPS 0.0896852742245504f
KDrainEE 0:30871514c229 6 //#define KD 0.072870569295611f
KDrainEE 0:30871514c229 7 //#define KPS 0.03f
KDrainEE 0:30871514c229 8
KDrainEE 0:30871514c229 9 #include "mbed.h"
KDrainEE 0:30871514c229 10 #include <iostream>
ELCT302Honors 6:1b4a677c468c 11 #include "lsc.h"
KDrainEE 14:e55e80c1bd9a 12 #include "kalman.h"
KDrainEE 0:30871514c229 13
KDrainEE 0:30871514c229 14 #define TI 0.001f
KDrainEE 0:30871514c229 15
KDrainEE 1:9149cfedd4d5 16 #define SCALAR 0.53f
KDrainEE 0:30871514c229 17 #define MINM 0.0f
KDrainEE 1:9149cfedd4d5 18 #define MAXM 0.53f
ELCT302Honors 3:246625f3ceee 19 #define KPM 0.15f //0.1414f
KDrainEE 0:30871514c229 20 #define KI 19.7408f
KDrainEE 0:30871514c229 21
KDrainEE 0:30871514c229 22 #define KPS 2.0E-2f //Original 2.0e-2
KDrainEE 0:30871514c229 23 #define KD 1.0e-4f
KDrainEE 0:30871514c229 24 #define SET 0.0f
KDrainEE 0:30871514c229 25 #define MINS 0.05f
KDrainEE 0:30871514c229 26 #define MAXS 0.1f
KDrainEE 0:30871514c229 27 #define BIAS 0.075f
KDrainEE 0:30871514c229 28 #define TOL 0.02f
KDrainEE 0:30871514c229 29 #define STEER_FREQ 0.02f //50 Hz
KDrainEE 0:30871514c229 30 #define STEERUPDATEPERIOD 0.05
KDrainEE 0:30871514c229 31
ELCT302Honors 6:1b4a677c468c 32 /***********************************|Pin Declarations|*************************************************************/
KDrainEE 0:30871514c229 33
ELCT302Honors 6:1b4a677c468c 34 //Feedback
ELCT302Honors 6:1b4a677c468c 35 AnalogIn speed(PTB0); //tachometer
ELCT302Honors 6:1b4a677c468c 36 AnalogIn _right(PTB1); //Right sensor
ELCT302Honors 6:1b4a677c468c 37 AnalogIn _left(PTB2); //Left sensor
ELCT302Honors 6:1b4a677c468c 38 //Output
ELCT302Honors 6:1b4a677c468c 39 PwmOut servoSig(PTA13); //PWM output to control servo position
ELCT302Honors 6:1b4a677c468c 40 PwmOut gateDrive(PTA4); //PWM output to control motor speed
ELCT302Honors 6:1b4a677c468c 41 DigitalOut brake(PTD0);
ELCT302Honors 6:1b4a677c468c 42 //Communication
ELCT302Honors 6:1b4a677c468c 43 Serial bt(PTE22, PTE23); //Serial Pins (Tx, Rx)
ELCT302Honors 6:1b4a677c468c 44 //LEDs
ELCT302Honors 6:1b4a677c468c 45 DigitalOut blue(LED_BLUE);
ELCT302Honors 6:1b4a677c468c 46 DigitalOut red(LED_RED);
ELCT302Honors 6:1b4a677c468c 47 DigitalOut green(LED_GREEN);
ELCT302Honors 6:1b4a677c468c 48 //Checkpoint Interrupts
ELCT302Honors 6:1b4a677c468c 49 InterruptIn navRt(PTD2);
ELCT302Honors 6:1b4a677c468c 50 InterruptIn navLft(PTD3);
ELCT302Honors 6:1b4a677c468c 51 //Button Interrupts
ELCT302Honors 6:1b4a677c468c 52 InterruptIn stopButton(PTD4);
ELCT302Honors 6:1b4a677c468c 53 InterruptIn goButton(PTA12);
ELCT302Honors 6:1b4a677c468c 54 //Camera timers
ELCT302Honors 6:1b4a677c468c 55 //DigitalOut si(PTC8);
ELCT302Honors 6:1b4a677c468c 56 //PwmOut clk(PTC9);
ELCT302Honors 6:1b4a677c468c 57 //InterruptIn edgeDetector(PTD5);
KDrainEE 0:30871514c229 58
ELCT302Honors 6:1b4a677c468c 59 /***********************************|Variable Declarations|*************************************************************/
KDrainEE 0:30871514c229 60
KDrainEE 0:30871514c229 61 Ticker control;
KDrainEE 0:30871514c229 62 Timer ctrlTimer;
ELCT302Honors 3:246625f3ceee 63
ELCT302Honors 5:aa582398b2eb 64 bool lTrig = false;
ELCT302Honors 5:aa582398b2eb 65 bool rTrig = false;
ELCT302Honors 3:246625f3ceee 66
ELCT302Honors 3:246625f3ceee 67 volatile int rightCount;
ELCT302Honors 3:246625f3ceee 68 volatile int leftCount;
ELCT302Honors 3:246625f3ceee 69
KDrainEE 14:e55e80c1bd9a 70 int data[6];
KDrainEE 14:e55e80c1bd9a 71 float temp;
KDrainEE 0:30871514c229 72
ELCT302Honors 3:246625f3ceee 73 float Setpoint = 0.0;
KDrainEE 4:af9973350ffe 74 float spHolder;
KDrainEE 0:30871514c229 75 float errSum = 0.0;
KDrainEE 0:30871514c229 76
KDrainEE 0:30871514c229 77 float fbPrev = 0.0f;
KDrainEE 1:9149cfedd4d5 78 float Kps = 2.0E-2; //0.013 for setpoint = 0.0/0.05
KDrainEE 1:9149cfedd4d5 79 float Kd = 1.0e-4;
KDrainEE 0:30871514c229 80
ELCT302Honors 3:246625f3ceee 81 float Kpm = 0.141;
ELCT302Honors 3:246625f3ceee 82
ELCT302Honors 6:1b4a677c468c 83 char state;
ELCT302Honors 6:1b4a677c468c 84 bool go = 0;
ELCT302Honors 6:1b4a677c468c 85
ELCT302Honors 6:1b4a677c468c 86 void waitState()
ELCT302Honors 6:1b4a677c468c 87 {
ELCT302Honors 6:1b4a677c468c 88 Setpoint = 0; //Makes sure the car isn't moving
ELCT302Honors 6:1b4a677c468c 89 state = 'w';
ELCT302Honors 6:1b4a677c468c 90 //bt.putc(state);
ELCT302Honors 6:1b4a677c468c 91 green = red = 1;
ELCT302Honors 6:1b4a677c468c 92 blue = 0; //blue LED indicating car is in wait state
ELCT302Honors 6:1b4a677c468c 93 }
ELCT302Honors 6:1b4a677c468c 94 void offTrack()
ELCT302Honors 6:1b4a677c468c 95 {
ELCT302Honors 6:1b4a677c468c 96 Setpoint = 0;
ELCT302Honors 6:1b4a677c468c 97 brake.write(1);
ELCT302Honors 6:1b4a677c468c 98 state = 'o';
ELCT302Honors 6:1b4a677c468c 99 //bt.putc(state);
ELCT302Honors 6:1b4a677c468c 100 green = blue = 1;
ELCT302Honors 6:1b4a677c468c 101 red = 0;
ELCT302Honors 6:1b4a677c468c 102 waitState();
ELCT302Honors 6:1b4a677c468c 103 }
ELCT302Honors 6:1b4a677c468c 104
ELCT302Honors 6:1b4a677c468c 105 void letsGo()
ELCT302Honors 6:1b4a677c468c 106 {
ELCT302Honors 6:1b4a677c468c 107 state = 'g';
ELCT302Honors 6:1b4a677c468c 108 //bt.putc(state);
ELCT302Honors 6:1b4a677c468c 109 red = blue = 1;
ELCT302Honors 6:1b4a677c468c 110 green = 0;
ELCT302Honors 6:1b4a677c468c 111 brake.write(0);
ELCT302Honors 6:1b4a677c468c 112 go = 1;
ELCT302Honors 6:1b4a677c468c 113 Setpoint = spHolder;
ELCT302Honors 6:1b4a677c468c 114
ELCT302Honors 6:1b4a677c468c 115
ELCT302Honors 6:1b4a677c468c 116 }
ELCT302Honors 6:1b4a677c468c 117
ELCT302Honors 6:1b4a677c468c 118
ELCT302Honors 6:1b4a677c468c 119 void stopState()
ELCT302Honors 6:1b4a677c468c 120 {
ELCT302Honors 6:1b4a677c468c 121 state = 's';
ELCT302Honors 6:1b4a677c468c 122 //bt.putc('s');
ELCT302Honors 6:1b4a677c468c 123 spHolder = Setpoint;
ELCT302Honors 6:1b4a677c468c 124 Setpoint = 0;
ELCT302Honors 6:1b4a677c468c 125 brake.write(1);
ELCT302Honors 6:1b4a677c468c 126 waitState();
ELCT302Honors 6:1b4a677c468c 127 }
ELCT302Honors 6:1b4a677c468c 128
KDrainEE 4:af9973350ffe 129 inline void applyBrake()
KDrainEE 4:af9973350ffe 130 {
KDrainEE 4:af9973350ffe 131 spHolder = Setpoint;
KDrainEE 4:af9973350ffe 132 brake.write(1);
KDrainEE 4:af9973350ffe 133 Setpoint = 0.0;
KDrainEE 4:af9973350ffe 134 }
KDrainEE 4:af9973350ffe 135
KDrainEE 4:af9973350ffe 136 inline void releaseBrake()
KDrainEE 4:af9973350ffe 137 {
KDrainEE 4:af9973350ffe 138 brake.write(0);
KDrainEE 4:af9973350ffe 139 Setpoint = spHolder;
KDrainEE 4:af9973350ffe 140 }
KDrainEE 4:af9973350ffe 141
KDrainEE 2:e87736742f99 142 void display()
KDrainEE 2:e87736742f99 143 {
ELCT302Honors 3:246625f3ceee 144 bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Kpm = %f, Brake = %f\r\n", Setpoint, Kps, Kd, Kpm, brake.read());
KDrainEE 2:e87736742f99 145 }
KDrainEE 2:e87736742f99 146
ELCT302Honors 6:1b4a677c468c 147
KDrainEE 0:30871514c229 148 void serCb()
KDrainEE 0:30871514c229 149 {
KDrainEE 0:30871514c229 150 char x = bt.getc();
KDrainEE 0:30871514c229 151 if (x == 'u')
KDrainEE 0:30871514c229 152 {
ELCT302Honors 3:246625f3ceee 153 Setpoint += 0.025;
KDrainEE 2:e87736742f99 154 display();
KDrainEE 0:30871514c229 155 }
KDrainEE 0:30871514c229 156 else if(x == 'h')
KDrainEE 0:30871514c229 157 {
ELCT302Honors 3:246625f3ceee 158 Setpoint -= 0.025;
KDrainEE 2:e87736742f99 159 display();
KDrainEE 0:30871514c229 160 }
KDrainEE 1:9149cfedd4d5 161 else if (x == 'i')
KDrainEE 1:9149cfedd4d5 162 {
KDrainEE 1:9149cfedd4d5 163 Kps += 1.0e-3;
KDrainEE 2:e87736742f99 164 display();
KDrainEE 1:9149cfedd4d5 165 }
KDrainEE 1:9149cfedd4d5 166 else if (x == 'j')
KDrainEE 1:9149cfedd4d5 167 {
KDrainEE 1:9149cfedd4d5 168 Kps -= 1.0e-3;
KDrainEE 2:e87736742f99 169 display();
KDrainEE 1:9149cfedd4d5 170 }
KDrainEE 1:9149cfedd4d5 171 else if (x == 'o')
KDrainEE 1:9149cfedd4d5 172 {
KDrainEE 1:9149cfedd4d5 173 Kd += 1.0e-5;
KDrainEE 2:e87736742f99 174 display();
KDrainEE 1:9149cfedd4d5 175 }
KDrainEE 1:9149cfedd4d5 176 else if (x == 'k')
KDrainEE 1:9149cfedd4d5 177 {
KDrainEE 1:9149cfedd4d5 178 Kd -= 1.0e-5;
KDrainEE 2:e87736742f99 179 display();
KDrainEE 1:9149cfedd4d5 180 }
KDrainEE 2:e87736742f99 181 else if (x == 'b')
KDrainEE 2:e87736742f99 182 {
KDrainEE 4:af9973350ffe 183 applyBrake();
KDrainEE 2:e87736742f99 184 display();
ELCT302Honors 3:246625f3ceee 185
KDrainEE 2:e87736742f99 186 }
ELCT302Honors 3:246625f3ceee 187 else if (x == 'n')
KDrainEE 2:e87736742f99 188 {
KDrainEE 4:af9973350ffe 189 releaseBrake();
KDrainEE 2:e87736742f99 190 display();
KDrainEE 2:e87736742f99 191 }
KDrainEE 2:e87736742f99 192 else if (x == 'p')
KDrainEE 2:e87736742f99 193 {
KDrainEE 2:e87736742f99 194 display();
ELCT302Honors 3:246625f3ceee 195 }
ELCT302Honors 3:246625f3ceee 196 else if (x == 'y')
ELCT302Honors 3:246625f3ceee 197 {
ELCT302Honors 3:246625f3ceee 198 Kpm += 0.003;
ELCT302Honors 3:246625f3ceee 199 display();
ELCT302Honors 3:246625f3ceee 200 }
ELCT302Honors 3:246625f3ceee 201 else if(x == 'g')
ELCT302Honors 3:246625f3ceee 202 {
ELCT302Honors 3:246625f3ceee 203 Kpm -= 0.003;
ELCT302Honors 3:246625f3ceee 204 display();
ELCT302Honors 3:246625f3ceee 205 }
ELCT302Honors 3:246625f3ceee 206 else if (x == 'z')
ELCT302Honors 3:246625f3ceee 207 {
ELCT302Honors 3:246625f3ceee 208 bt.printf("Right = %i Left = %i\r\n", rightCount, leftCount);
ELCT302Honors 3:246625f3ceee 209 }
ELCT302Honors 6:1b4a677c468c 210 else if(x== 's')
ELCT302Honors 6:1b4a677c468c 211 {
ELCT302Honors 6:1b4a677c468c 212 stopState();
ELCT302Honors 6:1b4a677c468c 213 }
ELCT302Honors 6:1b4a677c468c 214 else if(x == 'a')
ELCT302Honors 6:1b4a677c468c 215 {
ELCT302Honors 6:1b4a677c468c 216 letsGo();
ELCT302Honors 6:1b4a677c468c 217 }
KDrainEE 0:30871514c229 218 else
KDrainEE 0:30871514c229 219 {
KDrainEE 2:e87736742f99 220 bt.printf("Invalid input");
KDrainEE 0:30871514c229 221 }
KDrainEE 0:30871514c229 222 if(Setpoint > MAXM) Setpoint = MAXM;
KDrainEE 2:e87736742f99 223 if(Setpoint < MINM) Setpoint = MINM;
KDrainEE 0:30871514c229 224 }
KDrainEE 0:30871514c229 225
KDrainEE 2:e87736742f99 226
ELCT302Honors 3:246625f3ceee 227 void incL()
ELCT302Honors 3:246625f3ceee 228 {
ELCT302Honors 3:246625f3ceee 229 leftCount++;
ELCT302Honors 5:aa582398b2eb 230 lTrig = true;
ELCT302Honors 3:246625f3ceee 231 }
KDrainEE 2:e87736742f99 232
ELCT302Honors 3:246625f3ceee 233 void incR()
KDrainEE 0:30871514c229 234 {
ELCT302Honors 3:246625f3ceee 235 rightCount++;
ELCT302Honors 5:aa582398b2eb 236 rTrig = true;
KDrainEE 0:30871514c229 237 }
KDrainEE 0:30871514c229 238
KDrainEE 14:e55e80c1bd9a 239 Kalman ekf;
KDrainEE 0:30871514c229 240 void steer()
KDrainEE 0:30871514c229 241 {
KDrainEE 0:30871514c229 242 float L = _left.read();
KDrainEE 0:30871514c229 243 float R = _right.read();
ELCT302Honors 3:246625f3ceee 244 if (L == 0.0 && R == 0.0)
KDrainEE 0:30871514c229 245 {
ELCT302Honors 3:246625f3ceee 246 //off track
KDrainEE 13:d1f37c9038de 247 Setpoint = 0.0;
ELCT302Honors 3:246625f3ceee 248 }
KDrainEE 14:e55e80c1bd9a 249 float mfb = L - R;
KDrainEE 14:e55e80c1bd9a 250 data[0] = mfb;
KDrainEE 14:e55e80c1bd9a 251 float cfb = (float)camMax/64;
KDrainEE 14:e55e80c1bd9a 252 data[1] = cfb;
KDrainEE 14:e55e80c1bd9a 253 data[0] = 64-camMax;
KDrainEE 14:e55e80c1bd9a 254 // data[0] = camMax;
KDrainEE 14:e55e80c1bd9a 255 float observations[2] = {mfb,cfb};
KDrainEE 14:e55e80c1bd9a 256 float variances[2] = {0.19,1.5e-6};
KDrainEE 14:e55e80c1bd9a 257 //float angles[2] = {inductorAngles[64-camMax],cameraAngles[64-camMax]};
KDrainEE 14:e55e80c1bd9a 258
KDrainEE 14:e55e80c1bd9a 259 data[2] = angles[0];
KDrainEE 14:e55e80c1bd9a 260 data[3] = angles[1];
KDrainEE 14:e55e80c1bd9a 261 //EKF
KDrainEE 14:e55e80c1bd9a 262 float fb = ekf.kCompute(6.7475*speed.read(),1800*servoSig.read() - 135.0,observations,variances,angles);
KDrainEE 14:e55e80c1bd9a 263 // float fb = L - R;
KDrainEE 14:e55e80c1bd9a 264 // temp = L - R;
KDrainEE 14:e55e80c1bd9a 265 data[4] = fb;
KDrainEE 0:30871514c229 266 float e = SET - fb;
KDrainEE 14:e55e80c1bd9a 267
KDrainEE 13:d1f37c9038de 268 Kps = 0.005/speed.read();
KDrainEE 13:d1f37c9038de 269 if (Kps > 0.02) Kps = 0.02;
KDrainEE 13:d1f37c9038de 270 Kd = 1.0e-3/speed.read();
KDrainEE 13:d1f37c9038de 271 if (Kd > 1.0e-4) Kd = 1.0e-4;
KDrainEE 1:9149cfedd4d5 272 float Controlleroutput = Kps * e - (Kd * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error??
KDrainEE 0:30871514c229 273 if (Controlleroutput > MAXS) Controlleroutput = MAXS;
KDrainEE 0:30871514c229 274 else if (Controlleroutput < MINS) Controlleroutput = MINS;
KDrainEE 0:30871514c229 275 if (abs(Controlleroutput - servoSig.read()) > TOL || ctrlTimer.read() >= STEERUPDATEPERIOD)
KDrainEE 0:30871514c229 276 {
KDrainEE 0:30871514c229 277 ctrlTimer.reset();
KDrainEE 0:30871514c229 278 servoSig.write(Controlleroutput);
KDrainEE 0:30871514c229 279 }
KDrainEE 0:30871514c229 280 fbPrev = fb;
KDrainEE 13:d1f37c9038de 281 // data[0] = _right.read();
KDrainEE 13:d1f37c9038de 282 // data[1] = _left.read();
KDrainEE 13:d1f37c9038de 283 // data[2] = Controlleroutput;
KDrainEE 13:d1f37c9038de 284 // data[3] = e;
KDrainEE 0:30871514c229 285
KDrainEE 0:30871514c229 286 }
KDrainEE 0:30871514c229 287 void drive()
KDrainEE 0:30871514c229 288 {
KDrainEE 0:30871514c229 289 float error = Setpoint-speed.read();
KDrainEE 0:30871514c229 290 errSum +=(error * TI);
KDrainEE 0:30871514c229 291 float iTerm = KI*errSum;
KDrainEE 0:30871514c229 292 if(iTerm > MAXM) iTerm = MAXM;
KDrainEE 0:30871514c229 293 if(iTerm < MINM) iTerm = MINM;
KDrainEE 0:30871514c229 294 float output = KPM*error + iTerm;
KDrainEE 0:30871514c229 295 if(output > MAXM) output = MAXM;
ELCT302Honors 3:246625f3ceee 296 if(output < MINM) output = MINM;
ELCT302Honors 3:246625f3ceee 297
ELCT302Honors 3:246625f3ceee 298 gateDrive.write(output);
KDrainEE 13:d1f37c9038de 299 // data[4] = gateDrive.read();
KDrainEE 13:d1f37c9038de 300 // data[5] = speed.read();
KDrainEE 0:30871514c229 301 }
KDrainEE 0:30871514c229 302
KDrainEE 0:30871514c229 303 void cb()
KDrainEE 0:30871514c229 304 {
KDrainEE 0:30871514c229 305 steer();
ELCT302Honors 6:1b4a677c468c 306 if (state == 'g'){
ELCT302Honors 6:1b4a677c468c 307 drive();
ELCT302Honors 6:1b4a677c468c 308 }
KDrainEE 0:30871514c229 309 }
KDrainEE 0:30871514c229 310
KDrainEE 0:30871514c229 311 int main()
KDrainEE 0:30871514c229 312 {
KDrainEE 0:30871514c229 313 //startup checks
ELCT302Honors 3:246625f3ceee 314 bt.baud(115200);
ELCT302Honors 6:1b4a677c468c 315 bt.attach(&serCb);
ELCT302Honors 6:1b4a677c468c 316 cameraInit();
ELCT302Honors 6:1b4a677c468c 317 cameraDaq.attach(&acquire, 0.01f);
KDrainEE 0:30871514c229 318 servoSig.period(STEER_FREQ);
KDrainEE 0:30871514c229 319 gateDrive.period(.00005f);
KDrainEE 0:30871514c229 320 gateDrive.write(Setpoint);
KDrainEE 0:30871514c229 321 ctrlTimer.start();
KDrainEE 14:e55e80c1bd9a 322 control.attach(&cb, TI);
ELCT302Honors 3:246625f3ceee 323 rightCount = 0;
KDrainEE 14:e55e80c1bd9a 324 leftCount = 0;
ELCT302Honors 3:246625f3ceee 325 navRt.fall(&incR);
ELCT302Honors 3:246625f3ceee 326 navLft.fall(&incL);
ELCT302Honors 3:246625f3ceee 327
ELCT302Honors 6:1b4a677c468c 328 goButton.fall(&letsGo);
ELCT302Honors 6:1b4a677c468c 329 stopButton.fall(&stopState);
ELCT302Honors 6:1b4a677c468c 330 waitState();
KDrainEE 0:30871514c229 331 while(1) {
KDrainEE 14:e55e80c1bd9a 332 bt.printf("mfb: %f, cfb: %f, angle0: %f, angle1: %f, fb: %f\r\n", data[0], data[1], data[2], data[3], data[4]);
KDrainEE 14:e55e80c1bd9a 333 //bt.printf("%i, %f \r\n", camMax, temp);
ELCT302Honors 6:1b4a677c468c 334
KDrainEE 0:30871514c229 335 }
KDrainEE 0:30871514c229 336 }