Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@11:d980e0e581db, 2018-10-31 (annotated)

Committer:
brass_phoenix
Date:
Wed Oct 31 08:23:02 2018 +0000
Revision:
11:d980e0e581db
Parent:
10:b165ccd11afd
Child:
12:0c10396d0615
* Made the stop state say STOP instead of error.

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