Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@8:9090ab7c19a8, 2018-10-31 (annotated)

Committer:
brass_phoenix
Date:
Wed Oct 31 06:13:38 2018 +0000
Revision:
8:9090ab7c19a8
Parent:
7:e7f808875bc4
Child:
9:27d00b64076e
+ Added motor library and stop function in the failure 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"
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 7:e7f808875bc4 16 const double button_poll_interval = 0.1;
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 1:cfa5abca6d43 43
brass_phoenix 3:4b19b6cf6cc7 44 // The last arguent is the reset pin.
brass_phoenix 3:4b19b6cf6cc7 45 // The screen doesn't use it, but the library requires it
brass_phoenix 3:4b19b6cf6cc7 46 // So pick a pin we don't use.
brass_phoenix 3:4b19b6cf6cc7 47 Screen screen(D14, D15, D9);
brass_phoenix 2:141cfcafe72b 48
brass_phoenix 3:4b19b6cf6cc7 49
brass_phoenix 3:4b19b6cf6cc7 50 void do_state_waiting()
brass_phoenix 3:4b19b6cf6cc7 51 {
brass_phoenix 4:5a44ab7e94b3 52 if(last_state != current_state) {
brass_phoenix 4:5a44ab7e94b3 53 last_state = current_state;
brass_phoenix 4:5a44ab7e94b3 54 // State just changed to this one.
brass_phoenix 4:5a44ab7e94b3 55
brass_phoenix 4:5a44ab7e94b3 56 led_green = 1;
brass_phoenix 6:bfc6e68774f5 57 screen.clear_display();
brass_phoenix 4:5a44ab7e94b3 58 screen.display_state_name("Waiting");
brass_phoenix 6:bfc6e68774f5 59 screen.get_screen_handle()->printf(" Press to start ");
brass_phoenix 6:bfc6e68774f5 60 screen.get_screen_handle()->printf(" | ");
brass_phoenix 6:bfc6e68774f5 61 screen.get_screen_handle()->printf(" V ");
brass_phoenix 6:bfc6e68774f5 62 screen.display();
brass_phoenix 4:5a44ab7e94b3 63 }
brass_phoenix 4:5a44ab7e94b3 64
brass_phoenix 2:141cfcafe72b 65 if (ud_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 66 current_state = calib_motor;
brass_phoenix 2:141cfcafe72b 67 }
brass_phoenix 2:141cfcafe72b 68 }
brass_phoenix 2:141cfcafe72b 69
brass_phoenix 3:4b19b6cf6cc7 70 void do_state_calib_motor()
brass_phoenix 3:4b19b6cf6cc7 71 {
brass_phoenix 2:141cfcafe72b 72 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 73 last_state = current_state;
brass_phoenix 2:141cfcafe72b 74 // State just changed to this one.
brass_phoenix 3:4b19b6cf6cc7 75
brass_phoenix 2:141cfcafe72b 76 led_green = 0;
brass_phoenix 6:bfc6e68774f5 77 screen.clear_display();
brass_phoenix 5:2632dfc8454c 78 screen.display_state_name("Motor calibration");
brass_phoenix 2:141cfcafe72b 79 }
brass_phoenix 2:141cfcafe72b 80 }
brass_phoenix 2:141cfcafe72b 81
brass_phoenix 3:4b19b6cf6cc7 82 void do_state_calib_bicep1()
brass_phoenix 3:4b19b6cf6cc7 83 {
brass_phoenix 2:141cfcafe72b 84 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 85 last_state = current_state;
brass_phoenix 2:141cfcafe72b 86 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 87 screen.clear_display();
brass_phoenix 5:2632dfc8454c 88 screen.display_state_name("EMG 1 calibration");
brass_phoenix 2:141cfcafe72b 89 }
brass_phoenix 2:141cfcafe72b 90 }
brass_phoenix 2:141cfcafe72b 91
brass_phoenix 3:4b19b6cf6cc7 92 void do_state_calib_bicep2()
brass_phoenix 3:4b19b6cf6cc7 93 {
brass_phoenix 2:141cfcafe72b 94 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 95 last_state = current_state;
brass_phoenix 2:141cfcafe72b 96 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 97 screen.clear_display();
brass_phoenix 5:2632dfc8454c 98 screen.display_state_name("EMG 2 calibration");
brass_phoenix 2:141cfcafe72b 99 }
brass_phoenix 2:141cfcafe72b 100 }
brass_phoenix 2:141cfcafe72b 101
brass_phoenix 3:4b19b6cf6cc7 102 void do_state_homing()
brass_phoenix 3:4b19b6cf6cc7 103 {
brass_phoenix 2:141cfcafe72b 104 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 105 last_state = current_state;
brass_phoenix 2:141cfcafe72b 106 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 107 screen.clear_display();
brass_phoenix 5:2632dfc8454c 108 screen.display_state_name("Homing");
brass_phoenix 2:141cfcafe72b 109 }
brass_phoenix 2:141cfcafe72b 110 }
brass_phoenix 2:141cfcafe72b 111
brass_phoenix 3:4b19b6cf6cc7 112 void do_state_operation()
brass_phoenix 3:4b19b6cf6cc7 113 {
brass_phoenix 2:141cfcafe72b 114 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 115 last_state = current_state;
brass_phoenix 2:141cfcafe72b 116 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 117 screen.clear_display();
brass_phoenix 5:2632dfc8454c 118 screen.display_state_name("Normal operation");
brass_phoenix 2:141cfcafe72b 119 }
brass_phoenix 2:141cfcafe72b 120 }
brass_phoenix 2:141cfcafe72b 121
brass_phoenix 3:4b19b6cf6cc7 122 void do_state_failure()
brass_phoenix 3:4b19b6cf6cc7 123 {
brass_phoenix 2:141cfcafe72b 124 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 125 last_state = current_state;
brass_phoenix 2:141cfcafe72b 126 // State just changed.
brass_phoenix 2:141cfcafe72b 127 // Update the display.
brass_phoenix 2:141cfcafe72b 128 led_red = 0;
brass_phoenix 2:141cfcafe72b 129 led_green = 1;
brass_phoenix 6:bfc6e68774f5 130 screen.clear_display();
brass_phoenix 4:5a44ab7e94b3 131 screen.display_state_name("Error!");
brass_phoenix 2:141cfcafe72b 132 }
brass_phoenix 3:4b19b6cf6cc7 133
brass_phoenix 2:141cfcafe72b 134 // Stop the motors!
brass_phoenix 8:9090ab7c19a8 135 motor1.stop();
brass_phoenix 8:9090ab7c19a8 136 motor2.stop();
brass_phoenix 2:141cfcafe72b 137 }
brass_phoenix 2:141cfcafe72b 138
brass_phoenix 2:141cfcafe72b 139
brass_phoenix 1:cfa5abca6d43 140 void main_loop()
brass_phoenix 1:cfa5abca6d43 141 {
brass_phoenix 1:cfa5abca6d43 142 ud_button.update();
brass_phoenix 1:cfa5abca6d43 143 lr_button.update();
brass_phoenix 1:cfa5abca6d43 144 p_button.update();
brass_phoenix 3:4b19b6cf6cc7 145
brass_phoenix 1:cfa5abca6d43 146 switch (current_state) {
brass_phoenix 1:cfa5abca6d43 147 case waiting:
brass_phoenix 2:141cfcafe72b 148 do_state_waiting();
brass_phoenix 1:cfa5abca6d43 149 break;
brass_phoenix 1:cfa5abca6d43 150 case calib_motor:
brass_phoenix 2:141cfcafe72b 151 do_state_calib_motor();
brass_phoenix 1:cfa5abca6d43 152 break;
brass_phoenix 1:cfa5abca6d43 153 case calib_bicep1:
brass_phoenix 2:141cfcafe72b 154 do_state_calib_bicep1();
brass_phoenix 1:cfa5abca6d43 155 break;
brass_phoenix 1:cfa5abca6d43 156 case calib_bicep2:
brass_phoenix 2:141cfcafe72b 157 do_state_calib_bicep2();
brass_phoenix 1:cfa5abca6d43 158 break;
brass_phoenix 1:cfa5abca6d43 159 case homing:
brass_phoenix 2:141cfcafe72b 160 do_state_homing();
brass_phoenix 1:cfa5abca6d43 161 break;
brass_phoenix 1:cfa5abca6d43 162 case operation:
brass_phoenix 2:141cfcafe72b 163 do_state_operation();
brass_phoenix 1:cfa5abca6d43 164 break;
brass_phoenix 1:cfa5abca6d43 165 case failure:
brass_phoenix 2:141cfcafe72b 166 do_state_failure();
brass_phoenix 1:cfa5abca6d43 167 break;
brass_phoenix 1:cfa5abca6d43 168 }
brass_phoenix 3:4b19b6cf6cc7 169
brass_phoenix 2:141cfcafe72b 170 // Check if the panic button was pressed.
brass_phoenix 2:141cfcafe72b 171 // Doesn't matter in which state we are, we need to go to failure.
brass_phoenix 2:141cfcafe72b 172 if (p_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 173 current_state = failure;
brass_phoenix 3:4b19b6cf6cc7 174 }
brass_phoenix 1:cfa5abca6d43 175 }
MAHCSnijders 0:7d25c2ade6c5 176
brass_phoenix 7:e7f808875bc4 177 void poll_buttons() {
brass_phoenix 7:e7f808875bc4 178 // We need to poll the pins periodically.
brass_phoenix 7:e7f808875bc4 179 // Normally one would use rise and fall interrupts, so this wouldn't be
brass_phoenix 7:e7f808875bc4 180 // needed. But the buttons we use generate so much chatter that
brass_phoenix 7:e7f808875bc4 181 // sometimes a rising or a falling edge doesn't get registered.
brass_phoenix 7:e7f808875bc4 182 // With all the confusion that accompanies it.
brass_phoenix 7:e7f808875bc4 183 ud_button.poll_pin();
brass_phoenix 7:e7f808875bc4 184 lr_button.poll_pin();
brass_phoenix 7:e7f808875bc4 185 p_button.poll_pin();
brass_phoenix 7:e7f808875bc4 186 }
brass_phoenix 7:e7f808875bc4 187
brass_phoenix 1:cfa5abca6d43 188 int main()
brass_phoenix 1:cfa5abca6d43 189 {
brass_phoenix 1:cfa5abca6d43 190 led_red = 1;
brass_phoenix 8:9090ab7c19a8 191
brass_phoenix 8:9090ab7c19a8 192 motor1.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 8:9090ab7c19a8 193 motor2.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 8:9090ab7c19a8 194 // Start the motor controller at the desired frequency.
brass_phoenix 8:9090ab7c19a8 195 motor1.start(pid_period);
brass_phoenix 8:9090ab7c19a8 196 motor2.start(pid_period);
brass_phoenix 3:4b19b6cf6cc7 197
brass_phoenix 2:141cfcafe72b 198 // Start in the waiting state.
brass_phoenix 1:cfa5abca6d43 199 current_state = waiting;
brass_phoenix 4:5a44ab7e94b3 200 // Pretend we come from the operation state.
brass_phoenix 4:5a44ab7e94b3 201 // So that the waiting state knows it just got started.
brass_phoenix 4:5a44ab7e94b3 202 last_state = operation;
brass_phoenix 7:e7f808875bc4 203
brass_phoenix 7:e7f808875bc4 204 // Start the button polling ticker.
brass_phoenix 7:e7f808875bc4 205 button_ticker.attach(&poll_buttons, button_poll_interval);
brass_phoenix 3:4b19b6cf6cc7 206
brass_phoenix 1:cfa5abca6d43 207 while (true) {
brass_phoenix 1:cfa5abca6d43 208 main_loop();
brass_phoenix 1:cfa5abca6d43 209 wait(main_loop_wait_time);
brass_phoenix 1:cfa5abca6d43 210 }
brass_phoenix 1:cfa5abca6d43 211 }