Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@7:e7f808875bc4, 2018-10-31 (annotated)

Committer:
brass_phoenix
Date:
Wed Oct 31 05:58:51 2018 +0000
Revision:
7:e7f808875bc4
Parent:
6:bfc6e68774f5
Child:
8:9090ab7c19a8
* Changed to a button polling scheme. This because our buttons seem to generate more chatter than the rise and fall interrupts of the mbed can handle.

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"
brass_phoenix 3:4b19b6cf6cc7 4 #include "Screen.h"
MAHCSnijders 0:7d25c2ade6c5 5
brass_phoenix 1:cfa5abca6d43 6
brass_phoenix 1:cfa5abca6d43 7 enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, failure}; // The possible states of the state machine
MAHCSnijders 0:7d25c2ade6c5 8
MAHCSnijders 0:7d25c2ade6c5 9 // Global variables
MAHCSnijders 0:7d25c2ade6c5 10 const double PI = 3.14159265359;
brass_phoenix 1:cfa5abca6d43 11 // Main loop wait time per cycle. This does not influence the motor PID or EMG reading frequencies.
brass_phoenix 1:cfa5abca6d43 12 const double main_loop_wait_time = 0.01;
brass_phoenix 1:cfa5abca6d43 13
brass_phoenix 7:e7f808875bc4 14 // Time between two button polls. Used to debounce the button presses.
brass_phoenix 7:e7f808875bc4 15 const double button_poll_interval = 0.1;
brass_phoenix 7:e7f808875bc4 16
brass_phoenix 1:cfa5abca6d43 17
brass_phoenix 1:cfa5abca6d43 18 States current_state; // Defining the state we are currently in
brass_phoenix 2:141cfcafe72b 19 States last_state; // To detect state changes.
MAHCSnijders 0:7d25c2ade6c5 20 Ticker loop_ticker; // Ticker for the loop function
brass_phoenix 1:cfa5abca6d43 21
brass_phoenix 1:cfa5abca6d43 22 // Order of buttons: up_down, left_right, panic
brass_phoenix 1:cfa5abca6d43 23 // D2, D3, D8
brass_phoenix 7:e7f808875bc4 24 Button ud_button(D2);
brass_phoenix 2:141cfcafe72b 25 Button lr_button(D3);
brass_phoenix 7:e7f808875bc4 26 Button p_button(D8);
brass_phoenix 7:e7f808875bc4 27
brass_phoenix 7:e7f808875bc4 28 Ticker button_ticker;
MAHCSnijders 0:7d25c2ade6c5 29
brass_phoenix 1:cfa5abca6d43 30 DigitalOut led_red(LED_RED);
brass_phoenix 1:cfa5abca6d43 31 DigitalOut led_green(LED_GREEN);
brass_phoenix 1:cfa5abca6d43 32
brass_phoenix 3:4b19b6cf6cc7 33 // The last arguent is the reset pin.
brass_phoenix 3:4b19b6cf6cc7 34 // The screen doesn't use it, but the library requires it
brass_phoenix 3:4b19b6cf6cc7 35 // So pick a pin we don't use.
brass_phoenix 3:4b19b6cf6cc7 36 Screen screen(D14, D15, D9);
brass_phoenix 2:141cfcafe72b 37
brass_phoenix 3:4b19b6cf6cc7 38
brass_phoenix 3:4b19b6cf6cc7 39 void do_state_waiting()
brass_phoenix 3:4b19b6cf6cc7 40 {
brass_phoenix 4:5a44ab7e94b3 41 if(last_state != current_state) {
brass_phoenix 4:5a44ab7e94b3 42 last_state = current_state;
brass_phoenix 4:5a44ab7e94b3 43 // State just changed to this one.
brass_phoenix 4:5a44ab7e94b3 44
brass_phoenix 4:5a44ab7e94b3 45 led_green = 1;
brass_phoenix 6:bfc6e68774f5 46 screen.clear_display();
brass_phoenix 4:5a44ab7e94b3 47 screen.display_state_name("Waiting");
brass_phoenix 6:bfc6e68774f5 48 screen.get_screen_handle()->printf(" Press to start ");
brass_phoenix 6:bfc6e68774f5 49 screen.get_screen_handle()->printf(" | ");
brass_phoenix 6:bfc6e68774f5 50 screen.get_screen_handle()->printf(" V ");
brass_phoenix 6:bfc6e68774f5 51 screen.display();
brass_phoenix 4:5a44ab7e94b3 52 }
brass_phoenix 4:5a44ab7e94b3 53
brass_phoenix 2:141cfcafe72b 54 if (ud_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 55 current_state = calib_motor;
brass_phoenix 2:141cfcafe72b 56 }
brass_phoenix 2:141cfcafe72b 57 }
brass_phoenix 2:141cfcafe72b 58
brass_phoenix 3:4b19b6cf6cc7 59 void do_state_calib_motor()
brass_phoenix 3:4b19b6cf6cc7 60 {
brass_phoenix 2:141cfcafe72b 61 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 62 last_state = current_state;
brass_phoenix 2:141cfcafe72b 63 // State just changed to this one.
brass_phoenix 3:4b19b6cf6cc7 64
brass_phoenix 2:141cfcafe72b 65 led_green = 0;
brass_phoenix 6:bfc6e68774f5 66 screen.clear_display();
brass_phoenix 5:2632dfc8454c 67 screen.display_state_name("Motor calibration");
brass_phoenix 2:141cfcafe72b 68 }
brass_phoenix 2:141cfcafe72b 69 }
brass_phoenix 2:141cfcafe72b 70
brass_phoenix 3:4b19b6cf6cc7 71 void do_state_calib_bicep1()
brass_phoenix 3:4b19b6cf6cc7 72 {
brass_phoenix 2:141cfcafe72b 73 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 74 last_state = current_state;
brass_phoenix 2:141cfcafe72b 75 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 76 screen.clear_display();
brass_phoenix 5:2632dfc8454c 77 screen.display_state_name("EMG 1 calibration");
brass_phoenix 2:141cfcafe72b 78 }
brass_phoenix 2:141cfcafe72b 79 }
brass_phoenix 2:141cfcafe72b 80
brass_phoenix 3:4b19b6cf6cc7 81 void do_state_calib_bicep2()
brass_phoenix 3:4b19b6cf6cc7 82 {
brass_phoenix 2:141cfcafe72b 83 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 84 last_state = current_state;
brass_phoenix 2:141cfcafe72b 85 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 86 screen.clear_display();
brass_phoenix 5:2632dfc8454c 87 screen.display_state_name("EMG 2 calibration");
brass_phoenix 2:141cfcafe72b 88 }
brass_phoenix 2:141cfcafe72b 89 }
brass_phoenix 2:141cfcafe72b 90
brass_phoenix 3:4b19b6cf6cc7 91 void do_state_homing()
brass_phoenix 3:4b19b6cf6cc7 92 {
brass_phoenix 2:141cfcafe72b 93 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 94 last_state = current_state;
brass_phoenix 2:141cfcafe72b 95 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 96 screen.clear_display();
brass_phoenix 5:2632dfc8454c 97 screen.display_state_name("Homing");
brass_phoenix 2:141cfcafe72b 98 }
brass_phoenix 2:141cfcafe72b 99 }
brass_phoenix 2:141cfcafe72b 100
brass_phoenix 3:4b19b6cf6cc7 101 void do_state_operation()
brass_phoenix 3:4b19b6cf6cc7 102 {
brass_phoenix 2:141cfcafe72b 103 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 104 last_state = current_state;
brass_phoenix 2:141cfcafe72b 105 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 106 screen.clear_display();
brass_phoenix 5:2632dfc8454c 107 screen.display_state_name("Normal operation");
brass_phoenix 2:141cfcafe72b 108 }
brass_phoenix 2:141cfcafe72b 109 }
brass_phoenix 2:141cfcafe72b 110
brass_phoenix 3:4b19b6cf6cc7 111 void do_state_failure()
brass_phoenix 3:4b19b6cf6cc7 112 {
brass_phoenix 2:141cfcafe72b 113 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 114 last_state = current_state;
brass_phoenix 2:141cfcafe72b 115 // State just changed.
brass_phoenix 2:141cfcafe72b 116 // Update the display.
brass_phoenix 2:141cfcafe72b 117 led_red = 0;
brass_phoenix 2:141cfcafe72b 118 led_green = 1;
brass_phoenix 6:bfc6e68774f5 119 screen.clear_display();
brass_phoenix 4:5a44ab7e94b3 120 screen.display_state_name("Error!");
brass_phoenix 2:141cfcafe72b 121 }
brass_phoenix 3:4b19b6cf6cc7 122
brass_phoenix 2:141cfcafe72b 123 // Stop the motors!
brass_phoenix 2:141cfcafe72b 124 }
brass_phoenix 2:141cfcafe72b 125
brass_phoenix 2:141cfcafe72b 126
brass_phoenix 1:cfa5abca6d43 127 void main_loop()
brass_phoenix 1:cfa5abca6d43 128 {
brass_phoenix 1:cfa5abca6d43 129 ud_button.update();
brass_phoenix 1:cfa5abca6d43 130 lr_button.update();
brass_phoenix 1:cfa5abca6d43 131 p_button.update();
brass_phoenix 3:4b19b6cf6cc7 132
brass_phoenix 1:cfa5abca6d43 133 switch (current_state) {
brass_phoenix 1:cfa5abca6d43 134 case waiting:
brass_phoenix 2:141cfcafe72b 135 do_state_waiting();
brass_phoenix 1:cfa5abca6d43 136 break;
brass_phoenix 1:cfa5abca6d43 137 case calib_motor:
brass_phoenix 2:141cfcafe72b 138 do_state_calib_motor();
brass_phoenix 1:cfa5abca6d43 139 break;
brass_phoenix 1:cfa5abca6d43 140 case calib_bicep1:
brass_phoenix 2:141cfcafe72b 141 do_state_calib_bicep1();
brass_phoenix 1:cfa5abca6d43 142 break;
brass_phoenix 1:cfa5abca6d43 143 case calib_bicep2:
brass_phoenix 2:141cfcafe72b 144 do_state_calib_bicep2();
brass_phoenix 1:cfa5abca6d43 145 break;
brass_phoenix 1:cfa5abca6d43 146 case homing:
brass_phoenix 2:141cfcafe72b 147 do_state_homing();
brass_phoenix 1:cfa5abca6d43 148 break;
brass_phoenix 1:cfa5abca6d43 149 case operation:
brass_phoenix 2:141cfcafe72b 150 do_state_operation();
brass_phoenix 1:cfa5abca6d43 151 break;
brass_phoenix 1:cfa5abca6d43 152 case failure:
brass_phoenix 2:141cfcafe72b 153 do_state_failure();
brass_phoenix 1:cfa5abca6d43 154 break;
brass_phoenix 1:cfa5abca6d43 155 }
brass_phoenix 3:4b19b6cf6cc7 156
brass_phoenix 2:141cfcafe72b 157 // Check if the panic button was pressed.
brass_phoenix 2:141cfcafe72b 158 // Doesn't matter in which state we are, we need to go to failure.
brass_phoenix 2:141cfcafe72b 159 if (p_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 160 current_state = failure;
brass_phoenix 3:4b19b6cf6cc7 161 }
brass_phoenix 1:cfa5abca6d43 162 }
MAHCSnijders 0:7d25c2ade6c5 163
brass_phoenix 7:e7f808875bc4 164 void poll_buttons() {
brass_phoenix 7:e7f808875bc4 165 // We need to poll the pins periodically.
brass_phoenix 7:e7f808875bc4 166 // Normally one would use rise and fall interrupts, so this wouldn't be
brass_phoenix 7:e7f808875bc4 167 // needed. But the buttons we use generate so much chatter that
brass_phoenix 7:e7f808875bc4 168 // sometimes a rising or a falling edge doesn't get registered.
brass_phoenix 7:e7f808875bc4 169 // With all the confusion that accompanies it.
brass_phoenix 7:e7f808875bc4 170 ud_button.poll_pin();
brass_phoenix 7:e7f808875bc4 171 lr_button.poll_pin();
brass_phoenix 7:e7f808875bc4 172 p_button.poll_pin();
brass_phoenix 7:e7f808875bc4 173 }
brass_phoenix 7:e7f808875bc4 174
brass_phoenix 1:cfa5abca6d43 175 int main()
brass_phoenix 1:cfa5abca6d43 176 {
brass_phoenix 1:cfa5abca6d43 177 led_red = 1;
brass_phoenix 3:4b19b6cf6cc7 178
brass_phoenix 2:141cfcafe72b 179 // Start in the waiting state.
brass_phoenix 1:cfa5abca6d43 180 current_state = waiting;
brass_phoenix 4:5a44ab7e94b3 181 // Pretend we come from the operation state.
brass_phoenix 4:5a44ab7e94b3 182 // So that the waiting state knows it just got started.
brass_phoenix 4:5a44ab7e94b3 183 last_state = operation;
brass_phoenix 7:e7f808875bc4 184
brass_phoenix 7:e7f808875bc4 185 // Start the button polling ticker.
brass_phoenix 7:e7f808875bc4 186 button_ticker.attach(&poll_buttons, button_poll_interval);
brass_phoenix 3:4b19b6cf6cc7 187
brass_phoenix 1:cfa5abca6d43 188 while (true) {
brass_phoenix 1:cfa5abca6d43 189 main_loop();
brass_phoenix 1:cfa5abca6d43 190 wait(main_loop_wait_time);
brass_phoenix 1:cfa5abca6d43 191 }
brass_phoenix 1:cfa5abca6d43 192 }