M9 Biorobotics Group 8 / Mbed 2 deprecated CARD_program

De hele robot in 1 keer bam

Dependencies:   mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@37:806c7c8381a7, 2019-10-30 (annotated)

Committer:
Jellehierck
Date:
Wed Oct 30 14:13:27 2019 +0000
Revision:
37:806c7c8381a7
Parent:
36:ec2bb2a02856
Child:
38:8b597ab8344f
First backbone of global state machine, working and tested

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jellehierck 37:806c7c8381a7 1 /*
Jellehierck 37:806c7c8381a7 2 ------------------------------ ADD LIBRARIES ------------------------------
Jellehierck 37:806c7c8381a7 3 */
IsaRobin 0:6972d0e91af1 4 #include "mbed.h" //Base library
Jellehierck 8:ea3de43c9e8b 5 #include "MODSERIAL.h"// in order for connection with the pc
IsaRobin 0:6972d0e91af1 6
Jellehierck 15:421d3d9c563b 7 /*
Jellehierck 37:806c7c8381a7 8 ------------------------------ DEFINE MBED CONNECTIONS ------------------------------
Jellehierck 15:421d3d9c563b 9 */
IsaRobin 0:6972d0e91af1 10
Jellehierck 15:421d3d9c563b 11 // PC serial connection
Jellehierck 15:421d3d9c563b 12 MODSERIAL pc(USBTX, USBRX);
IsaRobin 0:6972d0e91af1 13
Jellehierck 8:ea3de43c9e8b 14 // Buttons
Jellehierck 8:ea3de43c9e8b 15 InterruptIn button1(D11);
Jellehierck 8:ea3de43c9e8b 16 InterruptIn button2(D10);
Jellehierck 37:806c7c8381a7 17 InterruptIn switch2(SW2);
Jellehierck 37:806c7c8381a7 18 InterruptIn switch3(SW3);
Jellehierck 4:09a01d2db8f7 19
Jellehierck 15:421d3d9c563b 20 /*
Jellehierck 37:806c7c8381a7 21 ------------------------------ GLOBAL VARIABLES ------------------------------
Jellehierck 15:421d3d9c563b 22 */
Jellehierck 15:421d3d9c563b 23
Jellehierck 37:806c7c8381a7 24 // State machine variables
Jellehierck 37:806c7c8381a7 25 enum GLOBAL_States { global_failure, global_wait, global_cal_emg, global_cal_motor, global_operation, global_demo }; // Define global states
Jellehierck 37:806c7c8381a7 26 GLOBAL_States global_curr_state = global_wait; // Initialize global state to waiting state
Jellehierck 37:806c7c8381a7 27 bool global_state_changed = true; // Enable entry functions
Jellehierck 37:806c7c8381a7 28 bool failure_mode = false;
Jellehierck 35:e82834e62e44 29
Jellehierck 37:806c7c8381a7 30 bool cal_emg_done = false;
Jellehierck 37:806c7c8381a7 31 bool cal_motor_done = false;
Jellehierck 8:ea3de43c9e8b 32
Jellehierck 37:806c7c8381a7 33 // Button press interrupts (to prevent bounce)
Jellehierck 37:806c7c8381a7 34 bool button1_pressed = false;
Jellehierck 37:806c7c8381a7 35 bool button2_pressed = false;
Jellehierck 37:806c7c8381a7 36 bool switch2_pressed = false;
Jellehierck 7:7a088536f1c9 37
Jellehierck 37:806c7c8381a7 38 // Global program variables
Jellehierck 37:806c7c8381a7 39 double Fs = 500.0;
Jellehierck 37:806c7c8381a7 40 double Ts = 1/Fs;
Jellehierck 26:7e81c7db6e7a 41
Jellehierck 37:806c7c8381a7 42 double Tcal_test = 5.0;
Jellehierck 35:e82834e62e44 43
Jellehierck 35:e82834e62e44 44 /*
Jellehierck 37:806c7c8381a7 45 ------------------------------ HELPER FUNCTIONS ------------------------------
Jellehierck 37:806c7c8381a7 46 */
Jellehierck 37:806c7c8381a7 47 void doStuff() {} // Empty placeholder function, needs to be deleted at end of project
Jellehierck 37:806c7c8381a7 48
Jellehierck 37:806c7c8381a7 49
Jellehierck 37:806c7c8381a7 50
Jellehierck 37:806c7c8381a7 51 /*
Jellehierck 37:806c7c8381a7 52 ------------------------------ BUTTON FUNCTIONS ------------------------------
Jellehierck 35:e82834e62e44 53 */
Jellehierck 35:e82834e62e44 54
Jellehierck 25:a1be4cf2ab0b 55 // Handle button press
Jellehierck 25:a1be4cf2ab0b 56 void button1Press()
Jellehierck 25:a1be4cf2ab0b 57 {
Jellehierck 25:a1be4cf2ab0b 58 button1_pressed = true;
Jellehierck 25:a1be4cf2ab0b 59 }
Jellehierck 25:a1be4cf2ab0b 60
Jellehierck 25:a1be4cf2ab0b 61 // Handle button press
Jellehierck 25:a1be4cf2ab0b 62 void button2Press()
Jellehierck 25:a1be4cf2ab0b 63 {
Jellehierck 25:a1be4cf2ab0b 64 button2_pressed = true;
Jellehierck 25:a1be4cf2ab0b 65 }
Jellehierck 25:a1be4cf2ab0b 66
Jellehierck 37:806c7c8381a7 67 void switch2Press()
Jellehierck 6:5437cc97e1e6 68 {
Jellehierck 37:806c7c8381a7 69 switch2_pressed = true;
Jellehierck 35:e82834e62e44 70 }
Jellehierck 6:5437cc97e1e6 71
Jellehierck 37:806c7c8381a7 72 void switch3Press()
Jellehierck 35:e82834e62e44 73 {
Jellehierck 37:806c7c8381a7 74 global_curr_state = global_failure;
Jellehierck 37:806c7c8381a7 75 global_state_changed = true;
Jellehierck 6:5437cc97e1e6 76 }
Jellehierck 6:5437cc97e1e6 77
Jellehierck 15:421d3d9c563b 78 /*
Jellehierck 37:806c7c8381a7 79 ------------------------------ TICKER, TIMER & TIMEOUT FUNCTIONS ------------------------------
Jellehierck 15:421d3d9c563b 80 */
Jellehierck 37:806c7c8381a7 81 // Initialize tickers and timeouts
Jellehierck 37:806c7c8381a7 82 Ticker tickGlobal; // Set global ticker
Jellehierck 37:806c7c8381a7 83 Timer timerStateMachineTest; // Set testing timer
Jellehierck 7:7a088536f1c9 84
Jellehierck 15:421d3d9c563b 85 /*
Jellehierck 37:806c7c8381a7 86 ------------------------------ GLOBAL STATE FUNCTIONS ------------------------------
Jellehierck 15:421d3d9c563b 87 */
Jellehierck 25:a1be4cf2ab0b 88 /* ALL STATES HAVE THE FOLLOWING FORM:
Jellehierck 25:a1be4cf2ab0b 89 void do_state_function() {
Jellehierck 25:a1be4cf2ab0b 90 // Entry function
Jellehierck 37:806c7c8381a7 91 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 92 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 93 // More functions
Jellehierck 25:a1be4cf2ab0b 94 }
Jellehierck 25:a1be4cf2ab0b 95
Jellehierck 25:a1be4cf2ab0b 96 // Do stuff until end condition is met
Jellehierck 25:a1be4cf2ab0b 97 doStuff();
Jellehierck 25:a1be4cf2ab0b 98
Jellehierck 25:a1be4cf2ab0b 99 // State transition guard
Jellehierck 25:a1be4cf2ab0b 100 if ( endCondition == true ) {
Jellehierck 37:806c7c8381a7 101 global_curr_state = next_state;
Jellehierck 37:806c7c8381a7 102 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 103 // More functions
Jellehierck 25:a1be4cf2ab0b 104 }
Jellehierck 25:a1be4cf2ab0b 105 }
Jellehierck 25:a1be4cf2ab0b 106 */
Jellehierck 25:a1be4cf2ab0b 107
Jellehierck 37:806c7c8381a7 108 // FAILURE MODE
Jellehierck 37:806c7c8381a7 109 void do_global_failure()
Jellehierck 7:7a088536f1c9 110 {
Jellehierck 37:806c7c8381a7 111 // Entry function
Jellehierck 37:806c7c8381a7 112 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 113 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 114
Jellehierck 37:806c7c8381a7 115 failure_mode = true; // Set failure mode
Jellehierck 22:9079c6c0d898 116 }
Jellehierck 37:806c7c8381a7 117
Jellehierck 37:806c7c8381a7 118 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 119
Jellehierck 37:806c7c8381a7 120 // State transition guard
Jellehierck 37:806c7c8381a7 121 if ( false ) { // Never move to other state
Jellehierck 37:806c7c8381a7 122 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 123 global_state_changed = true;
Jellehierck 37:806c7c8381a7 124 }
Jellehierck 25:a1be4cf2ab0b 125 }
Jellehierck 25:a1be4cf2ab0b 126
Jellehierck 37:806c7c8381a7 127 // DEMO MODE
Jellehierck 37:806c7c8381a7 128 void do_global_demo()
Jellehierck 25:a1be4cf2ab0b 129 {
Jellehierck 25:a1be4cf2ab0b 130 // Entry function
Jellehierck 37:806c7c8381a7 131 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 132 global_state_changed = false;
Jellehierck 37:806c7c8381a7 133 // More functions
Jellehierck 37:806c7c8381a7 134 }
Jellehierck 37:806c7c8381a7 135
Jellehierck 37:806c7c8381a7 136 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 137 doStuff();
Jellehierck 35:e82834e62e44 138
Jellehierck 37:806c7c8381a7 139 // State transition guard
Jellehierck 37:806c7c8381a7 140 if ( switch2_pressed == true ) {
Jellehierck 37:806c7c8381a7 141 switch2_pressed = false;
Jellehierck 37:806c7c8381a7 142 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 143 global_state_changed = true;
Jellehierck 37:806c7c8381a7 144 }
Jellehierck 37:806c7c8381a7 145 }
Jellehierck 37:806c7c8381a7 146
Jellehierck 37:806c7c8381a7 147 // WAIT MODE
Jellehierck 37:806c7c8381a7 148 void do_global_wait()
Jellehierck 37:806c7c8381a7 149 {
Jellehierck 37:806c7c8381a7 150 // Entry function
Jellehierck 37:806c7c8381a7 151 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 152 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 153 }
Jellehierck 25:a1be4cf2ab0b 154
Jellehierck 27:f18da01093c9 155 // Do nothing until end condition is met
Jellehierck 25:a1be4cf2ab0b 156
Jellehierck 37:806c7c8381a7 157 // State transition guard
Jellehierck 37:806c7c8381a7 158 if ( switch2_pressed == true ) { // DEMO MODE
Jellehierck 37:806c7c8381a7 159 switch2_pressed = false;
Jellehierck 37:806c7c8381a7 160 global_curr_state = global_demo;
Jellehierck 37:806c7c8381a7 161 global_state_changed = true;
Jellehierck 31:b5188b6d45db 162
Jellehierck 37:806c7c8381a7 163 } else if ( button1_pressed == true ) { // EMG CALIBRATION
Jellehierck 37:806c7c8381a7 164 button1_pressed = false;
Jellehierck 37:806c7c8381a7 165 global_curr_state = global_cal_emg;
Jellehierck 37:806c7c8381a7 166 global_state_changed = true;
Jellehierck 31:b5188b6d45db 167
Jellehierck 37:806c7c8381a7 168 } else if ( button2_pressed == true ) { // MOTOR CALIBRATION
Jellehierck 37:806c7c8381a7 169 button2_pressed = false;
Jellehierck 37:806c7c8381a7 170 global_curr_state = global_cal_motor;
Jellehierck 37:806c7c8381a7 171 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 172 }
Jellehierck 7:7a088536f1c9 173 }
Jellehierck 7:7a088536f1c9 174
Jellehierck 37:806c7c8381a7 175 // EMG CALIBRATION MODE
Jellehierck 37:806c7c8381a7 176 void do_global_cal_emg()
Jellehierck 21:e4569b47945e 177 {
Jellehierck 37:806c7c8381a7 178 // Entry function
Jellehierck 37:806c7c8381a7 179 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 180 global_state_changed = false;
Jellehierck 22:9079c6c0d898 181 }
Jellehierck 7:7a088536f1c9 182
Jellehierck 31:b5188b6d45db 183 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 184 doStuff();
Jellehierck 31:b5188b6d45db 185
Jellehierck 29:f51683a6cbbf 186 // State transition guard
Jellehierck 37:806c7c8381a7 187 if ( cal_motor_done == true ) { // OPERATION MODE
Jellehierck 37:806c7c8381a7 188 cal_emg_done = true;
Jellehierck 37:806c7c8381a7 189 global_curr_state = global_operation;
Jellehierck 37:806c7c8381a7 190 global_state_changed = true;
Jellehierck 37:806c7c8381a7 191 } else if ( button1_pressed == true ) { // WAIT MODE
Jellehierck 37:806c7c8381a7 192 button1_pressed = false;
Jellehierck 37:806c7c8381a7 193 cal_emg_done = true;
Jellehierck 37:806c7c8381a7 194 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 195 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 196 }
Jellehierck 25:a1be4cf2ab0b 197 }
Jellehierck 23:8a0a0b959af1 198
Jellehierck 37:806c7c8381a7 199 // MOTOR CALIBRATION MODE
Jellehierck 37:806c7c8381a7 200 void do_global_cal_motor()
Jellehierck 26:7e81c7db6e7a 201 {
Jellehierck 25:a1be4cf2ab0b 202 // Entry function
Jellehierck 37:806c7c8381a7 203 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 204 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 205 }
Jellehierck 25:a1be4cf2ab0b 206
Jellehierck 25:a1be4cf2ab0b 207 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 208 doStuff();
Jellehierck 28:59e8266f4633 209
Jellehierck 25:a1be4cf2ab0b 210 // State transition guard
Jellehierck 37:806c7c8381a7 211 if ( cal_emg_done == true ) { // OPERATION MODE
Jellehierck 37:806c7c8381a7 212 cal_motor_done = true;
Jellehierck 37:806c7c8381a7 213 global_curr_state = global_operation;
Jellehierck 37:806c7c8381a7 214 global_state_changed = true;
Jellehierck 37:806c7c8381a7 215 } else if ( button2_pressed == true ) { // WAIT MODE
Jellehierck 37:806c7c8381a7 216 button2_pressed = false;
Jellehierck 37:806c7c8381a7 217 cal_motor_done = true;
Jellehierck 37:806c7c8381a7 218 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 219 global_state_changed = true;
Jellehierck 23:8a0a0b959af1 220 }
Jellehierck 23:8a0a0b959af1 221 }
Jellehierck 23:8a0a0b959af1 222
Jellehierck 37:806c7c8381a7 223 // OPERATION MODE
Jellehierck 37:806c7c8381a7 224 void do_global_operation()
Jellehierck 37:806c7c8381a7 225 {
Jellehierck 37:806c7c8381a7 226 // Entry function
Jellehierck 37:806c7c8381a7 227 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 228 global_state_changed = false;
Jellehierck 37:806c7c8381a7 229 }
Jellehierck 37:806c7c8381a7 230
Jellehierck 37:806c7c8381a7 231 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 232 doStuff();
Jellehierck 37:806c7c8381a7 233
Jellehierck 37:806c7c8381a7 234 // State transition guard
Jellehierck 37:806c7c8381a7 235 if ( false ) { // Always stay in operation mode (can be changed)
Jellehierck 37:806c7c8381a7 236 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 237 global_state_changed = true;
Jellehierck 37:806c7c8381a7 238 }
Jellehierck 37:806c7c8381a7 239 }
Jellehierck 23:8a0a0b959af1 240 /*
Jellehierck 37:806c7c8381a7 241 ------------------------------ GLOBAL STATE MACHINE ------------------------------
Jellehierck 23:8a0a0b959af1 242 */
Jellehierck 37:806c7c8381a7 243 void global_state_machine()
Jellehierck 23:8a0a0b959af1 244 {
Jellehierck 37:806c7c8381a7 245 switch(global_curr_state) {
Jellehierck 37:806c7c8381a7 246 case global_failure:
Jellehierck 37:806c7c8381a7 247 do_global_failure();
Jellehierck 23:8a0a0b959af1 248 break;
Jellehierck 37:806c7c8381a7 249 case global_wait:
Jellehierck 37:806c7c8381a7 250 do_global_wait();
Jellehierck 37:806c7c8381a7 251 break;
Jellehierck 37:806c7c8381a7 252 case global_cal_emg:
Jellehierck 37:806c7c8381a7 253 do_global_cal_emg();
Jellehierck 23:8a0a0b959af1 254 break;
Jellehierck 37:806c7c8381a7 255 case global_cal_motor:
Jellehierck 37:806c7c8381a7 256 do_global_cal_motor();
Jellehierck 23:8a0a0b959af1 257 break;
Jellehierck 37:806c7c8381a7 258 case global_operation:
Jellehierck 37:806c7c8381a7 259 do_global_operation();
Jellehierck 37:806c7c8381a7 260 break;
Jellehierck 37:806c7c8381a7 261 case global_demo:
Jellehierck 37:806c7c8381a7 262 do_global_demo();
Jellehierck 23:8a0a0b959af1 263 break;
Jellehierck 23:8a0a0b959af1 264 }
Jellehierck 23:8a0a0b959af1 265 }
Jellehierck 23:8a0a0b959af1 266
Jellehierck 37:806c7c8381a7 267
Jellehierck 37:806c7c8381a7 268 /*
Jellehierck 37:806c7c8381a7 269 ------------------------------ GLOBAL PROGRAM LOOP ------------------------------
Jellehierck 37:806c7c8381a7 270 */
Jellehierck 25:a1be4cf2ab0b 271 void tickGlobalFunc()
Jellehierck 25:a1be4cf2ab0b 272 {
Jellehierck 37:806c7c8381a7 273 // sampleSignalsAndInputs();
Jellehierck 37:806c7c8381a7 274 global_state_machine();
Jellehierck 25:a1be4cf2ab0b 275 // controller();
Jellehierck 25:a1be4cf2ab0b 276 // outputToMotors();
Jellehierck 25:a1be4cf2ab0b 277 }
Jellehierck 25:a1be4cf2ab0b 278
Jellehierck 37:806c7c8381a7 279 /*
Jellehierck 37:806c7c8381a7 280 ------------------------------ MAIN FUNCTION ------------------------------
Jellehierck 37:806c7c8381a7 281 */
Jellehierck 23:8a0a0b959af1 282 void main()
Jellehierck 23:8a0a0b959af1 283 {
Jellehierck 23:8a0a0b959af1 284 pc.baud(115200); // MODSERIAL rate
Jellehierck 23:8a0a0b959af1 285 pc.printf("Starting\r\n");
Jellehierck 23:8a0a0b959af1 286
Jellehierck 37:806c7c8381a7 287 global_curr_state = global_wait; // Start off in EMG Wait state
Jellehierck 34:13fac02ef324 288 tickGlobal.attach( &tickGlobalFunc, Ts ); // Start global ticker
Jellehierck 8:ea3de43c9e8b 289
Jellehierck 37:806c7c8381a7 290 button1.fall( &button1Press );
Jellehierck 37:806c7c8381a7 291 button2.fall( &button2Press );
Jellehierck 37:806c7c8381a7 292 switch2.fall( &switch2Press );
Jellehierck 37:806c7c8381a7 293 switch3.fall( &switch3Press );
Jellehierck 37:806c7c8381a7 294
Jellehierck 23:8a0a0b959af1 295 while(true) {
Jellehierck 37:806c7c8381a7 296 pc.printf("Global state: %i \r\n", global_curr_state);
Jellehierck 30:bac3b60d6283 297 wait(0.5f);
Jellehierck 23:8a0a0b959af1 298 }
Jellehierck 23:8a0a0b959af1 299 }