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@15:95034d92bc76, 2019-10-03 (annotated)

Committer:
JornD
Date:
Thu Oct 03 13:03:38 2019 +0000
Revision:
15:95034d92bc76
Parent:
14:1a695fc45fc6
Child:
16:fa8001fef71b
Impremented controller;

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);
JornD 8:ccd58f78b5cb 31
JordanO 10:ad2da21a102c 32 //Variables
JordanO 10:ad2da21a102c 33 enum States {MovementIdle, CalibrationIdle, Demo, Startup, CalibrationPhysical, CalibrationEMG, Move, TiltCup, FailState};
JordanO 10:ad2da21a102c 34 States CurrentState;
JordanO 10:ad2da21a102c 35
JordanO 10:ad2da21a102c 36 volatile char ledcolor; //r is red, b is blue, g is green, t is bluegreen, p is purple
JordanO 13:f90e31c6af2b 37 volatile int errorCode;
JordanO 10:ad2da21a102c 38
JordanO 14:1a695fc45fc6 39 //Ticker Timings
JordanO 14:1a695fc45fc6 40 const float mainLoopT = 2; //Main Loopt Ticker wait
JordanO 14:1a695fc45fc6 41 const float ledFlipperT = .5; //LED Flicker wait
JordanO 14:1a695fc45fc6 42
JornD 9:cb88b16a97d5 43
JordanO 10:ad2da21a102c 44
JordanO 10:ad2da21a102c 45 //Tickers
JordanO 10:ad2da21a102c 46 Ticker Main_Ticker;
JordanO 10:ad2da21a102c 47 Ticker Tick_Blinky;//used for the blinking of the leds
JordanO 10:ad2da21a102c 48
JornD 8:ccd58f78b5cb 49 //Led FLicker
JornD 8:ccd58f78b5cb 50 void FlipLED(void)
JornD 8:ccd58f78b5cb 51 {
JordanO 13:f90e31c6af2b 52 pc.printf("FLIPLED \r\n");
JornD 8:ccd58f78b5cb 53 switch(ledcolor)
JornD 8:ccd58f78b5cb 54 {
JornD 8:ccd58f78b5cb 55 case 'r':
JordanO 13:f90e31c6af2b 56 ledr = !ledr;
JornD 8:ccd58f78b5cb 57 ledg = true;
JornD 8:ccd58f78b5cb 58 ledb = true;
JornD 8:ccd58f78b5cb 59 break;
JornD 8:ccd58f78b5cb 60 case 'b':
JornD 8:ccd58f78b5cb 61 ledr = true;
JornD 8:ccd58f78b5cb 62 ledg = true;
JordanO 13:f90e31c6af2b 63 ledb = !ledb;
JornD 8:ccd58f78b5cb 64 break;
JornD 8:ccd58f78b5cb 65 case 'g':
JornD 8:ccd58f78b5cb 66 ledr = true;
JordanO 13:f90e31c6af2b 67 ledg = !ledg;
JornD 8:ccd58f78b5cb 68 ledb = true;
JordanO 13:f90e31c6af2b 69 break;
JornD 8:ccd58f78b5cb 70 case 't':
JornD 8:ccd58f78b5cb 71 ledr = true;
JordanO 13:f90e31c6af2b 72 ledg = !ledg;
JordanO 13:f90e31c6af2b 73 ledb = !ledb;
JornD 8:ccd58f78b5cb 74 break;
JornD 8:ccd58f78b5cb 75 case 'p':
JordanO 13:f90e31c6af2b 76 ledr = !ledr;
JornD 8:ccd58f78b5cb 77 ledg = true;
JordanO 13:f90e31c6af2b 78 ledb = !ledb;
JornD 8:ccd58f78b5cb 79 break;
JordanO 13:f90e31c6af2b 80 default:
JordanO 13:f90e31c6af2b 81 errorCode = 1;
JordanO 14:1a695fc45fc6 82 CurrentState = FailState;
JornD 8:ccd58f78b5cb 83 }
JordanO 13:f90e31c6af2b 84 pc.printf("Color %c \r\n",ledcolor);
JornD 8:ccd58f78b5cb 85 }
JornD 8:ccd58f78b5cb 86
JordanO 14:1a695fc45fc6 87 // SW2 = button1 state changing
JordanO 11:32deb48774f7 88 void Run_StateChangerButton1()
JordanO 11:32deb48774f7 89 {
JordanO 11:32deb48774f7 90 switch(CurrentState)
JordanO 11:32deb48774f7 91 {
JordanO 14:1a695fc45fc6 92 case Startup: //From
JordanO 14:1a695fc45fc6 93 CurrentState = CalibrationPhysical; //To
JordanO 14:1a695fc45fc6 94 break; //Break from switch
JordanO 11:32deb48774f7 95 case CalibrationPhysical:
JordanO 11:32deb48774f7 96 CurrentState = CalibrationIdle;
JordanO 11:32deb48774f7 97 break;
JordanO 11:32deb48774f7 98 case CalibrationIdle:
JordanO 11:32deb48774f7 99 CurrentState = CalibrationEMG;
JordanO 11:32deb48774f7 100 break;
JordanO 11:32deb48774f7 101 case CalibrationEMG:
JordanO 11:32deb48774f7 102 CurrentState = MovementIdle;
JordanO 11:32deb48774f7 103 break;
JordanO 11:32deb48774f7 104 case MovementIdle:
JordanO 11:32deb48774f7 105 CurrentState = TiltCup;
JordanO 11:32deb48774f7 106 break;
JordanO 11:32deb48774f7 107 case TiltCup:
JordanO 11:32deb48774f7 108 CurrentState = MovementIdle;
JordanO 11:32deb48774f7 109 break;
JordanO 11:32deb48774f7 110 case Move:
JordanO 11:32deb48774f7 111 CurrentState = MovementIdle;
JordanO 11:32deb48774f7 112 break;
JordanO 11:32deb48774f7 113 }
JordanO 11:32deb48774f7 114 }
JordanO 14:1a695fc45fc6 115 // SW3 = button2 state changing
JornD 15:95034d92bc76 116 void Run_StateChangerButton2(void)
JordanO 12:d13ce121a781 117 {
JordanO 12:d13ce121a781 118 switch(CurrentState)
JordanO 12:d13ce121a781 119 {
JordanO 13:f90e31c6af2b 120 case CalibrationIdle:
JordanO 13:f90e31c6af2b 121 CurrentState = Demo;
JordanO 13:f90e31c6af2b 122 break;
JordanO 13:f90e31c6af2b 123 case MovementIdle:
JordanO 13:f90e31c6af2b 124 CurrentState = Move;
JordanO 13:f90e31c6af2b 125 break;
JordanO 13:f90e31c6af2b 126 case TiltCup:
JordanO 13:f90e31c6af2b 127 CurrentState = MovementIdle;
JordanO 13:f90e31c6af2b 128 break;
JordanO 12:d13ce121a781 129 }
JordanO 12:d13ce121a781 130 }
JordanO 12:d13ce121a781 131
JordanO 14:1a695fc45fc6 132
JordanO 14:1a695fc45fc6 133 //State functions
JornD 2:6d026d5655c5 134 void Run_Demo(void)
JornD 2:6d026d5655c5 135 {
JordanO 11:32deb48774f7 136 pc.printf("Starting Demo ...\r\n");
JornD 5:77929f5f1f32 137 }
JornD 2:6d026d5655c5 138
JornD 5:77929f5f1f32 139 void Run_MovementIdle(void)
JornD 5:77929f5f1f32 140 {
JordanO 11:32deb48774f7 141 pc.printf("Starting Idle ...\r\n");
JornD 5:77929f5f1f32 142 }
JornD 5:77929f5f1f32 143
JornD 5:77929f5f1f32 144 void Run_CalibrationIdle(void)
JornD 5:77929f5f1f32 145 {
JordanO 11:32deb48774f7 146 pc.printf("Starting Calibration Idle ...\r\n");
JornD 5:77929f5f1f32 147 }
JornD 5:77929f5f1f32 148
JornD 5:77929f5f1f32 149 void Run_Startup(void)
JornD 5:77929f5f1f32 150 {
JordanO 11:32deb48774f7 151 pc.printf("Starting Startup ...\r\n");
JornD 2:6d026d5655c5 152 }
JornD 2:6d026d5655c5 153
JornD 5:77929f5f1f32 154 void Run_CalibrationPhysical(void)
JornD 2:6d026d5655c5 155 {
JordanO 11:32deb48774f7 156 pc.printf("Starting Calibration Physical ... \r\n");
JordanO 13:f90e31c6af2b 157 wait(1);
JordanO 11:32deb48774f7 158 CurrentState = CalibrationIdle;
JordanO 11:32deb48774f7 159
JornD 5:77929f5f1f32 160 }
JornD 2:6d026d5655c5 161
JornD 5:77929f5f1f32 162 void Run_CalibrationEMG(void)
JornD 5:77929f5f1f32 163 {
JordanO 11:32deb48774f7 164 pc.printf("Starting Calibration EMG ... \r\n");
JornD 2:6d026d5655c5 165 }
JornD 2:6d026d5655c5 166
JornD 5:77929f5f1f32 167 void Run_Move(void)
JornD 5:77929f5f1f32 168 {
JordanO 13:f90e31c6af2b 169 pc.printf("Starting Move ... \r\n");
JornD 5:77929f5f1f32 170 }
JornD 5:77929f5f1f32 171
JornD 5:77929f5f1f32 172 void Run_TiltCup(void)
JornD 5:77929f5f1f32 173 {
JordanO 11:32deb48774f7 174 pc.printf("Starting Calibration TiltCup ... \r\n");
JornD 5:77929f5f1f32 175 }
JornD 5:77929f5f1f32 176
JornD 5:77929f5f1f32 177 void Run_FailState(void)
Rieks 6:41b02057fc2c 178 {
JordanO 14:1a695fc45fc6 179 pc.printf("Error: %i",errorCode);
JornD 5:77929f5f1f32 180 }
JornD 5:77929f5f1f32 181
JordanO 11:32deb48774f7 182
JornD 5:77929f5f1f32 183 //State Machine
JornD 2:6d026d5655c5 184 void StateMachine(void)
JornD 2:6d026d5655c5 185 {
JordanO 13:f90e31c6af2b 186 //Turn off all LEDs
JornD 2:6d026d5655c5 187 switch(CurrentState)
JornD 2:6d026d5655c5 188 {
JornD 2:6d026d5655c5 189 case Demo:
JordanO 13:f90e31c6af2b 190 ledcolor='t';
JordanO 11:32deb48774f7 191 Run_Demo();
JornD 2:6d026d5655c5 192 break;
JornD 5:77929f5f1f32 193 case MovementIdle:
JordanO 13:f90e31c6af2b 194 ledcolor='b';
JordanO 11:32deb48774f7 195 Run_MovementIdle();
JornD 5:77929f5f1f32 196 break;
JornD 5:77929f5f1f32 197 case CalibrationIdle:
JordanO 13:f90e31c6af2b 198 ledcolor='b';
JordanO 11:32deb48774f7 199 Run_CalibrationIdle();
JornD 5:77929f5f1f32 200 break;
JornD 9:cb88b16a97d5 201 case Startup:
JordanO 13:f90e31c6af2b 202 ledcolor='b';
JordanO 11:32deb48774f7 203 Run_Startup();
JornD 5:77929f5f1f32 204 break;
JornD 5:77929f5f1f32 205 case CalibrationPhysical:
Rieks 7:af0c16ae48d7 206 ledcolor='g';
JordanO 11:32deb48774f7 207 Run_CalibrationPhysical();
JornD 5:77929f5f1f32 208 break;
JornD 5:77929f5f1f32 209 case CalibrationEMG:
Rieks 7:af0c16ae48d7 210 ledcolor='g';
JordanO 11:32deb48774f7 211 Run_CalibrationEMG();
JornD 5:77929f5f1f32 212 break;
JornD 5:77929f5f1f32 213 case Move:
Rieks 7:af0c16ae48d7 214 ledcolor='p';
JordanO 11:32deb48774f7 215 Run_Move();
JornD 5:77929f5f1f32 216 break;
JornD 5:77929f5f1f32 217 case TiltCup:
Rieks 7:af0c16ae48d7 218 ledcolor='t';
JordanO 11:32deb48774f7 219 Run_TiltCup();
JornD 5:77929f5f1f32 220 break;
JornD 5:77929f5f1f32 221 case FailState:
JordanO 13:f90e31c6af2b 222 ledcolor='r';
JordanO 11:32deb48774f7 223 Run_FailState();
JornD 2:6d026d5655c5 224 break;
JordanO 12:d13ce121a781 225 default:
JordanO 14:1a695fc45fc6 226 CurrentState = FailState;
JordanO 14:1a695fc45fc6 227 errorCode = 2;
JordanO 12:d13ce121a781 228 break;
JornD 2:6d026d5655c5 229 }
JornD 2:6d026d5655c5 230 }
JornD 1:a5c1140f464a 231
JornD 5:77929f5f1f32 232 //Main Loop
JornD 8:ccd58f78b5cb 233 void mainloop()
JornD 8:ccd58f78b5cb 234 {
JornD 9:cb88b16a97d5 235 StateMachine();
JornD 8:ccd58f78b5cb 236 }
JornD 8:ccd58f78b5cb 237
JornD 1:a5c1140f464a 238 int main()
JornD 8:ccd58f78b5cb 239 {
JornD 8:ccd58f78b5cb 240 //Initialize
JordanO 13:f90e31c6af2b 241 ledr = true;
JordanO 13:f90e31c6af2b 242 //ledr.write(.4);
JordanO 13:f90e31c6af2b 243 ledg = true;
JordanO 13:f90e31c6af2b 244 //ledg.write(.4);
JordanO 13:f90e31c6af2b 245 ledb = true;
JordanO 13:f90e31c6af2b 246 //ledb.write(.4);
JordanO 11:32deb48774f7 247
JordanO 11:32deb48774f7 248 pc.baud(115200);
JornD 5:77929f5f1f32 249 CurrentState = Startup;
JornD 9:cb88b16a97d5 250
JordanO 11:32deb48774f7 251 button1.mode(PullUp);
JordanO 11:32deb48774f7 252 button1.rise(Run_StateChangerButton1);
JordanO 11:32deb48774f7 253 button2.mode(PullUp);
JordanO 12:d13ce121a781 254 button2.rise(Run_StateChangerButton2);
JordanO 12:d13ce121a781 255
JordanO 13:f90e31c6af2b 256 //Tickers
JordanO 14:1a695fc45fc6 257 Main_Ticker.attach(mainloop,mainLoopT);
JordanO 14:1a695fc45fc6 258 wait(mainLoopT);
JordanO 14:1a695fc45fc6 259 Tick_Blinky.attach(FlipLED,ledFlipperT);
JornD 15:95034d92bc76 260
JornD 15:95034d92bc76 261 double PlantError = 1;
JornD 15:95034d92bc76 262 double k = 1;
JornD 15:95034d92bc76 263 double u = ControllerPID(PlantError, k);
JordanO 11:32deb48774f7 264
JornD 1:a5c1140f464a 265 while(true)
JornD 1:a5c1140f464a 266 {
JornD 8:ccd58f78b5cb 267
JornD 1:a5c1140f464a 268 }
JornD 1:a5c1140f464a 269 }