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@38:3ff3cdb8740d, 2019-10-10 (annotated)

Committer:
JordanO
Date:
Thu Oct 10 15:42:26 2019 +0000
Revision:
38:3ff3cdb8740d
Parent:
37:6602655a80f4 
CalibrationPysical written

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