De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 43:1bd5417ded64
- Parent:
- 42:2937ad8f1032
- Child:
- 44:342af9b3c1a0
diff -r 2937ad8f1032 -r 1bd5417ded64 main.cpp --- a/main.cpp Thu Oct 31 11:04:45 2019 +0000 +++ b/main.cpp Thu Oct 31 11:45:03 2019 +0000 @@ -88,6 +88,11 @@ bool operation_state_changed = true; // Enable entry functions bool operation_showcard = false; // Internal flag to toggle servo position +// Demo Substate variables +enum Demo_States { demo_wait, demo_motor_cal, demo_XY }; // Define demo substates +Demo_States demo_curr_state; // Initialize demo substate variable +bool demo_state_changed = true; // Enable entry functions + // Button press interrupts (to prevent bounce) bool button1_pressed = false; bool button2_pressed = false; @@ -762,17 +767,6 @@ /* ------------------------------ OPERATION GLOBAL FUNCTIONS ------------------------------ */ -void operationFuncBundle() { - EMGOperationFunc(); - - Vx = emg1_out * 15.0f * emg1_dir; - Vy = emg2_out * 15.0f * emg2_dir; - - PID_controller(); - motorControl(); - RKI(); -} - void toggleServo() { if ( operation_showcard == true ) { @@ -800,7 +794,15 @@ } // Do stuff until end condition is met - operationFuncBundle(); // Reads all signals and calculates references + EMGOperationFunc(); + + Vx = emg1_out * 15.0f * emg1_dir; + Vy = emg2_out * 15.0f * emg2_dir; + + PID_controller(); + motorControl(); + RKI(); + motorKillPower(); // Disables motor outputs if ( switch2_pressed == true) { @@ -826,7 +828,14 @@ } // Do stuff until end condition is met - operationFuncBundle(); + EMGOperationFunc(); + + Vx = emg1_out * 15.0f * emg1_dir; + Vy = emg2_out * 15.0f * emg2_dir; + + PID_controller(); + motorControl(); + RKI(); if ( switch2_pressed == true) { switch2_pressed = false; @@ -851,7 +860,14 @@ } // Do stuff until end condition is met - operationFuncBundle(); + EMGOperationFunc(); + + Vx = emg1_out * 15.0f * emg1_dir; + Vy = emg2_out * 15.0f * emg2_dir; + + PID_controller(); + motorControl(); + RKI(); // Function to move to starting position @@ -927,6 +943,91 @@ } /* +------------------------------ DEMO SUBSTATE FUNCTIONS ------------------------------ +*/ +void do_demo_wait() { + // Entry function + if ( demo_state_changed == true ) { + demo_state_changed = false; + } + + // Do nothing until end condition is met + + // State transition guard + if ( button1_pressed == true ) { + button1_pressed = false; + demo_curr_state = demo_XY; + demo_state_changed = true; + // More functions + } else if (button2_pressed == true) { + button2_pressed = false; + demo_curr_state = demo_wait; + demo_state_changed = true; + motor_cal_done = false; // Disables motor calibration again (robot is probably not in reference position) + global_curr_state = global_wait; + global_state_changed = true; + } +} + +void do_demo_motor_cal() { + // Entry function + if ( demo_state_changed == true ) { + demo_state_changed = false; + } + + // Do stuff until end condition is met + motor_state_machine(); + + // State transition guard + if ( motor_cal_done == true ) { // WAIT MODE + demo_curr_state = demo_wait; + demo_state_changed = true; + } +} + +void do_demo_XY() { + // Entry function + if ( demo_state_changed == true ) { + demo_state_changed = false; + } + + // Do stuff until end condition is met + static float t = 0; + Vy = 10.0f * sin(1.0f*t); + Vx = 0.0f; + t += Ts; + + PID_controller(); + motorControl(); + RKI(); + + // State transition guard + if ( motor_cal_done == true ) { // WAIT MODE + demo_curr_state = demo_wait; + demo_state_changed = true; + } +} + +/* +------------------------------ DEMO SUBSTATE MACHINE ------------------------------ +*/ + +void demo_state_machine() +{ + switch(demo_curr_state) { + case demo_wait: + do_demo_wait(); + break; + case demo_motor_cal: + do_demo_motor_cal(); + break; + case demo_XY: + do_demo_XY(); + break; + } +} + +/* ------------------------------ GLOBAL STATE FUNCTIONS ------------------------------ */ /* ALL STATES HAVE THE FOLLOWING FORM: @@ -1069,22 +1170,7 @@ } // Do stuff until end condition is met - - - /* - // For testing Vx and Vy - static float t=0; - Vy=10.0f*sin(1.0f*t); - Vx=0.0f; - t+=Ts; - */ - - // Move motors - PID_controller(); - motorControl(); - RKI(); - - + operation_state_machine(); // Set HIDScope outputs scope.set(0, emg1 );