Jorn Dokter / Mbed 2 deprecated TEB_branch2

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 QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@25:86741f4565f1, 2019-10-07 (annotated)

Committer:
JordanO
Date:
Mon Oct 07 12:48:52 2019 +0000
Revision:
25:86741f4565f1
Parent:
17:16d29ed4ab00
Child:
26:21911b4a79b4
Implementing struc instead of pass;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JornD 8:ccd58f78b5cb 1 //Libraries
JordanO 10:ad2da21a102c 2 #include "mbed.h"
JordanO 10:ad2da21a102c 3 #include "FastPWM.h"
JordanO 10:ad2da21a102c 4 #include "HIDScope.h"
JordanO 10:ad2da21a102c 5 #include "MODSERIAL.h"
JordanO 10:ad2da21a102c 6 #include "QEI.h"
JornD 15:95034d92bc76 7
JornD 15:95034d92bc76 8 //Homebrew libraries
JornD 15:95034d92bc76 9 #include "header.h"
JornD 15:95034d92bc76 10 //#include "controller.cpp"
JornD 15:95034d92bc76 11
JornD 8:ccd58f78b5cb 12
JordanO 10:ad2da21a102c 13 //Objects
JordanO 10:ad2da21a102c 14 //LED
JordanO 10:ad2da21a102c 15 DigitalOut ledb(LED_BLUE); // ledb=true & ledb=1 is led off!!
JordanO 10:ad2da21a102c 16 DigitalOut ledg(LED_GREEN);
JordanO 10:ad2da21a102c 17 DigitalOut ledr(LED_RED);
JordanO 10:ad2da21a102c 18 //Motors
JordanO 10:ad2da21a102c 19
JordanO 10:ad2da21a102c 20 //Sensors
JordanO 14:1a695fc45fc6 21 //QEI Encoder1(D12,D13,NC,64,QEI::X2_ENCODING);
JordanO 14:1a695fc45fc6 22 //QEI Encoder2(D14,D15,NC,64,QEI::X2_ENCODING);
JordanO 14:1a695fc45fc6 23 //QEI Encoder3(D16,D17,NC,64,QEI::X2_ENCODING);
JordanO 10:ad2da21a102c 24
JordanO 10:ad2da21a102c 25 //Buttons
JordanO 11:32deb48774f7 26 InterruptIn button1(SW2); //button on the side of the reset button
JordanO 11:32deb48774f7 27 InterruptIn button2(SW3); //button on the side opposite of the reset button
JordanO 11:32deb48774f7 28
JordanO 11:32deb48774f7 29 //PC
JornD 15:95034d92bc76 30 Serial pc(USBTX,USBRX);
JordanO 25:86741f4565f1 31
JordanO 25:86741f4565f1 32 //Structure
JordanO 25:86741f4565f1 33 struct struc1
JordanO 25:86741f4565f1 34 {
JordanO 25:86741f4565f1 35 int calibrationCountsMotor1;
JordanO 25:86741f4565f1 36 int calibrationCountsMotor2;
JordanO 25:86741f4565f1 37 int calibrationCountsMotor3;
JordanO 25:86741f4565f1 38
JordanO 25:86741f4565f1 39 float countsMotor1Return;
JordanO 25:86741f4565f1 40 float countsMotor2Return;
JordanO 25:86741f4565f1 41 float velocityMotor1Return;
JordanO 25:86741f4565f1 42 float velocityMotor2Return;
JordanO 25:86741f4565f1 43 };
JordanO 25:86741f4565f1 44 struc1 strucM&E
JornD 8:ccd58f78b5cb 45
JordanO 10:ad2da21a102c 46 //Variables
JordanO 10:ad2da21a102c 47 enum States {MovementIdle, CalibrationIdle, Demo, Startup, CalibrationPhysical, CalibrationEMG, Move, TiltCup, FailState};
JordanO 10:ad2da21a102c 48 States CurrentState;
JordanO 10:ad2da21a102c 49
JordanO 10:ad2da21a102c 50 volatile char ledcolor; //r is red, b is blue, g is green, t is bluegreen, p is purple
JordanO 13:f90e31c6af2b 51 volatile int errorCode;
JordanO 10:ad2da21a102c 52
JordanO 25:86741f4565f1 53
JordanO 14:1a695fc45fc6 54 //Ticker Timings
JordanO 14:1a695fc45fc6 55 const float mainLoopT = 2; //Main Loopt Ticker wait
JordanO 14:1a695fc45fc6 56 const float ledFlipperT = .5; //LED Flicker wait
JordanO 14:1a695fc45fc6 57
JornD 9:cb88b16a97d5 58
JordanO 10:ad2da21a102c 59
JordanO 10:ad2da21a102c 60 //Tickers
JordanO 10:ad2da21a102c 61 Ticker Main_Ticker;
JordanO 10:ad2da21a102c 62 Ticker Tick_Blinky;//used for the blinking of the leds
JordanO 10:ad2da21a102c 63
JornD 8:ccd58f78b5cb 64 //Led FLicker
JornD 8:ccd58f78b5cb 65 void FlipLED(void)
JornD 8:ccd58f78b5cb 66 {
JordanO 13:f90e31c6af2b 67 pc.printf("FLIPLED \r\n");
JornD 8:ccd58f78b5cb 68 switch(ledcolor)
JornD 8:ccd58f78b5cb 69 {
JornD 8:ccd58f78b5cb 70 case 'r':
JordanO 13:f90e31c6af2b 71 ledr = !ledr;
JornD 8:ccd58f78b5cb 72 ledg = true;
JornD 8:ccd58f78b5cb 73 ledb = true;
JornD 8:ccd58f78b5cb 74 break;
JornD 8:ccd58f78b5cb 75 case 'b':
JornD 8:ccd58f78b5cb 76 ledr = true;
JornD 8:ccd58f78b5cb 77 ledg = true;
JordanO 13:f90e31c6af2b 78 ledb = !ledb;
JornD 8:ccd58f78b5cb 79 break;
JornD 8:ccd58f78b5cb 80 case 'g':
JornD 8:ccd58f78b5cb 81 ledr = true;
JordanO 13:f90e31c6af2b 82 ledg = !ledg;
JornD 8:ccd58f78b5cb 83 ledb = true;
JordanO 13:f90e31c6af2b 84 break;
JornD 8:ccd58f78b5cb 85 case 't':
JornD 8:ccd58f78b5cb 86 ledr = true;
JordanO 13:f90e31c6af2b 87 ledg = !ledg;
JordanO 13:f90e31c6af2b 88 ledb = !ledb;
JornD 8:ccd58f78b5cb 89 break;
JornD 8:ccd58f78b5cb 90 case 'p':
JordanO 13:f90e31c6af2b 91 ledr = !ledr;
JornD 8:ccd58f78b5cb 92 ledg = true;
JordanO 13:f90e31c6af2b 93 ledb = !ledb;
JornD 8:ccd58f78b5cb 94 break;
JordanO 13:f90e31c6af2b 95 default:
JordanO 13:f90e31c6af2b 96 errorCode = 1;
JordanO 14:1a695fc45fc6 97 CurrentState = FailState;
JornD 8:ccd58f78b5cb 98 }
JordanO 13:f90e31c6af2b 99 pc.printf("Color %c \r\n",ledcolor);
JornD 8:ccd58f78b5cb 100 }
JornD 8:ccd58f78b5cb 101
JordanO 14:1a695fc45fc6 102 // SW2 = button1 state changing
JordanO 11:32deb48774f7 103 void Run_StateChangerButton1()
JordanO 11:32deb48774f7 104 {
JordanO 11:32deb48774f7 105 switch(CurrentState)
JordanO 11:32deb48774f7 106 {
JordanO 14:1a695fc45fc6 107 case Startup: //From
JordanO 14:1a695fc45fc6 108 CurrentState = CalibrationPhysical; //To
JordanO 14:1a695fc45fc6 109 break; //Break from switch
JordanO 11:32deb48774f7 110 case CalibrationPhysical:
JordanO 11:32deb48774f7 111 CurrentState = CalibrationIdle;
JordanO 11:32deb48774f7 112 break;
JordanO 11:32deb48774f7 113 case CalibrationIdle:
JordanO 11:32deb48774f7 114 CurrentState = CalibrationEMG;
JordanO 11:32deb48774f7 115 break;
JordanO 11:32deb48774f7 116 case CalibrationEMG:
JordanO 11:32deb48774f7 117 CurrentState = MovementIdle;
JordanO 11:32deb48774f7 118 break;
JordanO 11:32deb48774f7 119 case MovementIdle:
JordanO 11:32deb48774f7 120 CurrentState = TiltCup;
JordanO 11:32deb48774f7 121 break;
JordanO 11:32deb48774f7 122 case TiltCup:
JordanO 11:32deb48774f7 123 CurrentState = MovementIdle;
JordanO 11:32deb48774f7 124 break;
JordanO 11:32deb48774f7 125 case Move:
JordanO 11:32deb48774f7 126 CurrentState = MovementIdle;
JordanO 11:32deb48774f7 127 break;
JordanO 11:32deb48774f7 128 }
JordanO 11:32deb48774f7 129 }
JordanO 14:1a695fc45fc6 130 // SW3 = button2 state changing
JornD 15:95034d92bc76 131 void Run_StateChangerButton2(void)
JordanO 12:d13ce121a781 132 {
JordanO 12:d13ce121a781 133 switch(CurrentState)
JordanO 12:d13ce121a781 134 {
JordanO 13:f90e31c6af2b 135 case CalibrationIdle:
JordanO 13:f90e31c6af2b 136 CurrentState = Demo;
JordanO 13:f90e31c6af2b 137 break;
JordanO 13:f90e31c6af2b 138 case MovementIdle:
JordanO 13:f90e31c6af2b 139 CurrentState = Move;
JordanO 13:f90e31c6af2b 140 break;
JordanO 13:f90e31c6af2b 141 case TiltCup:
JordanO 13:f90e31c6af2b 142 CurrentState = MovementIdle;
JordanO 13:f90e31c6af2b 143 break;
JordanO 12:d13ce121a781 144 }
JordanO 12:d13ce121a781 145 }
JordanO 12:d13ce121a781 146
JordanO 14:1a695fc45fc6 147
JordanO 14:1a695fc45fc6 148 //State functions
JornD 2:6d026d5655c5 149 void Run_Demo(void)
JornD 2:6d026d5655c5 150 {
JordanO 11:32deb48774f7 151 pc.printf("Starting Demo ...\r\n");
JornD 5:77929f5f1f32 152 }
JornD 2:6d026d5655c5 153
JornD 5:77929f5f1f32 154 void Run_MovementIdle(void)
JornD 5:77929f5f1f32 155 {
JordanO 11:32deb48774f7 156 pc.printf("Starting Idle ...\r\n");
JornD 5:77929f5f1f32 157 }
JornD 5:77929f5f1f32 158
JornD 5:77929f5f1f32 159 void Run_CalibrationIdle(void)
JornD 5:77929f5f1f32 160 {
JordanO 11:32deb48774f7 161 pc.printf("Starting Calibration Idle ...\r\n");
JornD 5:77929f5f1f32 162 }
JornD 5:77929f5f1f32 163
JornD 5:77929f5f1f32 164 void Run_Startup(void)
JornD 5:77929f5f1f32 165 {
JordanO 11:32deb48774f7 166 pc.printf("Starting Startup ...\r\n");
JornD 2:6d026d5655c5 167 }
JornD 2:6d026d5655c5 168
JornD 5:77929f5f1f32 169 void Run_CalibrationPhysical(void)
JornD 2:6d026d5655c5 170 {
JordanO 11:32deb48774f7 171 pc.printf("Starting Calibration Physical ... \r\n");
JordanO 25:86741f4565f1 172 float calibrationPeriodMotor1 = 1/2000;
JordanO 25:86741f4565f1 173 float calibrationPeriodMotor2 = 1/2000;
JordanO 25:86741f4565f1 174 float calibrationPeriodMotor3 = 1/2000;
JordanO 25:86741f4565f1 175
JordanO 25:86741f4565f1 176 float calibrationPWM = .1;
JordanO 25:86741f4565f1 177 float calibrationVelocity = .1;
JordanO 25:86741f4565f1 178
JordanO 25:86741f4565f1 179 float calibrationPWM1 = calibrationPWM;
JordanO 25:86741f4565f1 180 float calibrationPWM2 = 0;
JordanO 25:86741f4565f1 181 float calibrationPWM3 = 0;
JordanO 25:86741f4565f1 182
JordanO 25:86741f4565f1 183 motorAndEncoder(calibrationPWM1, calibrationPeriodMotor1, calibrationPWM2, calibrationPeriodMotor2, mainLoopT, &countsMotor1Return, &countsMotor2Return, &velocityMotor1Return, &velocityMotor2Return, calibrationCountsMotor1, calibrationCountsMotor2, calibrationCountsMotor3);
JordanO 25:86741f4565f1 184
JordanO 25:86741f4565f1 185 if (velocityMotor1 <= calibrationVelocity)
JordanO 25:86741f4565f1 186 {
JordanO 25:86741f4565f1 187 calibrationCountsMotor1 = countsMotor1Return; //calibrate motor1
JordanO 25:86741f4565f1 188 calibrationPWM1 = 0;
JordanO 25:86741f4565f1 189 calibrationPWM2 = calibrationPWM;
JordanO 25:86741f4565f1 190 calibrationPWM3 = 0;
JordanO 25:86741f4565f1 191 if (velocityMotor2 <= calibrationVelocity) //Calibrate motor2
JordanO 25:86741f4565f1 192 {
JordanO 25:86741f4565f1 193 calibrationCountsMotor2 = countsMotor2Return;
JordanO 25:86741f4565f1 194 calibrationPWM1 = 0;
JordanO 25:86741f4565f1 195 calibrationPWM2 = 0;
JordanO 25:86741f4565f1 196 calibrationPWM3 = calibrationPWM;
JordanO 25:86741f4565f1 197
JordanO 25:86741f4565f1 198 if (velocityMotor3 <= calibrationVelocity) //calibrate motor3
JordanO 25:86741f4565f1 199 {
JordanO 25:86741f4565f1 200 calibrationCountsMotor3 = countsMotor3Return;
JordanO 25:86741f4565f1 201 calibrationPWM1 = 0;
JordanO 25:86741f4565f1 202 calibrationPWM2 = 0;
JordanO 25:86741f4565f1 203 calibrationPWM3 = 0;
JordanO 25:86741f4565f1 204 motorAndEncoder(calibrationPWM1, calibrationPeriodMotor1, calibrationPWM2, calibrationPeriodMotor2, mainLoopT, float &countsMotor1Return, float &countsMotor2Return, float &velocityMotor1Return, float &velocityMotor2Return, calibrationCountsMotor1, calibrationCountsMotor2, calibrationCountsMotor3);
JordanO 25:86741f4565f1 205 }
JordanO 25:86741f4565f1 206 }
JordanO 25:86741f4565f1 207
JordanO 25:86741f4565f1 208 }
JordanO 25:86741f4565f1 209
JordanO 25:86741f4565f1 210
JordanO 11:32deb48774f7 211 CurrentState = CalibrationIdle;
JordanO 11:32deb48774f7 212
JornD 5:77929f5f1f32 213 }
JornD 2:6d026d5655c5 214
JornD 5:77929f5f1f32 215 void Run_CalibrationEMG(void)
JornD 5:77929f5f1f32 216 {
JordanO 11:32deb48774f7 217 pc.printf("Starting Calibration EMG ... \r\n");
JornD 2:6d026d5655c5 218 }
JornD 2:6d026d5655c5 219
JornD 5:77929f5f1f32 220 void Run_Move(void)
JornD 5:77929f5f1f32 221 {
JordanO 13:f90e31c6af2b 222 pc.printf("Starting Move ... \r\n");
JornD 5:77929f5f1f32 223 }
JornD 5:77929f5f1f32 224
JornD 5:77929f5f1f32 225 void Run_TiltCup(void)
JornD 5:77929f5f1f32 226 {
JordanO 11:32deb48774f7 227 pc.printf("Starting Calibration TiltCup ... \r\n");
JornD 5:77929f5f1f32 228 }
JornD 5:77929f5f1f32 229
JornD 5:77929f5f1f32 230 void Run_FailState(void)
Rieks 6:41b02057fc2c 231 {
JordanO 14:1a695fc45fc6 232 pc.printf("Error: %i",errorCode);
JornD 5:77929f5f1f32 233 }
JornD 5:77929f5f1f32 234
JordanO 11:32deb48774f7 235
JornD 5:77929f5f1f32 236 //State Machine
JornD 2:6d026d5655c5 237 void StateMachine(void)
JornD 2:6d026d5655c5 238 {
JordanO 13:f90e31c6af2b 239 //Turn off all LEDs
JornD 2:6d026d5655c5 240 switch(CurrentState)
JornD 2:6d026d5655c5 241 {
JornD 2:6d026d5655c5 242 case Demo:
JordanO 13:f90e31c6af2b 243 ledcolor='t';
JordanO 11:32deb48774f7 244 Run_Demo();
JornD 2:6d026d5655c5 245 break;
JornD 5:77929f5f1f32 246 case MovementIdle:
JordanO 13:f90e31c6af2b 247 ledcolor='b';
JordanO 11:32deb48774f7 248 Run_MovementIdle();
JornD 5:77929f5f1f32 249 break;
JornD 5:77929f5f1f32 250 case CalibrationIdle:
JordanO 13:f90e31c6af2b 251 ledcolor='b';
JordanO 11:32deb48774f7 252 Run_CalibrationIdle();
JornD 5:77929f5f1f32 253 break;
JornD 9:cb88b16a97d5 254 case Startup:
JordanO 13:f90e31c6af2b 255 ledcolor='b';
JordanO 11:32deb48774f7 256 Run_Startup();
JornD 5:77929f5f1f32 257 break;
JornD 5:77929f5f1f32 258 case CalibrationPhysical:
Rieks 7:af0c16ae48d7 259 ledcolor='g';
JordanO 11:32deb48774f7 260 Run_CalibrationPhysical();
JornD 5:77929f5f1f32 261 break;
JornD 5:77929f5f1f32 262 case CalibrationEMG:
Rieks 7:af0c16ae48d7 263 ledcolor='g';
JordanO 11:32deb48774f7 264 Run_CalibrationEMG();
JornD 5:77929f5f1f32 265 break;
JornD 5:77929f5f1f32 266 case Move:
Rieks 7:af0c16ae48d7 267 ledcolor='p';
JordanO 11:32deb48774f7 268 Run_Move();
JornD 5:77929f5f1f32 269 break;
JornD 5:77929f5f1f32 270 case TiltCup:
Rieks 7:af0c16ae48d7 271 ledcolor='t';
JordanO 11:32deb48774f7 272 Run_TiltCup();
JornD 5:77929f5f1f32 273 break;
JornD 5:77929f5f1f32 274 case FailState:
JordanO 13:f90e31c6af2b 275 ledcolor='r';
JordanO 11:32deb48774f7 276 Run_FailState();
JornD 2:6d026d5655c5 277 break;
JordanO 12:d13ce121a781 278 default:
JordanO 14:1a695fc45fc6 279 CurrentState = FailState;
JordanO 14:1a695fc45fc6 280 errorCode = 2;
JordanO 12:d13ce121a781 281 break;
JornD 2:6d026d5655c5 282 }
JornD 2:6d026d5655c5 283 }
JornD 1:a5c1140f464a 284
JornD 5:77929f5f1f32 285 //Main Loop
JornD 8:ccd58f78b5cb 286 void mainloop()
JornD 8:ccd58f78b5cb 287 {
JornD 9:cb88b16a97d5 288 StateMachine();
JornD 8:ccd58f78b5cb 289 }
JornD 8:ccd58f78b5cb 290
JornD 1:a5c1140f464a 291 int main()
JornD 8:ccd58f78b5cb 292 {
JornD 8:ccd58f78b5cb 293 //Initialize
JordanO 13:f90e31c6af2b 294 ledr = true;
JordanO 13:f90e31c6af2b 295 //ledr.write(.4);
JordanO 13:f90e31c6af2b 296 ledg = true;
JordanO 13:f90e31c6af2b 297 //ledg.write(.4);
JordanO 13:f90e31c6af2b 298 ledb = true;
JordanO 13:f90e31c6af2b 299 //ledb.write(.4);
JordanO 11:32deb48774f7 300
JordanO 11:32deb48774f7 301 pc.baud(115200);
JornD 5:77929f5f1f32 302 CurrentState = Startup;
JornD 9:cb88b16a97d5 303
JordanO 11:32deb48774f7 304 button1.mode(PullUp);
JordanO 11:32deb48774f7 305 button1.rise(Run_StateChangerButton1);
JordanO 11:32deb48774f7 306 button2.mode(PullUp);
JordanO 12:d13ce121a781 307 button2.rise(Run_StateChangerButton2);
JordanO 12:d13ce121a781 308
JordanO 13:f90e31c6af2b 309 //Tickers
JordanO 14:1a695fc45fc6 310 Main_Ticker.attach(mainloop,mainLoopT);
JordanO 14:1a695fc45fc6 311 wait(mainLoopT);
JordanO 14:1a695fc45fc6 312 Tick_Blinky.attach(FlipLED,ledFlipperT);
JornD 16:fa8001fef71b 313
JornD 16:fa8001fef71b 314 //Placeholder function call
JornD 15:95034d92bc76 315 double PlantError = 1;
JornD 16:fa8001fef71b 316 double Ts = 1;
JornD 17:16d29ed4ab00 317 double u = ControllerPID(PlantError, Ts);
JordanO 11:32deb48774f7 318
JornD 1:a5c1140f464a 319 while(true)
JornD 1:a5c1140f464a 320 {
JornD 8:ccd58f78b5cb 321
JornD 1:a5c1140f464a 322 }
JornD 1:a5c1140f464a 323 }