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@27:71be6e074d0f, 2019-10-09 (annotated)

Committer:
JordanO
Date:
Wed Oct 09 12:56:35 2019 +0000
Revision:
27:71be6e074d0f
Parent:
24:d0ca537913c2
Child:
28:4a5671b3d88d
struct;

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 8:ccd58f78b5cb 10
JordanO 10:ad2da21a102c 11 //Objects
JordanO 10:ad2da21a102c 12 //LED
JordanO 10:ad2da21a102c 13 DigitalOut ledb(LED_BLUE); // ledb=true & ledb=1 is led off!!
JordanO 10:ad2da21a102c 14 DigitalOut ledg(LED_GREEN);
JordanO 10:ad2da21a102c 15 DigitalOut ledr(LED_RED);
JordanO 10:ad2da21a102c 16 //Motors
JordanO 10:ad2da21a102c 17
JordanO 10:ad2da21a102c 18 //Sensors
JordanO 14:1a695fc45fc6 19 //QEI Encoder1(D12,D13,NC,64,QEI::X2_ENCODING);
JordanO 14:1a695fc45fc6 20 //QEI Encoder2(D14,D15,NC,64,QEI::X2_ENCODING);
JordanO 14:1a695fc45fc6 21 //QEI Encoder3(D16,D17,NC,64,QEI::X2_ENCODING);
JordanO 10:ad2da21a102c 22
JordanO 10:ad2da21a102c 23 //Buttons
JordanO 11:32deb48774f7 24 InterruptIn button1(SW2); //button on the side of the reset button
JordanO 11:32deb48774f7 25 InterruptIn button2(SW3); //button on the side opposite of the reset button
JordanO 11:32deb48774f7 26
JordanO 11:32deb48774f7 27 //PC
JornD 15:95034d92bc76 28 Serial pc(USBTX,USBRX);
JornD 8:ccd58f78b5cb 29
JordanO 10:ad2da21a102c 30 //Variables
JordanO 10:ad2da21a102c 31 enum States {MovementIdle, CalibrationIdle, Demo, Startup, CalibrationPhysical, CalibrationEMG, Move, TiltCup, FailState};
JordanO 10:ad2da21a102c 32 States CurrentState;
JordanO 10:ad2da21a102c 33
JornD 23:767911637f3a 34 extern double X;
JornD 23:767911637f3a 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
JornD 24:d0ca537913c2 39 //Ticker Timings
JornD 24:d0ca537913c2 40 const float mainLoopT = 2; //Main Loopt Ticker wait
JornD 24:d0ca537913c2 41 const float ledFlipperT = .5; //LED Flicker wait
JordanO 14:1a695fc45fc6 42
JordanO 10:ad2da21a102c 43 //Tickers
JordanO 10:ad2da21a102c 44 Ticker Main_Ticker;
JordanO 10:ad2da21a102c 45 Ticker Tick_Blinky;//used for the blinking of the leds
JordanO 27:71be6e074d0f 46
JordanO 27:71be6e074d0f 47 struct motorReturnSub
JordanO 27:71be6e074d0f 48 {
JordanO 27:71be6e074d0f 49 int counts;
JordanO 27:71be6e074d0f 50 float angle;
JordanO 27:71be6e074d0f 51 float velocity;
JordanO 27:71be6e074d0f 52 };
JordanO 27:71be6e074d0f 53
JordanO 27:71be6e074d0f 54 struct motorReturn
JordanO 27:71be6e074d0f 55 {
JordanO 27:71be6e074d0f 56 motorReturnSub motor1;
JordanO 27:71be6e074d0f 57 motorReturnSub motor2;
JordanO 27:71be6e074d0f 58 motorReturnSub motor3;
JordanO 27:71be6e074d0f 59 } motorReturn;
JordanO 10:ad2da21a102c 60
JornD 24:d0ca537913c2 61 //Changing LED colour depending on current state
JornD 8:ccd58f78b5cb 62 void FlipLED(void)
JornD 8:ccd58f78b5cb 63 {
JordanO 13:f90e31c6af2b 64 pc.printf("FLIPLED \r\n");
JornD 8:ccd58f78b5cb 65 switch(ledcolor)
JornD 8:ccd58f78b5cb 66 {
JornD 8:ccd58f78b5cb 67 case 'r':
JordanO 13:f90e31c6af2b 68 ledr = !ledr;
JornD 8:ccd58f78b5cb 69 ledg = true;
JornD 8:ccd58f78b5cb 70 ledb = true;
JornD 8:ccd58f78b5cb 71 break;
JornD 8:ccd58f78b5cb 72 case 'b':
JornD 8:ccd58f78b5cb 73 ledr = true;
JornD 8:ccd58f78b5cb 74 ledg = true;
JordanO 13:f90e31c6af2b 75 ledb = !ledb;
JornD 8:ccd58f78b5cb 76 break;
JornD 8:ccd58f78b5cb 77 case 'g':
JornD 8:ccd58f78b5cb 78 ledr = true;
JordanO 13:f90e31c6af2b 79 ledg = !ledg;
JornD 8:ccd58f78b5cb 80 ledb = true;
JordanO 13:f90e31c6af2b 81 break;
JornD 8:ccd58f78b5cb 82 case 't':
JornD 8:ccd58f78b5cb 83 ledr = true;
JordanO 13:f90e31c6af2b 84 ledg = !ledg;
JordanO 13:f90e31c6af2b 85 ledb = !ledb;
JornD 8:ccd58f78b5cb 86 break;
JornD 8:ccd58f78b5cb 87 case 'p':
JordanO 13:f90e31c6af2b 88 ledr = !ledr;
JornD 8:ccd58f78b5cb 89 ledg = true;
JordanO 13:f90e31c6af2b 90 ledb = !ledb;
JornD 8:ccd58f78b5cb 91 break;
JordanO 13:f90e31c6af2b 92 default:
JordanO 13:f90e31c6af2b 93 errorCode = 1;
JordanO 14:1a695fc45fc6 94 CurrentState = FailState;
JornD 8:ccd58f78b5cb 95 }
JordanO 13:f90e31c6af2b 96 pc.printf("Color %c \r\n",ledcolor);
JornD 8:ccd58f78b5cb 97 }
JornD 8:ccd58f78b5cb 98
JornD 24:d0ca537913c2 99 // Testing Placeholder
JordanO 14:1a695fc45fc6 100 // SW2 = button1 state changing
JordanO 11:32deb48774f7 101 void Run_StateChangerButton1()
JordanO 11:32deb48774f7 102 {
JordanO 11:32deb48774f7 103 switch(CurrentState)
JordanO 11:32deb48774f7 104 {
JordanO 14:1a695fc45fc6 105 case Startup: //From
JordanO 14:1a695fc45fc6 106 CurrentState = CalibrationPhysical; //To
JordanO 14:1a695fc45fc6 107 break; //Break from switch
JordanO 11:32deb48774f7 108 case CalibrationPhysical:
JordanO 11:32deb48774f7 109 CurrentState = CalibrationIdle;
JordanO 11:32deb48774f7 110 break;
JordanO 11:32deb48774f7 111 case CalibrationIdle:
JordanO 11:32deb48774f7 112 CurrentState = CalibrationEMG;
JordanO 11:32deb48774f7 113 break;
JordanO 11:32deb48774f7 114 case CalibrationEMG:
JordanO 11:32deb48774f7 115 CurrentState = MovementIdle;
JordanO 11:32deb48774f7 116 break;
JordanO 11:32deb48774f7 117 case MovementIdle:
JordanO 11:32deb48774f7 118 CurrentState = TiltCup;
JordanO 11:32deb48774f7 119 break;
JordanO 11:32deb48774f7 120 case TiltCup:
JordanO 11:32deb48774f7 121 CurrentState = MovementIdle;
JordanO 11:32deb48774f7 122 break;
JordanO 11:32deb48774f7 123 case Move:
JordanO 11:32deb48774f7 124 CurrentState = MovementIdle;
JordanO 11:32deb48774f7 125 break;
JordanO 11:32deb48774f7 126 }
JordanO 11:32deb48774f7 127 }
JornD 24:d0ca537913c2 128
JornD 24:d0ca537913c2 129 // Testing Placeholder
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 //State functions
JornD 2:6d026d5655c5 148 void Run_Demo(void)
JornD 2:6d026d5655c5 149 {
JordanO 11:32deb48774f7 150 pc.printf("Starting Demo ...\r\n");
JornD 5:77929f5f1f32 151 }
JornD 2:6d026d5655c5 152
JornD 5:77929f5f1f32 153 void Run_MovementIdle(void)
JornD 5:77929f5f1f32 154 {
JordanO 11:32deb48774f7 155 pc.printf("Starting Idle ...\r\n");
JornD 5:77929f5f1f32 156 }
JornD 5:77929f5f1f32 157
JornD 5:77929f5f1f32 158 void Run_CalibrationIdle(void)
JornD 5:77929f5f1f32 159 {
JordanO 11:32deb48774f7 160 pc.printf("Starting Calibration Idle ...\r\n");
JornD 5:77929f5f1f32 161 }
JornD 5:77929f5f1f32 162
JornD 5:77929f5f1f32 163 void Run_Startup(void)
JornD 5:77929f5f1f32 164 {
JordanO 11:32deb48774f7 165 pc.printf("Starting Startup ...\r\n");
JornD 2:6d026d5655c5 166 }
JornD 2:6d026d5655c5 167
JornD 5:77929f5f1f32 168 void Run_CalibrationPhysical(void)
JornD 2:6d026d5655c5 169 {
JordanO 11:32deb48774f7 170 pc.printf("Starting Calibration Physical ... \r\n");
JordanO 13:f90e31c6af2b 171 wait(1);
JordanO 11:32deb48774f7 172 CurrentState = CalibrationIdle;
JornD 5:77929f5f1f32 173 }
JornD 2:6d026d5655c5 174
JornD 5:77929f5f1f32 175 void Run_CalibrationEMG(void)
JornD 5:77929f5f1f32 176 {
JordanO 11:32deb48774f7 177 pc.printf("Starting Calibration EMG ... \r\n");
JornD 2:6d026d5655c5 178 }
JornD 2:6d026d5655c5 179
JornD 5:77929f5f1f32 180 void Run_Move(void)
JornD 5:77929f5f1f32 181 {
JordanO 13:f90e31c6af2b 182 pc.printf("Starting Move ... \r\n");
JornD 5:77929f5f1f32 183 }
JornD 5:77929f5f1f32 184
JornD 5:77929f5f1f32 185 void Run_TiltCup(void)
JornD 5:77929f5f1f32 186 {
JordanO 11:32deb48774f7 187 pc.printf("Starting Calibration TiltCup ... \r\n");
JornD 5:77929f5f1f32 188 }
JornD 5:77929f5f1f32 189
JornD 5:77929f5f1f32 190 void Run_FailState(void)
Rieks 6:41b02057fc2c 191 {
JordanO 14:1a695fc45fc6 192 pc.printf("Error: %i",errorCode);
JornD 5:77929f5f1f32 193 }
JornD 5:77929f5f1f32 194
JordanO 11:32deb48774f7 195
JornD 5:77929f5f1f32 196 //State Machine
JornD 2:6d026d5655c5 197 void StateMachine(void)
JornD 2:6d026d5655c5 198 {
JornD 2:6d026d5655c5 199 switch(CurrentState)
JornD 2:6d026d5655c5 200 {
JornD 2:6d026d5655c5 201 case Demo:
JordanO 13:f90e31c6af2b 202 ledcolor='t';
JordanO 11:32deb48774f7 203 Run_Demo();
JornD 2:6d026d5655c5 204 break;
JornD 5:77929f5f1f32 205 case MovementIdle:
JordanO 13:f90e31c6af2b 206 ledcolor='b';
JordanO 11:32deb48774f7 207 Run_MovementIdle();
JornD 5:77929f5f1f32 208 break;
JornD 5:77929f5f1f32 209 case CalibrationIdle:
JordanO 13:f90e31c6af2b 210 ledcolor='b';
JordanO 11:32deb48774f7 211 Run_CalibrationIdle();
JornD 5:77929f5f1f32 212 break;
JornD 9:cb88b16a97d5 213 case Startup:
JordanO 13:f90e31c6af2b 214 ledcolor='b';
JordanO 11:32deb48774f7 215 Run_Startup();
JornD 5:77929f5f1f32 216 break;
JornD 5:77929f5f1f32 217 case CalibrationPhysical:
Rieks 7:af0c16ae48d7 218 ledcolor='g';
JordanO 11:32deb48774f7 219 Run_CalibrationPhysical();
JornD 5:77929f5f1f32 220 break;
JornD 5:77929f5f1f32 221 case CalibrationEMG:
Rieks 7:af0c16ae48d7 222 ledcolor='g';
JordanO 11:32deb48774f7 223 Run_CalibrationEMG();
JornD 5:77929f5f1f32 224 break;
JornD 5:77929f5f1f32 225 case Move:
Rieks 7:af0c16ae48d7 226 ledcolor='p';
JordanO 11:32deb48774f7 227 Run_Move();
JornD 5:77929f5f1f32 228 break;
JornD 5:77929f5f1f32 229 case TiltCup:
Rieks 7:af0c16ae48d7 230 ledcolor='t';
JordanO 11:32deb48774f7 231 Run_TiltCup();
JornD 5:77929f5f1f32 232 break;
JornD 5:77929f5f1f32 233 case FailState:
JordanO 13:f90e31c6af2b 234 ledcolor='r';
JordanO 11:32deb48774f7 235 Run_FailState();
JornD 2:6d026d5655c5 236 break;
JordanO 12:d13ce121a781 237 default:
JordanO 14:1a695fc45fc6 238 CurrentState = FailState;
JordanO 14:1a695fc45fc6 239 errorCode = 2;
JordanO 12:d13ce121a781 240 break;
JornD 2:6d026d5655c5 241 }
JornD 2:6d026d5655c5 242 }
JornD 1:a5c1140f464a 243
JornD 24:d0ca537913c2 244 //Main Loop, constantly running function
JornD 8:ccd58f78b5cb 245 void mainloop()
JornD 8:ccd58f78b5cb 246 {
JornD 9:cb88b16a97d5 247 StateMachine();
JornD 8:ccd58f78b5cb 248 }
JornD 8:ccd58f78b5cb 249
JornD 24:d0ca537913c2 250 //The main function
JornD 1:a5c1140f464a 251 int main()
JornD 8:ccd58f78b5cb 252 {
JornD 24:d0ca537913c2 253 //-Initialize
JornD 24:d0ca537913c2 254 //--Turning LEDs off
JornD 24:d0ca537913c2 255 ledr = true;
JornD 24:d0ca537913c2 256 ledg = true;
JornD 24:d0ca537913c2 257 ledb = true;
JornD 24:d0ca537913c2 258 //--Set baud
JornD 24:d0ca537913c2 259 pc.baud(115200);
JornD 24:d0ca537913c2 260 //--Set first state
JornD 24:d0ca537913c2 261 CurrentState = Startup;
JornD 24:d0ca537913c2 262 //--Setup Buttons (Placeholder for testing)
JordanO 11:32deb48774f7 263 button1.mode(PullUp);
JordanO 11:32deb48774f7 264 button1.rise(Run_StateChangerButton1);
JordanO 11:32deb48774f7 265 button2.mode(PullUp);
JordanO 12:d13ce121a781 266 button2.rise(Run_StateChangerButton2);
JordanO 12:d13ce121a781 267
JornD 24:d0ca537913c2 268 //-Activate Tickers
JordanO 14:1a695fc45fc6 269 Main_Ticker.attach(mainloop,mainLoopT);
JordanO 14:1a695fc45fc6 270 wait(mainLoopT);
JordanO 14:1a695fc45fc6 271 Tick_Blinky.attach(FlipLED,ledFlipperT);
JornD 16:fa8001fef71b 272
JornD 16:fa8001fef71b 273 //Placeholder function call
JornD 24:d0ca537913c2 274 //double PlantError = 1;
JornD 24:d0ca537913c2 275 //double Ts = 1;
JornD 24:d0ca537913c2 276 //double u = ControllerPID(1, 1);
JornD 24:d0ca537913c2 277 //X = 1;
JornD 24:d0ca537913c2 278 //double Y = ProcessEMG(X);
JornD 24:d0ca537913c2 279 int x;
JornD 24:d0ca537913c2 280 int a;
JornD 24:d0ca537913c2 281 a = 3;
JornD 24:d0ca537913c2 282 int b;
JornD 24:d0ca537913c2 283 b = 8;
JornD 24:d0ca537913c2 284 x = AddInt(a,b);
JordanO 11:32deb48774f7 285
JornD 24:d0ca537913c2 286 //-Constantly running function
JornD 1:a5c1140f464a 287 while(true)
JornD 1:a5c1140f464a 288 {
JornD 24:d0ca537913c2 289
JornD 1:a5c1140f464a 290 }
JornD 1:a5c1140f464a 291 }