Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@12:0c10396d0615, 2018-10-31 (annotated)

Committer:
brass_phoenix
Date:
Wed Oct 31 09:36:52 2018 +0000
Revision:
12:0c10396d0615
Parent:
11:d980e0e581db
Child:
13:88967c004446
+ Set motor to arm gear ratios.

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 12:0c10396d0615 13 const float 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 12:0c10396d0615 16 const float 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 12:0c10396d0615 20 const float Kp = 10.0;
brass_phoenix 12:0c10396d0615 21 const float Ki = 0.1;
brass_phoenix 12:0c10396d0615 22 const float Kd = 0.5;
brass_phoenix 8:9090ab7c19a8 23
brass_phoenix 8:9090ab7c19a8 24
brass_phoenix 12:0c10396d0615 25 Motor main_motor(D6, D7, D13, D12);
brass_phoenix 12:0c10396d0615 26 Motor sec_motor(D5, D4, D10, D11);
brass_phoenix 12:0c10396d0615 27
brass_phoenix 12:0c10396d0615 28 // The motor -> main gear ratio is 25 / 60.
brass_phoenix 12:0c10396d0615 29 // The motor -> secondary gear ratio is 25/50
brass_phoenix 12:0c10396d0615 30 const float main_gear_ratio = 25.0/60.0;
brass_phoenix 12:0c10396d0615 31 const float sec_gear_ratio = 25.0/50.0;
brass_phoenix 8:9090ab7c19a8 32
brass_phoenix 1:cfa5abca6d43 33
brass_phoenix 1:cfa5abca6d43 34 States current_state; // Defining the state we are currently in
brass_phoenix 2:141cfcafe72b 35 States last_state; // To detect state changes.
MAHCSnijders 0:7d25c2ade6c5 36 Ticker loop_ticker; // Ticker for the loop function
brass_phoenix 1:cfa5abca6d43 37
brass_phoenix 1:cfa5abca6d43 38 // Order of buttons: up_down, left_right, panic
brass_phoenix 1:cfa5abca6d43 39 // D2, D3, D8
brass_phoenix 7:e7f808875bc4 40 Button ud_button(D2);
brass_phoenix 2:141cfcafe72b 41 Button lr_button(D3);
brass_phoenix 7:e7f808875bc4 42 Button p_button(D8);
brass_phoenix 7:e7f808875bc4 43
brass_phoenix 7:e7f808875bc4 44 Ticker button_ticker;
MAHCSnijders 0:7d25c2ade6c5 45
brass_phoenix 1:cfa5abca6d43 46 DigitalOut led_red(LED_RED);
brass_phoenix 1:cfa5abca6d43 47 DigitalOut led_green(LED_GREEN);
brass_phoenix 9:27d00b64076e 48 DigitalOut led_blue(LED_BLUE);
brass_phoenix 1:cfa5abca6d43 49
brass_phoenix 3:4b19b6cf6cc7 50 // The last arguent is the reset pin.
brass_phoenix 3:4b19b6cf6cc7 51 // The screen doesn't use it, but the library requires it
brass_phoenix 3:4b19b6cf6cc7 52 // So pick a pin we don't use.
brass_phoenix 3:4b19b6cf6cc7 53 Screen screen(D14, D15, D9);
brass_phoenix 2:141cfcafe72b 54
brass_phoenix 12:0c10396d0615 55 // Which direction the emg will control the arm.
brass_phoenix 12:0c10396d0615 56 // Up or down.
brass_phoenix 12:0c10396d0615 57 // Left or right.
brass_phoenix 12:0c10396d0615 58 bool control_goes_up = false;
brass_phoenix 12:0c10396d0615 59 bool control_goes_right = false;
brass_phoenix 12:0c10396d0615 60
brass_phoenix 3:4b19b6cf6cc7 61
brass_phoenix 3:4b19b6cf6cc7 62 void do_state_waiting()
brass_phoenix 3:4b19b6cf6cc7 63 {
brass_phoenix 4:5a44ab7e94b3 64 if(last_state != current_state) {
brass_phoenix 4:5a44ab7e94b3 65 last_state = current_state;
brass_phoenix 4:5a44ab7e94b3 66 // State just changed to this one.
brass_phoenix 4:5a44ab7e94b3 67
brass_phoenix 4:5a44ab7e94b3 68 led_green = 1;
brass_phoenix 6:bfc6e68774f5 69 screen.clear_display();
brass_phoenix 4:5a44ab7e94b3 70 screen.display_state_name("Waiting");
brass_phoenix 6:bfc6e68774f5 71 screen.get_screen_handle()->printf(" Press to start ");
brass_phoenix 6:bfc6e68774f5 72 screen.get_screen_handle()->printf(" | ");
brass_phoenix 6:bfc6e68774f5 73 screen.get_screen_handle()->printf(" V ");
brass_phoenix 6:bfc6e68774f5 74 screen.display();
brass_phoenix 4:5a44ab7e94b3 75 }
brass_phoenix 4:5a44ab7e94b3 76
brass_phoenix 9:27d00b64076e 77 if (ud_button.has_just_been_pressed()) {
brass_phoenix 2:141cfcafe72b 78 current_state = calib_motor;
brass_phoenix 2:141cfcafe72b 79 }
brass_phoenix 2:141cfcafe72b 80 }
brass_phoenix 2:141cfcafe72b 81
brass_phoenix 3:4b19b6cf6cc7 82 void do_state_calib_motor()
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 3:4b19b6cf6cc7 87
brass_phoenix 2:141cfcafe72b 88 led_green = 0;
brass_phoenix 6:bfc6e68774f5 89 screen.clear_display();
brass_phoenix 5:2632dfc8454c 90 screen.display_state_name("Motor calibration");
brass_phoenix 2:141cfcafe72b 91 }
brass_phoenix 9:27d00b64076e 92
brass_phoenix 9:27d00b64076e 93 if (ud_button.has_just_been_pressed()) {
brass_phoenix 9:27d00b64076e 94 current_state = calib_bicep1;
brass_phoenix 9:27d00b64076e 95 }
brass_phoenix 2:141cfcafe72b 96 }
brass_phoenix 2:141cfcafe72b 97
brass_phoenix 3:4b19b6cf6cc7 98 void do_state_calib_bicep1()
brass_phoenix 3:4b19b6cf6cc7 99 {
brass_phoenix 2:141cfcafe72b 100 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 101 last_state = current_state;
brass_phoenix 2:141cfcafe72b 102 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 103 screen.clear_display();
brass_phoenix 5:2632dfc8454c 104 screen.display_state_name("EMG 1 calibration");
brass_phoenix 2:141cfcafe72b 105 }
brass_phoenix 9:27d00b64076e 106
brass_phoenix 9:27d00b64076e 107 if (ud_button.has_just_been_pressed()) {
brass_phoenix 9:27d00b64076e 108 current_state = calib_bicep2;
brass_phoenix 9:27d00b64076e 109 }
brass_phoenix 2:141cfcafe72b 110 }
brass_phoenix 2:141cfcafe72b 111
brass_phoenix 3:4b19b6cf6cc7 112 void do_state_calib_bicep2()
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("EMG 2 calibration");
brass_phoenix 2:141cfcafe72b 119 }
brass_phoenix 9:27d00b64076e 120
brass_phoenix 9:27d00b64076e 121 if (ud_button.has_just_been_pressed()) {
brass_phoenix 9:27d00b64076e 122 current_state = homing;
brass_phoenix 9:27d00b64076e 123 }
brass_phoenix 2:141cfcafe72b 124 }
brass_phoenix 2:141cfcafe72b 125
brass_phoenix 3:4b19b6cf6cc7 126 void do_state_homing()
brass_phoenix 3:4b19b6cf6cc7 127 {
brass_phoenix 2:141cfcafe72b 128 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 129 last_state = current_state;
brass_phoenix 2:141cfcafe72b 130 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 131 screen.clear_display();
brass_phoenix 5:2632dfc8454c 132 screen.display_state_name("Homing");
brass_phoenix 2:141cfcafe72b 133 }
brass_phoenix 9:27d00b64076e 134
brass_phoenix 9:27d00b64076e 135 if (ud_button.has_just_been_pressed()) {
brass_phoenix 9:27d00b64076e 136 current_state = operation;
brass_phoenix 9:27d00b64076e 137 }
brass_phoenix 2:141cfcafe72b 138 }
brass_phoenix 2:141cfcafe72b 139
brass_phoenix 3:4b19b6cf6cc7 140 void do_state_operation()
brass_phoenix 3:4b19b6cf6cc7 141 {
brass_phoenix 2:141cfcafe72b 142 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 143 last_state = current_state;
brass_phoenix 2:141cfcafe72b 144 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 145 screen.clear_display();
brass_phoenix 5:2632dfc8454c 146 screen.display_state_name("Normal operation");
brass_phoenix 2:141cfcafe72b 147 }
brass_phoenix 9:27d00b64076e 148
brass_phoenix 9:27d00b64076e 149 if (ud_button.has_just_been_pressed()) {
brass_phoenix 12:0c10396d0615 150 control_goes_up = !control_goes_up;
brass_phoenix 12:0c10396d0615 151 }
brass_phoenix 12:0c10396d0615 152 if (lr_button.has_just_been_pressed()) {
brass_phoenix 12:0c10396d0615 153 control_goes_right = !control_goes_right;
brass_phoenix 9:27d00b64076e 154 }
brass_phoenix 2:141cfcafe72b 155 }
brass_phoenix 2:141cfcafe72b 156
brass_phoenix 3:4b19b6cf6cc7 157 void do_state_failure()
brass_phoenix 3:4b19b6cf6cc7 158 {
brass_phoenix 2:141cfcafe72b 159 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 160 last_state = current_state;
brass_phoenix 2:141cfcafe72b 161 // State just changed.
brass_phoenix 2:141cfcafe72b 162 // Update the display.
brass_phoenix 2:141cfcafe72b 163 led_red = 0;
brass_phoenix 2:141cfcafe72b 164 led_green = 1;
brass_phoenix 6:bfc6e68774f5 165 screen.clear_display();
brass_phoenix 11:d980e0e581db 166 screen.display_state_name("STOP");
brass_phoenix 2:141cfcafe72b 167 }
brass_phoenix 3:4b19b6cf6cc7 168
brass_phoenix 2:141cfcafe72b 169 // Stop the motors!
brass_phoenix 12:0c10396d0615 170 main_motor.stop();
brass_phoenix 12:0c10396d0615 171 sec_motor.stop();
brass_phoenix 2:141cfcafe72b 172 }
brass_phoenix 2:141cfcafe72b 173
brass_phoenix 2:141cfcafe72b 174
brass_phoenix 1:cfa5abca6d43 175 void main_loop()
brass_phoenix 1:cfa5abca6d43 176 {
brass_phoenix 1:cfa5abca6d43 177 ud_button.update();
brass_phoenix 1:cfa5abca6d43 178 lr_button.update();
brass_phoenix 1:cfa5abca6d43 179 p_button.update();
brass_phoenix 3:4b19b6cf6cc7 180
brass_phoenix 1:cfa5abca6d43 181 switch (current_state) {
brass_phoenix 1:cfa5abca6d43 182 case waiting:
brass_phoenix 2:141cfcafe72b 183 do_state_waiting();
brass_phoenix 1:cfa5abca6d43 184 break;
brass_phoenix 1:cfa5abca6d43 185 case calib_motor:
brass_phoenix 2:141cfcafe72b 186 do_state_calib_motor();
brass_phoenix 1:cfa5abca6d43 187 break;
brass_phoenix 1:cfa5abca6d43 188 case calib_bicep1:
brass_phoenix 2:141cfcafe72b 189 do_state_calib_bicep1();
brass_phoenix 1:cfa5abca6d43 190 break;
brass_phoenix 1:cfa5abca6d43 191 case calib_bicep2:
brass_phoenix 2:141cfcafe72b 192 do_state_calib_bicep2();
brass_phoenix 1:cfa5abca6d43 193 break;
brass_phoenix 1:cfa5abca6d43 194 case homing:
brass_phoenix 2:141cfcafe72b 195 do_state_homing();
brass_phoenix 1:cfa5abca6d43 196 break;
brass_phoenix 1:cfa5abca6d43 197 case operation:
brass_phoenix 2:141cfcafe72b 198 do_state_operation();
brass_phoenix 1:cfa5abca6d43 199 break;
brass_phoenix 1:cfa5abca6d43 200 case failure:
brass_phoenix 2:141cfcafe72b 201 do_state_failure();
brass_phoenix 1:cfa5abca6d43 202 break;
brass_phoenix 1:cfa5abca6d43 203 }
brass_phoenix 3:4b19b6cf6cc7 204
brass_phoenix 2:141cfcafe72b 205 // Check if the panic button was pressed.
brass_phoenix 2:141cfcafe72b 206 // Doesn't matter in which state we are, we need to go to failure.
brass_phoenix 2:141cfcafe72b 207 if (p_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 208 current_state = failure;
brass_phoenix 3:4b19b6cf6cc7 209 }
brass_phoenix 1:cfa5abca6d43 210 }
MAHCSnijders 0:7d25c2ade6c5 211
brass_phoenix 7:e7f808875bc4 212 void poll_buttons() {
brass_phoenix 7:e7f808875bc4 213 // We need to poll the pins periodically.
brass_phoenix 7:e7f808875bc4 214 // Normally one would use rise and fall interrupts, so this wouldn't be
brass_phoenix 7:e7f808875bc4 215 // needed. But the buttons we use generate so much chatter that
brass_phoenix 7:e7f808875bc4 216 // sometimes a rising or a falling edge doesn't get registered.
brass_phoenix 7:e7f808875bc4 217 // With all the confusion that accompanies it.
brass_phoenix 7:e7f808875bc4 218 ud_button.poll_pin();
brass_phoenix 7:e7f808875bc4 219 lr_button.poll_pin();
brass_phoenix 7:e7f808875bc4 220 p_button.poll_pin();
brass_phoenix 7:e7f808875bc4 221 }
brass_phoenix 7:e7f808875bc4 222
brass_phoenix 1:cfa5abca6d43 223 int main()
brass_phoenix 1:cfa5abca6d43 224 {
brass_phoenix 1:cfa5abca6d43 225 led_red = 1;
brass_phoenix 9:27d00b64076e 226 led_green = 1;
brass_phoenix 9:27d00b64076e 227 led_blue = 1;
brass_phoenix 8:9090ab7c19a8 228
brass_phoenix 10:b165ccd11afd 229 screen.clear_display();
brass_phoenix 10:b165ccd11afd 230
brass_phoenix 12:0c10396d0615 231 main_motor.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 12:0c10396d0615 232 sec_motor.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 12:0c10396d0615 233
brass_phoenix 12:0c10396d0615 234 // One of the motors is reversed in the electronics.
brass_phoenix 12:0c10396d0615 235 // This is fixed in the motor controll board, so we have to account
brass_phoenix 12:0c10396d0615 236 // for it in software.
brass_phoenix 12:0c10396d0615 237 main_motor.set_extra_reduction_ratio(main_gear_ratio);
brass_phoenix 12:0c10396d0615 238 sec_motor.set_extra_reduction_ratio(-sec_gear_ratio);
brass_phoenix 12:0c10396d0615 239
brass_phoenix 8:9090ab7c19a8 240 // Start the motor controller at the desired frequency.
brass_phoenix 12:0c10396d0615 241 main_motor.start(pid_period);
brass_phoenix 12:0c10396d0615 242 sec_motor.start(pid_period);
brass_phoenix 3:4b19b6cf6cc7 243
brass_phoenix 2:141cfcafe72b 244 // Start in the waiting state.
brass_phoenix 1:cfa5abca6d43 245 current_state = waiting;
brass_phoenix 4:5a44ab7e94b3 246 // Pretend we come from the operation state.
brass_phoenix 4:5a44ab7e94b3 247 // So that the waiting state knows it just got started.
brass_phoenix 4:5a44ab7e94b3 248 last_state = operation;
brass_phoenix 7:e7f808875bc4 249
brass_phoenix 7:e7f808875bc4 250 // Start the button polling ticker.
brass_phoenix 7:e7f808875bc4 251 button_ticker.attach(&poll_buttons, button_poll_interval);
brass_phoenix 3:4b19b6cf6cc7 252
brass_phoenix 1:cfa5abca6d43 253 while (true) {
brass_phoenix 1:cfa5abca6d43 254 main_loop();
brass_phoenix 9:27d00b64076e 255
brass_phoenix 9:27d00b64076e 256 // Button debugging.
brass_phoenix 9:27d00b64076e 257 if (ud_button.has_just_been_pressed() || lr_button.has_just_been_pressed() || p_button.has_just_been_pressed()) {
brass_phoenix 9:27d00b64076e 258 led_blue = 0;
brass_phoenix 9:27d00b64076e 259 } else {
brass_phoenix 9:27d00b64076e 260 led_blue = 1;
brass_phoenix 9:27d00b64076e 261 }
brass_phoenix 9:27d00b64076e 262
brass_phoenix 1:cfa5abca6d43 263 wait(main_loop_wait_time);
brass_phoenix 1:cfa5abca6d43 264 }
brass_phoenix 1:cfa5abca6d43 265 }