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@9:1ba8ee3f4ee3, 2018-04-22 (annotated)

Committer:
KDrainEE
Date:
Sun Apr 22 13:45:48 2018 +0000
Revision:
9:1ba8ee3f4ee3
Parent:
8:5bfd4ca9ce17
Child:
10:d24a94a677ea
started breaking up functionality into headers. Added circular include safguard

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