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@3:246625f3ceee, 2018-04-17 (annotated)

Committer:
ELCT302Honors
Date:
Tue Apr 17 22:56:24 2018 +0000
Revision:
3:246625f3ceee
Parent:
2:e87736742f99
Child:
4:af9973350ffe
has counter implemented, still buggy, might want to do something with timing so only the long pulses of the tape trigger the counter;

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 3:246625f3ceee 46
ELCT302Honors 3:246625f3ceee 47 volatile int rightCount;
ELCT302Honors 3:246625f3ceee 48 volatile int leftCount;
ELCT302Honors 3:246625f3ceee 49
KDrainEE 0:30871514c229 50 float data[6];
KDrainEE 0:30871514c229 51
ELCT302Honors 3:246625f3ceee 52 float Setpoint = 0.0;
ELCT302Honors 3:246625f3ceee 53 //float sp = 0.05;
KDrainEE 0:30871514c229 54 float errSum = 0.0;
KDrainEE 0:30871514c229 55
KDrainEE 0:30871514c229 56 float fbPrev = 0.0f;
KDrainEE 1:9149cfedd4d5 57 float Kps = 2.0E-2; //0.013 for setpoint = 0.0/0.05
KDrainEE 1:9149cfedd4d5 58 float Kd = 1.0e-4;
KDrainEE 0:30871514c229 59
ELCT302Honors 3:246625f3ceee 60 float Kpm = 0.141;
ELCT302Honors 3:246625f3ceee 61
KDrainEE 2:e87736742f99 62 void display()
KDrainEE 2:e87736742f99 63 {
ELCT302Honors 3:246625f3ceee 64 bt.printf("Setpoint = %f, Kps = %f, Kd = %f, Kpm = %f, Brake = %f\r\n", Setpoint, Kps, Kd, Kpm, brake.read());
KDrainEE 2:e87736742f99 65 }
KDrainEE 2:e87736742f99 66
KDrainEE 0:30871514c229 67 void serCb()
KDrainEE 0:30871514c229 68 {
KDrainEE 0:30871514c229 69 char x = bt.getc();
KDrainEE 0:30871514c229 70 if (x == 'u')
KDrainEE 0:30871514c229 71 {
ELCT302Honors 3:246625f3ceee 72 Setpoint += 0.025;
KDrainEE 2:e87736742f99 73 display();
KDrainEE 0:30871514c229 74 }
KDrainEE 0:30871514c229 75 else if(x == 'h')
KDrainEE 0:30871514c229 76 {
ELCT302Honors 3:246625f3ceee 77 Setpoint -= 0.025;
KDrainEE 2:e87736742f99 78 display();
KDrainEE 0:30871514c229 79 }
KDrainEE 1:9149cfedd4d5 80 else if (x == 'i')
KDrainEE 1:9149cfedd4d5 81 {
KDrainEE 1:9149cfedd4d5 82 Kps += 1.0e-3;
KDrainEE 2:e87736742f99 83 display();
KDrainEE 1:9149cfedd4d5 84 }
KDrainEE 1:9149cfedd4d5 85 else if (x == 'j')
KDrainEE 1:9149cfedd4d5 86 {
KDrainEE 1:9149cfedd4d5 87 Kps -= 1.0e-3;
KDrainEE 2:e87736742f99 88 display();
KDrainEE 1:9149cfedd4d5 89 }
KDrainEE 1:9149cfedd4d5 90 else if (x == 'o')
KDrainEE 1:9149cfedd4d5 91 {
KDrainEE 1:9149cfedd4d5 92 Kd += 1.0e-5;
KDrainEE 2:e87736742f99 93 display();
KDrainEE 1:9149cfedd4d5 94 }
KDrainEE 1:9149cfedd4d5 95 else if (x == 'k')
KDrainEE 1:9149cfedd4d5 96 {
KDrainEE 1:9149cfedd4d5 97 Kd -= 1.0e-5;
KDrainEE 2:e87736742f99 98 display();
KDrainEE 1:9149cfedd4d5 99 }
KDrainEE 2:e87736742f99 100 else if (x == 'b')
KDrainEE 2:e87736742f99 101 {
KDrainEE 2:e87736742f99 102 brake.write(1);
KDrainEE 2:e87736742f99 103 display();
ELCT302Honors 3:246625f3ceee 104
KDrainEE 2:e87736742f99 105 }
ELCT302Honors 3:246625f3ceee 106 else if (x == 'n')
KDrainEE 2:e87736742f99 107 {
KDrainEE 2:e87736742f99 108 brake.write(0);
KDrainEE 2:e87736742f99 109 display();
KDrainEE 2:e87736742f99 110 }
KDrainEE 2:e87736742f99 111 else if (x == 'p')
KDrainEE 2:e87736742f99 112 {
KDrainEE 2:e87736742f99 113 display();
ELCT302Honors 3:246625f3ceee 114 }
ELCT302Honors 3:246625f3ceee 115 else if (x == 'y')
ELCT302Honors 3:246625f3ceee 116 {
ELCT302Honors 3:246625f3ceee 117 Kpm += 0.003;
ELCT302Honors 3:246625f3ceee 118 display();
ELCT302Honors 3:246625f3ceee 119 }
ELCT302Honors 3:246625f3ceee 120 else if(x == 'g')
ELCT302Honors 3:246625f3ceee 121 {
ELCT302Honors 3:246625f3ceee 122 Kpm -= 0.003;
ELCT302Honors 3:246625f3ceee 123 display();
ELCT302Honors 3:246625f3ceee 124 }
ELCT302Honors 3:246625f3ceee 125 else if (x == 'z')
ELCT302Honors 3:246625f3ceee 126 {
ELCT302Honors 3:246625f3ceee 127 bt.printf("Right = %i Left = %i\r\n", rightCount, leftCount);
ELCT302Honors 3:246625f3ceee 128 }
KDrainEE 0:30871514c229 129 else
KDrainEE 0:30871514c229 130 {
KDrainEE 2:e87736742f99 131 bt.printf("Invalid input");
KDrainEE 0:30871514c229 132 }
KDrainEE 0:30871514c229 133 if(Setpoint > MAXM) Setpoint = MAXM;
KDrainEE 2:e87736742f99 134 if(Setpoint < MINM) Setpoint = MINM;
KDrainEE 0:30871514c229 135 }
KDrainEE 0:30871514c229 136
KDrainEE 2:e87736742f99 137
ELCT302Honors 3:246625f3ceee 138 void incL()
ELCT302Honors 3:246625f3ceee 139 {
ELCT302Honors 3:246625f3ceee 140 leftCount++;
ELCT302Honors 3:246625f3ceee 141 }
KDrainEE 2:e87736742f99 142
ELCT302Honors 3:246625f3ceee 143 void incR()
KDrainEE 0:30871514c229 144 {
ELCT302Honors 3:246625f3ceee 145 rightCount++;
KDrainEE 0:30871514c229 146 }
KDrainEE 0:30871514c229 147
KDrainEE 0:30871514c229 148 void steer()
KDrainEE 0:30871514c229 149 {
KDrainEE 0:30871514c229 150 float L = _left.read();
KDrainEE 0:30871514c229 151 float R = _right.read();
ELCT302Honors 3:246625f3ceee 152 if (L == 0.0 && R == 0.0)
KDrainEE 0:30871514c229 153 {
ELCT302Honors 3:246625f3ceee 154 //off track
ELCT302Honors 3:246625f3ceee 155 Setpoint = 0.0;
ELCT302Honors 3:246625f3ceee 156 }
KDrainEE 0:30871514c229 157 float fb = L - R;
KDrainEE 0:30871514c229 158 float e = SET - fb;
KDrainEE 1:9149cfedd4d5 159 float Controlleroutput = Kps * e - (Kd * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error??
KDrainEE 0:30871514c229 160 if (Controlleroutput > MAXS) Controlleroutput = MAXS;
KDrainEE 0:30871514c229 161 else if (Controlleroutput < MINS) Controlleroutput = MINS;
KDrainEE 0:30871514c229 162 if (abs(Controlleroutput - servoSig.read()) > TOL || ctrlTimer.read() >= STEERUPDATEPERIOD)
KDrainEE 0:30871514c229 163 {
KDrainEE 0:30871514c229 164 ctrlTimer.reset();
KDrainEE 0:30871514c229 165 servoSig.write(Controlleroutput);
KDrainEE 0:30871514c229 166 }
KDrainEE 0:30871514c229 167 fbPrev = fb;
KDrainEE 0:30871514c229 168 data[0] = _right.read();
KDrainEE 0:30871514c229 169 data[1] = _left.read();
KDrainEE 0:30871514c229 170 data[2] = Controlleroutput;
KDrainEE 0:30871514c229 171 data[3] = e;
KDrainEE 0:30871514c229 172
KDrainEE 0:30871514c229 173 }
KDrainEE 0:30871514c229 174 void drive()
KDrainEE 0:30871514c229 175 {
KDrainEE 0:30871514c229 176 float error = Setpoint-speed.read();
KDrainEE 0:30871514c229 177 errSum +=(error * TI);
KDrainEE 0:30871514c229 178 float iTerm = KI*errSum;
KDrainEE 0:30871514c229 179 if(iTerm > MAXM) iTerm = MAXM;
KDrainEE 0:30871514c229 180 if(iTerm < MINM) iTerm = MINM;
KDrainEE 0:30871514c229 181 float output = KPM*error + iTerm;
KDrainEE 0:30871514c229 182 if(output > MAXM) output = MAXM;
ELCT302Honors 3:246625f3ceee 183 if(output < MINM) output = MINM;
ELCT302Honors 3:246625f3ceee 184
ELCT302Honors 3:246625f3ceee 185 gateDrive.write(output);
KDrainEE 0:30871514c229 186 data[4] = gateDrive.read();
KDrainEE 0:30871514c229 187 data[5] = speed.read();
KDrainEE 0:30871514c229 188 }
KDrainEE 0:30871514c229 189
KDrainEE 0:30871514c229 190 void cb()
KDrainEE 0:30871514c229 191 {
KDrainEE 0:30871514c229 192 steer();
KDrainEE 0:30871514c229 193 drive();
KDrainEE 0:30871514c229 194 }
KDrainEE 0:30871514c229 195
KDrainEE 0:30871514c229 196 int main()
KDrainEE 0:30871514c229 197 {
KDrainEE 0:30871514c229 198 //startup checks
ELCT302Honors 3:246625f3ceee 199 bt.baud(115200);
ELCT302Honors 3:246625f3ceee 200 bt.attach(&serCb);
KDrainEE 0:30871514c229 201 servoSig.period(STEER_FREQ);
KDrainEE 0:30871514c229 202 gateDrive.period(.00005f);
KDrainEE 0:30871514c229 203 gateDrive.write(Setpoint);
KDrainEE 0:30871514c229 204
KDrainEE 0:30871514c229 205 ctrlTimer.start();
KDrainEE 0:30871514c229 206 control.attach(&cb, TI);
KDrainEE 0:30871514c229 207
ELCT302Honors 3:246625f3ceee 208 rightCount = 0;
ELCT302Honors 3:246625f3ceee 209 leftCount = 0;
ELCT302Honors 3:246625f3ceee 210
ELCT302Honors 3:246625f3ceee 211 navRt.fall(&incR);
ELCT302Honors 3:246625f3ceee 212 navLft.fall(&incL);
ELCT302Honors 3:246625f3ceee 213
KDrainEE 0:30871514c229 214 while(1) {
KDrainEE 0:30871514c229 215 myled = !myled;
KDrainEE 1:9149cfedd4d5 216 //bt.printf("%f ",data[0]);
KDrainEE 1:9149cfedd4d5 217 // bt.printf("%f ", data[1]);
KDrainEE 1:9149cfedd4d5 218 // bt.printf("%f ", data[2]);
KDrainEE 1:9149cfedd4d5 219 // bt.printf("%f ", data[3]);
KDrainEE 1:9149cfedd4d5 220 // bt.printf("%f ", data[4]);
KDrainEE 1:9149cfedd4d5 221 // bt.printf("%f\r\n", data[5]);
KDrainEE 0:30871514c229 222 }
KDrainEE 0:30871514c229 223 }