Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@6:bfc6e68774f5, 2018-10-30 (annotated)

Committer:
brass_phoenix
Date:
Tue Oct 30 12:19:51 2018 +0000
Revision:
6:bfc6e68774f5
Parent:
5:2632dfc8454c
Child:
7:e7f808875bc4
+ Additional text on screen now works.

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 1:cfa5abca6d43 14
brass_phoenix 1:cfa5abca6d43 15 States current_state; // Defining the state we are currently in
brass_phoenix 2:141cfcafe72b 16 States last_state; // To detect state changes.
MAHCSnijders 0:7d25c2ade6c5 17 Ticker loop_ticker; // Ticker for the loop function
brass_phoenix 1:cfa5abca6d43 18
brass_phoenix 1:cfa5abca6d43 19 // Order of buttons: up_down, left_right, panic
brass_phoenix 1:cfa5abca6d43 20 // D2, D3, D8
brass_phoenix 1:cfa5abca6d43 21 Button ud_button(PTA4);
brass_phoenix 2:141cfcafe72b 22 Button lr_button(D3);
brass_phoenix 2:141cfcafe72b 23 Button p_button(PTC6);
MAHCSnijders 0:7d25c2ade6c5 24
brass_phoenix 1:cfa5abca6d43 25 DigitalOut led_red(LED_RED);
brass_phoenix 1:cfa5abca6d43 26 DigitalOut led_green(LED_GREEN);
brass_phoenix 1:cfa5abca6d43 27
brass_phoenix 3:4b19b6cf6cc7 28 // The last arguent is the reset pin.
brass_phoenix 3:4b19b6cf6cc7 29 // The screen doesn't use it, but the library requires it
brass_phoenix 3:4b19b6cf6cc7 30 // So pick a pin we don't use.
brass_phoenix 3:4b19b6cf6cc7 31 Screen screen(D14, D15, D9);
brass_phoenix 2:141cfcafe72b 32
brass_phoenix 3:4b19b6cf6cc7 33
brass_phoenix 3:4b19b6cf6cc7 34 void do_state_waiting()
brass_phoenix 3:4b19b6cf6cc7 35 {
brass_phoenix 4:5a44ab7e94b3 36 if(last_state != current_state) {
brass_phoenix 4:5a44ab7e94b3 37 last_state = current_state;
brass_phoenix 4:5a44ab7e94b3 38 // State just changed to this one.
brass_phoenix 4:5a44ab7e94b3 39
brass_phoenix 4:5a44ab7e94b3 40 led_green = 1;
brass_phoenix 6:bfc6e68774f5 41 screen.clear_display();
brass_phoenix 4:5a44ab7e94b3 42 screen.display_state_name("Waiting");
brass_phoenix 6:bfc6e68774f5 43 screen.get_screen_handle()->printf(" Press to start ");
brass_phoenix 6:bfc6e68774f5 44 screen.get_screen_handle()->printf(" | ");
brass_phoenix 6:bfc6e68774f5 45 screen.get_screen_handle()->printf(" V ");
brass_phoenix 6:bfc6e68774f5 46 screen.display();
brass_phoenix 4:5a44ab7e94b3 47 }
brass_phoenix 4:5a44ab7e94b3 48
brass_phoenix 2:141cfcafe72b 49 if (ud_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 50 current_state = calib_motor;
brass_phoenix 2:141cfcafe72b 51 }
brass_phoenix 2:141cfcafe72b 52 }
brass_phoenix 2:141cfcafe72b 53
brass_phoenix 3:4b19b6cf6cc7 54 void do_state_calib_motor()
brass_phoenix 3:4b19b6cf6cc7 55 {
brass_phoenix 2:141cfcafe72b 56 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 57 last_state = current_state;
brass_phoenix 2:141cfcafe72b 58 // State just changed to this one.
brass_phoenix 3:4b19b6cf6cc7 59
brass_phoenix 2:141cfcafe72b 60 led_green = 0;
brass_phoenix 6:bfc6e68774f5 61 screen.clear_display();
brass_phoenix 5:2632dfc8454c 62 screen.display_state_name("Motor calibration");
brass_phoenix 2:141cfcafe72b 63 }
brass_phoenix 2:141cfcafe72b 64 }
brass_phoenix 2:141cfcafe72b 65
brass_phoenix 3:4b19b6cf6cc7 66 void do_state_calib_bicep1()
brass_phoenix 3:4b19b6cf6cc7 67 {
brass_phoenix 2:141cfcafe72b 68 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 69 last_state = current_state;
brass_phoenix 2:141cfcafe72b 70 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 71 screen.clear_display();
brass_phoenix 5:2632dfc8454c 72 screen.display_state_name("EMG 1 calibration");
brass_phoenix 2:141cfcafe72b 73 }
brass_phoenix 2:141cfcafe72b 74 }
brass_phoenix 2:141cfcafe72b 75
brass_phoenix 3:4b19b6cf6cc7 76 void do_state_calib_bicep2()
brass_phoenix 3:4b19b6cf6cc7 77 {
brass_phoenix 2:141cfcafe72b 78 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 79 last_state = current_state;
brass_phoenix 2:141cfcafe72b 80 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 81 screen.clear_display();
brass_phoenix 5:2632dfc8454c 82 screen.display_state_name("EMG 2 calibration");
brass_phoenix 2:141cfcafe72b 83 }
brass_phoenix 2:141cfcafe72b 84 }
brass_phoenix 2:141cfcafe72b 85
brass_phoenix 3:4b19b6cf6cc7 86 void do_state_homing()
brass_phoenix 3:4b19b6cf6cc7 87 {
brass_phoenix 2:141cfcafe72b 88 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 89 last_state = current_state;
brass_phoenix 2:141cfcafe72b 90 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 91 screen.clear_display();
brass_phoenix 5:2632dfc8454c 92 screen.display_state_name("Homing");
brass_phoenix 2:141cfcafe72b 93 }
brass_phoenix 2:141cfcafe72b 94 }
brass_phoenix 2:141cfcafe72b 95
brass_phoenix 3:4b19b6cf6cc7 96 void do_state_operation()
brass_phoenix 3:4b19b6cf6cc7 97 {
brass_phoenix 2:141cfcafe72b 98 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 99 last_state = current_state;
brass_phoenix 2:141cfcafe72b 100 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 101 screen.clear_display();
brass_phoenix 5:2632dfc8454c 102 screen.display_state_name("Normal operation");
brass_phoenix 2:141cfcafe72b 103 }
brass_phoenix 2:141cfcafe72b 104 }
brass_phoenix 2:141cfcafe72b 105
brass_phoenix 3:4b19b6cf6cc7 106 void do_state_failure()
brass_phoenix 3:4b19b6cf6cc7 107 {
brass_phoenix 2:141cfcafe72b 108 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 109 last_state = current_state;
brass_phoenix 2:141cfcafe72b 110 // State just changed.
brass_phoenix 2:141cfcafe72b 111 // Update the display.
brass_phoenix 2:141cfcafe72b 112 led_red = 0;
brass_phoenix 2:141cfcafe72b 113 led_green = 1;
brass_phoenix 6:bfc6e68774f5 114 screen.clear_display();
brass_phoenix 4:5a44ab7e94b3 115 screen.display_state_name("Error!");
brass_phoenix 2:141cfcafe72b 116 }
brass_phoenix 3:4b19b6cf6cc7 117
brass_phoenix 2:141cfcafe72b 118 // Stop the motors!
brass_phoenix 2:141cfcafe72b 119 }
brass_phoenix 2:141cfcafe72b 120
brass_phoenix 2:141cfcafe72b 121
brass_phoenix 1:cfa5abca6d43 122 void main_loop()
brass_phoenix 1:cfa5abca6d43 123 {
brass_phoenix 1:cfa5abca6d43 124 ud_button.update();
brass_phoenix 1:cfa5abca6d43 125 lr_button.update();
brass_phoenix 1:cfa5abca6d43 126 p_button.update();
brass_phoenix 3:4b19b6cf6cc7 127
brass_phoenix 1:cfa5abca6d43 128 switch (current_state) {
brass_phoenix 1:cfa5abca6d43 129 case waiting:
brass_phoenix 2:141cfcafe72b 130 do_state_waiting();
brass_phoenix 1:cfa5abca6d43 131 break;
brass_phoenix 1:cfa5abca6d43 132 case calib_motor:
brass_phoenix 2:141cfcafe72b 133 do_state_calib_motor();
brass_phoenix 1:cfa5abca6d43 134 break;
brass_phoenix 1:cfa5abca6d43 135 case calib_bicep1:
brass_phoenix 2:141cfcafe72b 136 do_state_calib_bicep1();
brass_phoenix 1:cfa5abca6d43 137 break;
brass_phoenix 1:cfa5abca6d43 138 case calib_bicep2:
brass_phoenix 2:141cfcafe72b 139 do_state_calib_bicep2();
brass_phoenix 1:cfa5abca6d43 140 break;
brass_phoenix 1:cfa5abca6d43 141 case homing:
brass_phoenix 2:141cfcafe72b 142 do_state_homing();
brass_phoenix 1:cfa5abca6d43 143 break;
brass_phoenix 1:cfa5abca6d43 144 case operation:
brass_phoenix 2:141cfcafe72b 145 do_state_operation();
brass_phoenix 1:cfa5abca6d43 146 break;
brass_phoenix 1:cfa5abca6d43 147 case failure:
brass_phoenix 2:141cfcafe72b 148 do_state_failure();
brass_phoenix 1:cfa5abca6d43 149 break;
brass_phoenix 1:cfa5abca6d43 150 }
brass_phoenix 3:4b19b6cf6cc7 151
brass_phoenix 2:141cfcafe72b 152 // Check if the panic button was pressed.
brass_phoenix 2:141cfcafe72b 153 // Doesn't matter in which state we are, we need to go to failure.
brass_phoenix 2:141cfcafe72b 154 if (p_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 155 current_state = failure;
brass_phoenix 3:4b19b6cf6cc7 156 }
brass_phoenix 1:cfa5abca6d43 157 }
MAHCSnijders 0:7d25c2ade6c5 158
brass_phoenix 1:cfa5abca6d43 159 int main()
brass_phoenix 1:cfa5abca6d43 160 {
brass_phoenix 1:cfa5abca6d43 161 led_red = 1;
brass_phoenix 3:4b19b6cf6cc7 162
brass_phoenix 2:141cfcafe72b 163 // Start in the waiting state.
brass_phoenix 1:cfa5abca6d43 164 current_state = waiting;
brass_phoenix 4:5a44ab7e94b3 165 // Pretend we come from the operation state.
brass_phoenix 4:5a44ab7e94b3 166 // So that the waiting state knows it just got started.
brass_phoenix 4:5a44ab7e94b3 167 last_state = operation;
brass_phoenix 3:4b19b6cf6cc7 168
brass_phoenix 1:cfa5abca6d43 169 while (true) {
brass_phoenix 1:cfa5abca6d43 170 main_loop();
brass_phoenix 1:cfa5abca6d43 171 wait(main_loop_wait_time);
brass_phoenix 1:cfa5abca6d43 172 }
brass_phoenix 1:cfa5abca6d43 173 }