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@5:aa582398b2eb, 2018-04-19 (annotated)

Committer:
ELCT302Honors
Date:
Thu Apr 19 15:42:21 2018 +0000
Revision:
5:aa582398b2eb
Parent:
4:af9973350ffe
Child:
6:1b4a677c468c
Most Recent;

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>
KDrainEE 0:30871514c229 11
KDrainEE 0:30871514c229 12 #define TI 0.001f
KDrainEE 0:30871514c229 13
KDrainEE 1:9149cfedd4d5 14 #define SCALAR 0.53f
KDrainEE 0:30871514c229 15 #define MINM 0.0f
KDrainEE 1:9149cfedd4d5 16 #define MAXM 0.53f
ELCT302Honors 3:246625f3ceee 17 #define KPM 0.15f //0.1414f
KDrainEE 0:30871514c229 18 #define KI 19.7408f
KDrainEE 0:30871514c229 19
KDrainEE 0:30871514c229 20 #define KPS 2.0E-2f //Original 2.0e-2
KDrainEE 0:30871514c229 21 #define KD 1.0e-4f
KDrainEE 0:30871514c229 22 #define SET 0.0f
KDrainEE 0:30871514c229 23 #define MINS 0.05f
KDrainEE 0:30871514c229 24 #define MAXS 0.1f
KDrainEE 0:30871514c229 25 #define BIAS 0.075f
KDrainEE 0:30871514c229 26 #define TOL 0.02f
KDrainEE 0:30871514c229 27 #define STEER_FREQ 0.02f //50 Hz
KDrainEE 0:30871514c229 28 #define STEERUPDATEPERIOD 0.05
KDrainEE 0:30871514c229 29
KDrainEE 0:30871514c229 30 AnalogIn _right(PTB1); //Right sensor
KDrainEE 0:30871514c229 31 AnalogIn _left(PTB3); //Left sensor
KDrainEE 0:30871514c229 32 AnalogIn speed(PTB2); //tachometer
KDrainEE 0:30871514c229 33
KDrainEE 0:30871514c229 34 PwmOut servoSig(PTA13); //PWM output to control servo
KDrainEE 0:30871514c229 35 PwmOut gateDrive(PTA4);
KDrainEE 0:30871514c229 36
KDrainEE 0:30871514c229 37 Serial bt(PTE0, PTE1); //COM12
KDrainEE 0:30871514c229 38 DigitalOut myled(LED_BLUE);
KDrainEE 2:e87736742f99 39 DigitalOut brake(PTD0);
KDrainEE 0:30871514c229 40
KDrainEE 0:30871514c229 41 Ticker control;
KDrainEE 0:30871514c229 42 Timer ctrlTimer;
ELCT302Honors 3:246625f3ceee 43
ELCT302Honors 3:246625f3ceee 44 InterruptIn navRt(PTD2);
ELCT302Honors 3:246625f3ceee 45 InterruptIn navLft(PTD3);
ELCT302Honors 5:aa582398b2eb 46 bool lTrig = false;
ELCT302Honors 5:aa582398b2eb 47 bool rTrig = false;
ELCT302Honors 3:246625f3ceee 48
ELCT302Honors 3:246625f3ceee 49 volatile int rightCount;
ELCT302Honors 3:246625f3ceee 50 volatile int leftCount;
ELCT302Honors 3:246625f3ceee 51
KDrainEE 0:30871514c229 52 float data[6];
KDrainEE 0:30871514c229 53
ELCT302Honors 3:246625f3ceee 54 float Setpoint = 0.0;
KDrainEE 4:af9973350ffe 55 float spHolder;
KDrainEE 0:30871514c229 56 float errSum = 0.0;
KDrainEE 0:30871514c229 57
KDrainEE 0:30871514c229 58 float fbPrev = 0.0f;
KDrainEE 1:9149cfedd4d5 59 float Kps = 2.0E-2; //0.013 for setpoint = 0.0/0.05
KDrainEE 1:9149cfedd4d5 60 float Kd = 1.0e-4;
KDrainEE 0:30871514c229 61
ELCT302Honors 3:246625f3ceee 62 float Kpm = 0.141;
ELCT302Honors 3:246625f3ceee 63
KDrainEE 4:af9973350ffe 64 inline void applyBrake()
KDrainEE 4:af9973350ffe 65 {
KDrainEE 4:af9973350ffe 66 spHolder = Setpoint;
KDrainEE 4:af9973350ffe 67 brake.write(1);
KDrainEE 4:af9973350ffe 68 Setpoint = 0.0;
KDrainEE 4:af9973350ffe 69 }
KDrainEE 4:af9973350ffe 70
KDrainEE 4:af9973350ffe 71 inline void releaseBrake()
KDrainEE 4:af9973350ffe 72 {
KDrainEE 4:af9973350ffe 73 brake.write(0);
KDrainEE 4:af9973350ffe 74 Setpoint = spHolder;
KDrainEE 4:af9973350ffe 75 }
KDrainEE 4:af9973350ffe 76
KDrainEE 2:e87736742f99 77 void display()
KDrainEE 2:e87736742f99 78 {
ELCT302Honors 3:246625f3ceee 79 bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Kpm = %f, Brake = %f\r\n", Setpoint, Kps, Kd, Kpm, brake.read());
KDrainEE 2:e87736742f99 80 }
KDrainEE 2:e87736742f99 81
KDrainEE 0:30871514c229 82 void serCb()
KDrainEE 0:30871514c229 83 {
KDrainEE 0:30871514c229 84 char x = bt.getc();
KDrainEE 0:30871514c229 85 if (x == 'u')
KDrainEE 0:30871514c229 86 {
ELCT302Honors 3:246625f3ceee 87 Setpoint += 0.025;
KDrainEE 2:e87736742f99 88 display();
KDrainEE 0:30871514c229 89 }
KDrainEE 0:30871514c229 90 else if(x == 'h')
KDrainEE 0:30871514c229 91 {
ELCT302Honors 3:246625f3ceee 92 Setpoint -= 0.025;
KDrainEE 2:e87736742f99 93 display();
KDrainEE 0:30871514c229 94 }
KDrainEE 1:9149cfedd4d5 95 else if (x == 'i')
KDrainEE 1:9149cfedd4d5 96 {
KDrainEE 1:9149cfedd4d5 97 Kps += 1.0e-3;
KDrainEE 2:e87736742f99 98 display();
KDrainEE 1:9149cfedd4d5 99 }
KDrainEE 1:9149cfedd4d5 100 else if (x == 'j')
KDrainEE 1:9149cfedd4d5 101 {
KDrainEE 1:9149cfedd4d5 102 Kps -= 1.0e-3;
KDrainEE 2:e87736742f99 103 display();
KDrainEE 1:9149cfedd4d5 104 }
KDrainEE 1:9149cfedd4d5 105 else if (x == 'o')
KDrainEE 1:9149cfedd4d5 106 {
KDrainEE 1:9149cfedd4d5 107 Kd += 1.0e-5;
KDrainEE 2:e87736742f99 108 display();
KDrainEE 1:9149cfedd4d5 109 }
KDrainEE 1:9149cfedd4d5 110 else if (x == 'k')
KDrainEE 1:9149cfedd4d5 111 {
KDrainEE 1:9149cfedd4d5 112 Kd -= 1.0e-5;
KDrainEE 2:e87736742f99 113 display();
KDrainEE 1:9149cfedd4d5 114 }
KDrainEE 2:e87736742f99 115 else if (x == 'b')
KDrainEE 2:e87736742f99 116 {
KDrainEE 4:af9973350ffe 117 applyBrake();
KDrainEE 2:e87736742f99 118 display();
ELCT302Honors 3:246625f3ceee 119
KDrainEE 2:e87736742f99 120 }
ELCT302Honors 3:246625f3ceee 121 else if (x == 'n')
KDrainEE 2:e87736742f99 122 {
KDrainEE 4:af9973350ffe 123 releaseBrake();
KDrainEE 2:e87736742f99 124 display();
KDrainEE 2:e87736742f99 125 }
KDrainEE 2:e87736742f99 126 else if (x == 'p')
KDrainEE 2:e87736742f99 127 {
KDrainEE 2:e87736742f99 128 display();
ELCT302Honors 3:246625f3ceee 129 }
ELCT302Honors 3:246625f3ceee 130 else if (x == 'y')
ELCT302Honors 3:246625f3ceee 131 {
ELCT302Honors 3:246625f3ceee 132 Kpm += 0.003;
ELCT302Honors 3:246625f3ceee 133 display();
ELCT302Honors 3:246625f3ceee 134 }
ELCT302Honors 3:246625f3ceee 135 else if(x == 'g')
ELCT302Honors 3:246625f3ceee 136 {
ELCT302Honors 3:246625f3ceee 137 Kpm -= 0.003;
ELCT302Honors 3:246625f3ceee 138 display();
ELCT302Honors 3:246625f3ceee 139 }
ELCT302Honors 3:246625f3ceee 140 else if (x == 'z')
ELCT302Honors 3:246625f3ceee 141 {
ELCT302Honors 3:246625f3ceee 142 bt.printf("Right = %i Left = %i\r\n", rightCount, leftCount);
ELCT302Honors 3:246625f3ceee 143 }
KDrainEE 0:30871514c229 144 else
KDrainEE 0:30871514c229 145 {
KDrainEE 2:e87736742f99 146 bt.printf("Invalid input");
KDrainEE 0:30871514c229 147 }
KDrainEE 0:30871514c229 148 if(Setpoint > MAXM) Setpoint = MAXM;
KDrainEE 2:e87736742f99 149 if(Setpoint < MINM) Setpoint = MINM;
KDrainEE 0:30871514c229 150 }
KDrainEE 0:30871514c229 151
KDrainEE 2:e87736742f99 152
ELCT302Honors 3:246625f3ceee 153 void incL()
ELCT302Honors 3:246625f3ceee 154 {
ELCT302Honors 3:246625f3ceee 155 leftCount++;
ELCT302Honors 5:aa582398b2eb 156 lTrig = true;
ELCT302Honors 3:246625f3ceee 157 }
KDrainEE 2:e87736742f99 158
ELCT302Honors 3:246625f3ceee 159 void incR()
KDrainEE 0:30871514c229 160 {
ELCT302Honors 3:246625f3ceee 161 rightCount++;
ELCT302Honors 5:aa582398b2eb 162 rTrig = true;
KDrainEE 0:30871514c229 163 }
KDrainEE 0:30871514c229 164
KDrainEE 0:30871514c229 165 void steer()
KDrainEE 0:30871514c229 166 {
KDrainEE 0:30871514c229 167 float L = _left.read();
KDrainEE 0:30871514c229 168 float R = _right.read();
ELCT302Honors 3:246625f3ceee 169 if (L == 0.0 && R == 0.0)
KDrainEE 0:30871514c229 170 {
ELCT302Honors 3:246625f3ceee 171 //off track
ELCT302Honors 3:246625f3ceee 172 Setpoint = 0.0;
ELCT302Honors 3:246625f3ceee 173 }
KDrainEE 0:30871514c229 174 float fb = L - R;
KDrainEE 0:30871514c229 175 float e = SET - fb;
KDrainEE 1:9149cfedd4d5 176 float Controlleroutput = Kps * e - (Kd * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error??
KDrainEE 0:30871514c229 177 if (Controlleroutput > MAXS) Controlleroutput = MAXS;
KDrainEE 0:30871514c229 178 else if (Controlleroutput < MINS) Controlleroutput = MINS;
KDrainEE 0:30871514c229 179 if (abs(Controlleroutput - servoSig.read()) > TOL || ctrlTimer.read() >= STEERUPDATEPERIOD)
KDrainEE 0:30871514c229 180 {
KDrainEE 0:30871514c229 181 ctrlTimer.reset();
KDrainEE 0:30871514c229 182 servoSig.write(Controlleroutput);
KDrainEE 0:30871514c229 183 }
KDrainEE 0:30871514c229 184 fbPrev = fb;
KDrainEE 0:30871514c229 185 data[0] = _right.read();
KDrainEE 0:30871514c229 186 data[1] = _left.read();
KDrainEE 0:30871514c229 187 data[2] = Controlleroutput;
KDrainEE 0:30871514c229 188 data[3] = e;
KDrainEE 0:30871514c229 189
KDrainEE 0:30871514c229 190 }
KDrainEE 0:30871514c229 191 void drive()
KDrainEE 0:30871514c229 192 {
KDrainEE 0:30871514c229 193 float error = Setpoint-speed.read();
KDrainEE 0:30871514c229 194 errSum +=(error * TI);
KDrainEE 0:30871514c229 195 float iTerm = KI*errSum;
KDrainEE 0:30871514c229 196 if(iTerm > MAXM) iTerm = MAXM;
KDrainEE 0:30871514c229 197 if(iTerm < MINM) iTerm = MINM;
KDrainEE 0:30871514c229 198 float output = KPM*error + iTerm;
KDrainEE 0:30871514c229 199 if(output > MAXM) output = MAXM;
ELCT302Honors 3:246625f3ceee 200 if(output < MINM) output = MINM;
ELCT302Honors 3:246625f3ceee 201
ELCT302Honors 3:246625f3ceee 202 gateDrive.write(output);
KDrainEE 0:30871514c229 203 data[4] = gateDrive.read();
KDrainEE 0:30871514c229 204 data[5] = speed.read();
KDrainEE 0:30871514c229 205 }
KDrainEE 0:30871514c229 206
KDrainEE 0:30871514c229 207 void cb()
KDrainEE 0:30871514c229 208 {
KDrainEE 0:30871514c229 209 steer();
KDrainEE 0:30871514c229 210 drive();
KDrainEE 0:30871514c229 211 }
KDrainEE 0:30871514c229 212
KDrainEE 0:30871514c229 213 int main()
KDrainEE 0:30871514c229 214 {
KDrainEE 0:30871514c229 215 //startup checks
ELCT302Honors 3:246625f3ceee 216 bt.baud(115200);
ELCT302Honors 3:246625f3ceee 217 bt.attach(&serCb);
KDrainEE 0:30871514c229 218 servoSig.period(STEER_FREQ);
KDrainEE 0:30871514c229 219 gateDrive.period(.00005f);
KDrainEE 0:30871514c229 220 gateDrive.write(Setpoint);
KDrainEE 0:30871514c229 221
KDrainEE 0:30871514c229 222 ctrlTimer.start();
KDrainEE 0:30871514c229 223 control.attach(&cb, TI);
KDrainEE 0:30871514c229 224
ELCT302Honors 3:246625f3ceee 225 rightCount = 0;
ELCT302Honors 3:246625f3ceee 226 leftCount = 0;
ELCT302Honors 5:aa582398b2eb 227
ELCT302Honors 5:aa582398b2eb 228
ELCT302Honors 3:246625f3ceee 229 navRt.fall(&incR);
ELCT302Honors 3:246625f3ceee 230 navLft.fall(&incL);
ELCT302Honors 3:246625f3ceee 231
KDrainEE 0:30871514c229 232 while(1) {
KDrainEE 0:30871514c229 233 myled = !myled;
ELCT302Honors 5:aa582398b2eb 234 if(lTrig){
ELCT302Honors 5:aa582398b2eb 235 bt.putc('l');
ELCT302Honors 5:aa582398b2eb 236 lTrig = false;
ELCT302Honors 5:aa582398b2eb 237 }
ELCT302Honors 5:aa582398b2eb 238 if(rTrig){
ELCT302Honors 5:aa582398b2eb 239 bt.putc('r');
ELCT302Honors 5:aa582398b2eb 240 rTrig = false;
ELCT302Honors 5:aa582398b2eb 241 }
KDrainEE 1:9149cfedd4d5 242 //bt.printf("%f ",data[0]);
KDrainEE 1:9149cfedd4d5 243 // bt.printf("%f ", data[1]);
KDrainEE 1:9149cfedd4d5 244 // bt.printf("%f ", data[2]);
KDrainEE 1:9149cfedd4d5 245 // bt.printf("%f ", data[3]);
KDrainEE 1:9149cfedd4d5 246 // bt.printf("%f ", data[4]);
KDrainEE 1:9149cfedd4d5 247 // bt.printf("%f\r\n", data[5]);
KDrainEE 0:30871514c229 248 }
KDrainEE 0:30871514c229 249 }