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@2:e87736742f99, 2018-04-16 (annotated)

Committer:
KDrainEE
Date:
Mon Apr 16 12:19:39 2018 +0000
Revision:
2:e87736742f99
Parent:
1:9149cfedd4d5
Child:
3:246625f3ceee
Still can change parameters on the fly. Braking is also implemented but seems to stay on perpetually. Probably a hardware issue. Burned controller last time so be careful

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