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?
User | Revision | Line number | New 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 | } |