Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@2:141cfcafe72b, 2018-10-30 (annotated)

Committer:
brass_phoenix
Date:
Tue Oct 30 11:21:09 2018 +0000
Revision:
2:141cfcafe72b
Parent:
1:cfa5abca6d43
Child:
3:4b19b6cf6cc7
+ Functions for every state.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MAHCSnijders 0:7d25c2ade6c5 1 #include "mbed.h"
MAHCSnijders 0:7d25c2ade6c5 2
brass_phoenix 1:cfa5abca6d43 3 #include "Button.h"
MAHCSnijders 0:7d25c2ade6c5 4
brass_phoenix 1:cfa5abca6d43 5
brass_phoenix 1:cfa5abca6d43 6 enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, failure}; // The possible states of the state machine
MAHCSnijders 0:7d25c2ade6c5 7
MAHCSnijders 0:7d25c2ade6c5 8 // Global variables
MAHCSnijders 0:7d25c2ade6c5 9 const double PI = 3.14159265359;
brass_phoenix 1:cfa5abca6d43 10 // Main loop wait time per cycle. This does not influence the motor PID or EMG reading frequencies.
brass_phoenix 1:cfa5abca6d43 11 const double main_loop_wait_time = 0.01;
brass_phoenix 1:cfa5abca6d43 12
brass_phoenix 1:cfa5abca6d43 13
brass_phoenix 1:cfa5abca6d43 14 States current_state; // Defining the state we are currently in
brass_phoenix 2:141cfcafe72b 15 States last_state; // To detect state changes.
MAHCSnijders 0:7d25c2ade6c5 16 Ticker loop_ticker; // Ticker for the loop function
brass_phoenix 1:cfa5abca6d43 17
brass_phoenix 1:cfa5abca6d43 18 // Order of buttons: up_down, left_right, panic
brass_phoenix 1:cfa5abca6d43 19 // D2, D3, D8
brass_phoenix 1:cfa5abca6d43 20 Button ud_button(PTA4);
brass_phoenix 2:141cfcafe72b 21 Button lr_button(D3);
brass_phoenix 2:141cfcafe72b 22 Button p_button(PTC6);
MAHCSnijders 0:7d25c2ade6c5 23
brass_phoenix 1:cfa5abca6d43 24 DigitalOut led_red(LED_RED);
brass_phoenix 1:cfa5abca6d43 25 DigitalOut led_green(LED_GREEN);
brass_phoenix 1:cfa5abca6d43 26
brass_phoenix 2:141cfcafe72b 27
brass_phoenix 2:141cfcafe72b 28 void do_state_waiting() {
brass_phoenix 2:141cfcafe72b 29 if (ud_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 30 current_state = calib_motor;
brass_phoenix 2:141cfcafe72b 31 }
brass_phoenix 2:141cfcafe72b 32
brass_phoenix 2:141cfcafe72b 33 led_green = 1;
brass_phoenix 2:141cfcafe72b 34 }
brass_phoenix 2:141cfcafe72b 35
brass_phoenix 2:141cfcafe72b 36 void do_state_calib_motor() {
brass_phoenix 2:141cfcafe72b 37 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 38 last_state = current_state;
brass_phoenix 2:141cfcafe72b 39 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 40
brass_phoenix 2:141cfcafe72b 41 led_green = 0;
brass_phoenix 2:141cfcafe72b 42 }
brass_phoenix 2:141cfcafe72b 43 }
brass_phoenix 2:141cfcafe72b 44
brass_phoenix 2:141cfcafe72b 45 void do_state_calib_bicep1() {
brass_phoenix 2:141cfcafe72b 46 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 47 last_state = current_state;
brass_phoenix 2:141cfcafe72b 48 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 49 }
brass_phoenix 2:141cfcafe72b 50 }
brass_phoenix 2:141cfcafe72b 51
brass_phoenix 2:141cfcafe72b 52 void do_state_calib_bicep2() {
brass_phoenix 2:141cfcafe72b 53 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 54 last_state = current_state;
brass_phoenix 2:141cfcafe72b 55 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 56 }
brass_phoenix 2:141cfcafe72b 57 }
brass_phoenix 2:141cfcafe72b 58
brass_phoenix 2:141cfcafe72b 59 void do_state_homing() {
brass_phoenix 2:141cfcafe72b 60 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 61 last_state = current_state;
brass_phoenix 2:141cfcafe72b 62 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 63 }
brass_phoenix 2:141cfcafe72b 64 }
brass_phoenix 2:141cfcafe72b 65
brass_phoenix 2:141cfcafe72b 66 void do_state_operation() {
brass_phoenix 2:141cfcafe72b 67 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 68 last_state = current_state;
brass_phoenix 2:141cfcafe72b 69 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 70 }
brass_phoenix 2:141cfcafe72b 71 }
brass_phoenix 2:141cfcafe72b 72
brass_phoenix 2:141cfcafe72b 73 void do_state_failure() {
brass_phoenix 2:141cfcafe72b 74 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 75 last_state = current_state;
brass_phoenix 2:141cfcafe72b 76 // State just changed.
brass_phoenix 2:141cfcafe72b 77 // Update the display.
brass_phoenix 2:141cfcafe72b 78 led_red = 0;
brass_phoenix 2:141cfcafe72b 79 led_green = 1;
brass_phoenix 2:141cfcafe72b 80 }
brass_phoenix 2:141cfcafe72b 81
brass_phoenix 2:141cfcafe72b 82 // Stop the motors!
brass_phoenix 2:141cfcafe72b 83 }
brass_phoenix 2:141cfcafe72b 84
brass_phoenix 2:141cfcafe72b 85
brass_phoenix 1:cfa5abca6d43 86 void main_loop()
brass_phoenix 1:cfa5abca6d43 87 {
brass_phoenix 1:cfa5abca6d43 88 ud_button.update();
brass_phoenix 1:cfa5abca6d43 89 lr_button.update();
brass_phoenix 1:cfa5abca6d43 90 p_button.update();
MAHCSnijders 0:7d25c2ade6c5 91
brass_phoenix 1:cfa5abca6d43 92 switch (current_state) {
brass_phoenix 1:cfa5abca6d43 93 case waiting:
brass_phoenix 2:141cfcafe72b 94 do_state_waiting();
brass_phoenix 1:cfa5abca6d43 95 break;
brass_phoenix 1:cfa5abca6d43 96 case calib_motor:
brass_phoenix 2:141cfcafe72b 97 do_state_calib_motor();
brass_phoenix 1:cfa5abca6d43 98 break;
brass_phoenix 1:cfa5abca6d43 99 case calib_bicep1:
brass_phoenix 2:141cfcafe72b 100 do_state_calib_bicep1();
brass_phoenix 1:cfa5abca6d43 101 break;
brass_phoenix 1:cfa5abca6d43 102 case calib_bicep2:
brass_phoenix 2:141cfcafe72b 103 do_state_calib_bicep2();
brass_phoenix 1:cfa5abca6d43 104 break;
brass_phoenix 1:cfa5abca6d43 105 case homing:
brass_phoenix 2:141cfcafe72b 106 do_state_homing();
brass_phoenix 1:cfa5abca6d43 107 break;
brass_phoenix 1:cfa5abca6d43 108 case operation:
brass_phoenix 2:141cfcafe72b 109 do_state_operation();
brass_phoenix 1:cfa5abca6d43 110 break;
brass_phoenix 1:cfa5abca6d43 111 case failure:
brass_phoenix 2:141cfcafe72b 112 do_state_failure();
brass_phoenix 1:cfa5abca6d43 113 break;
brass_phoenix 1:cfa5abca6d43 114 }
brass_phoenix 1:cfa5abca6d43 115
brass_phoenix 2:141cfcafe72b 116 // Check if the panic button was pressed.
brass_phoenix 2:141cfcafe72b 117 // Doesn't matter in which state we are, we need to go to failure.
brass_phoenix 2:141cfcafe72b 118 if (p_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 119 current_state = failure;
brass_phoenix 2:141cfcafe72b 120 }
brass_phoenix 1:cfa5abca6d43 121 }
MAHCSnijders 0:7d25c2ade6c5 122
brass_phoenix 1:cfa5abca6d43 123 int main()
brass_phoenix 1:cfa5abca6d43 124 {
brass_phoenix 1:cfa5abca6d43 125 led_red = 1;
brass_phoenix 2:141cfcafe72b 126
brass_phoenix 2:141cfcafe72b 127 // Start in the waiting state.
brass_phoenix 1:cfa5abca6d43 128 current_state = waiting;
brass_phoenix 2:141cfcafe72b 129 last_state = waiting;
brass_phoenix 2:141cfcafe72b 130
brass_phoenix 1:cfa5abca6d43 131 while (true) {
brass_phoenix 1:cfa5abca6d43 132 main_loop();
brass_phoenix 1:cfa5abca6d43 133 wait(main_loop_wait_time);
brass_phoenix 1:cfa5abca6d43 134 }
brass_phoenix 1:cfa5abca6d43 135 }