State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
main.cpp@50:5d2176b93a65, 2018-11-05 (annotated)
- Committer:
- MAHCSnijders
- Date:
- Mon Nov 05 10:14:31 2018 +0000
- Revision:
- 50:5d2176b93a65
- Parent:
- 49:b4fd52d244f9
With new comments
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 | 20:af1a6cd7469b | 3 | #include "constants.h" |
brass_phoenix | 20:af1a6cd7469b | 4 | |
brass_phoenix | 1:cfa5abca6d43 | 5 | #include "Button.h" |
brass_phoenix | 3:4b19b6cf6cc7 | 6 | #include "Screen.h" |
brass_phoenix | 8:9090ab7c19a8 | 7 | #include "motor.h" |
brass_phoenix | 39:f119ca6fc821 | 8 | #include "EMGFilter.h" |
brass_phoenix | 16:9c5ef6fe6780 | 9 | #include "motor_calibration.h" |
brass_phoenix | 20:af1a6cd7469b | 10 | #include "forward_kinematics.h" |
brass_phoenix | 21:d541303a2ea6 | 11 | #include "inverse_kinematics.h" |
brass_phoenix | 32:b63b5837bcb1 | 12 | #include "end_effector_control.h" |
brass_phoenix | 39:f119ca6fc821 | 13 | #include "EMG_calibration.h" |
MAHCSnijders | 0:7d25c2ade6c5 | 14 | |
brass_phoenix | 1:cfa5abca6d43 | 15 | |
brass_phoenix | 32:b63b5837bcb1 | 16 | enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, demo, failure}; // The possible states of the state machine |
MAHCSnijders | 0:7d25c2ade6c5 | 17 | |
brass_phoenix | 34:ae62ebf4d494 | 18 | |
brass_phoenix | 34:ae62ebf4d494 | 19 | |
brass_phoenix | 34:ae62ebf4d494 | 20 | // Controll directions for the demo controller. |
brass_phoenix | 34:ae62ebf4d494 | 21 | enum DebugControlDirection {debug_up, debug_down, debug_left, debug_right}; |
brass_phoenix | 1:cfa5abca6d43 | 22 | |
brass_phoenix | 8:9090ab7c19a8 | 23 | |
brass_phoenix | 12:0c10396d0615 | 24 | Motor main_motor(D6, D7, D13, D12); |
brass_phoenix | 12:0c10396d0615 | 25 | Motor sec_motor(D5, D4, D10, D11); |
brass_phoenix | 12:0c10396d0615 | 26 | |
MAHCSnijders | 50:5d2176b93a65 | 27 | // For debugging purposes (with potmeter) |
brass_phoenix | 13:88967c004446 | 28 | AnalogIn potmeter1(A5); // Analoge input van potmeter 1 -> Motor 1 |
brass_phoenix | 13:88967c004446 | 29 | AnalogIn potmeter2(A4); // Analoge input van potmeter 2 -> Motor 2 |
brass_phoenix | 13:88967c004446 | 30 | |
MAHCSnijders | 50:5d2176b93a65 | 31 | EMGFilter emg_1(A0); // Input for EMG signals |
MAHCSnijders | 50:5d2176b93a65 | 32 | EMGFilter emg_2(A1); // Input for EMG signals |
brass_phoenix | 13:88967c004446 | 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. |
brass_phoenix | 1:cfa5abca6d43 | 36 | |
brass_phoenix | 1:cfa5abca6d43 | 37 | // Order of buttons: up_down, left_right, panic |
brass_phoenix | 1:cfa5abca6d43 | 38 | // D2, D3, D8 |
MAHCSnijders | 50:5d2176b93a65 | 39 | Button ud_button(D2); // Up-down |
MAHCSnijders | 50:5d2176b93a65 | 40 | Button lr_button(D3); // Left-right |
MAHCSnijders | 50:5d2176b93a65 | 41 | Button p_button(D8); // Panic (stop) |
brass_phoenix | 7:e7f808875bc4 | 42 | |
MAHCSnijders | 50:5d2176b93a65 | 43 | Ticker button_ticker; // Ticker for poll button to check whether the button is still pressed |
MAHCSnijders | 0:7d25c2ade6c5 | 44 | |
MAHCSnijders | 50:5d2176b93a65 | 45 | // LEDs for debugging purposes |
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 | |
MAHCSnijders | 50:5d2176b93a65 | 50 | // The last arguent (D6) 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. |
MAHCSnijders | 50:5d2176b93a65 | 53 | // D14 and D15 are from and to the screen |
MAHCSnijders | 50:5d2176b93a65 | 54 | Screen screen(D14, D15, D9); // Connects OLED screen (Adafruit) |
brass_phoenix | 2:141cfcafe72b | 55 | |
MAHCSnijders | 50:5d2176b93a65 | 56 | // Which direction the EMG (or for demo mode) will control the arm. |
brass_phoenix | 12:0c10396d0615 | 57 | // Up or down. |
brass_phoenix | 12:0c10396d0615 | 58 | // Left or right. |
brass_phoenix | 12:0c10396d0615 | 59 | bool control_goes_up = false; |
brass_phoenix | 12:0c10396d0615 | 60 | bool control_goes_right = false; |
brass_phoenix | 12:0c10396d0615 | 61 | |
brass_phoenix | 3:4b19b6cf6cc7 | 62 | |
brass_phoenix | 3:4b19b6cf6cc7 | 63 | void do_state_waiting() |
brass_phoenix | 3:4b19b6cf6cc7 | 64 | { |
brass_phoenix | 4:5a44ab7e94b3 | 65 | if(last_state != current_state) { |
brass_phoenix | 4:5a44ab7e94b3 | 66 | last_state = current_state; |
brass_phoenix | 4:5a44ab7e94b3 | 67 | // State just changed to this one. |
brass_phoenix | 4:5a44ab7e94b3 | 68 | |
brass_phoenix | 4:5a44ab7e94b3 | 69 | led_green = 1; |
brass_phoenix | 6:bfc6e68774f5 | 70 | screen.clear_display(); |
brass_phoenix | 4:5a44ab7e94b3 | 71 | screen.display_state_name("Waiting"); |
brass_phoenix | 6:bfc6e68774f5 | 72 | screen.get_screen_handle()->printf(" Press to start "); |
brass_phoenix | 6:bfc6e68774f5 | 73 | screen.get_screen_handle()->printf(" | "); |
brass_phoenix | 6:bfc6e68774f5 | 74 | screen.get_screen_handle()->printf(" V "); |
brass_phoenix | 6:bfc6e68774f5 | 75 | screen.display(); |
brass_phoenix | 4:5a44ab7e94b3 | 76 | } |
brass_phoenix | 41:8640b5782bc7 | 77 | |
brass_phoenix | 9:27d00b64076e | 78 | if (ud_button.has_just_been_pressed()) { |
brass_phoenix | 2:141cfcafe72b | 79 | current_state = calib_motor; |
brass_phoenix | 2:141cfcafe72b | 80 | } |
brass_phoenix | 2:141cfcafe72b | 81 | } |
brass_phoenix | 2:141cfcafe72b | 82 | |
brass_phoenix | 3:4b19b6cf6cc7 | 83 | void do_state_calib_motor() |
brass_phoenix | 3:4b19b6cf6cc7 | 84 | { |
brass_phoenix | 16:9c5ef6fe6780 | 85 | static double main_last_angle; |
brass_phoenix | 16:9c5ef6fe6780 | 86 | static double sec_last_angle; |
brass_phoenix | 16:9c5ef6fe6780 | 87 | static int main_iterations_not_moving; |
brass_phoenix | 16:9c5ef6fe6780 | 88 | static int sec_iterations_not_moving; |
brass_phoenix | 16:9c5ef6fe6780 | 89 | static bool main_is_calibrated; |
brass_phoenix | 16:9c5ef6fe6780 | 90 | static bool sec_is_calibrated; |
brass_phoenix | 41:8640b5782bc7 | 91 | |
brass_phoenix | 2:141cfcafe72b | 92 | if(last_state != current_state) { |
brass_phoenix | 2:141cfcafe72b | 93 | last_state = current_state; |
brass_phoenix | 2:141cfcafe72b | 94 | // State just changed to this one. |
brass_phoenix | 3:4b19b6cf6cc7 | 95 | |
brass_phoenix | 28:25917b26022c | 96 | led_green = 1; |
brass_phoenix | 28:25917b26022c | 97 | led_blue = 1; |
brass_phoenix | 6:bfc6e68774f5 | 98 | screen.clear_display(); |
brass_phoenix | 5:2632dfc8454c | 99 | screen.display_state_name("Motor calibration"); |
brass_phoenix | 41:8640b5782bc7 | 100 | |
brass_phoenix | 16:9c5ef6fe6780 | 101 | main_last_angle = -10; |
brass_phoenix | 16:9c5ef6fe6780 | 102 | sec_last_angle = -10; |
brass_phoenix | 16:9c5ef6fe6780 | 103 | main_iterations_not_moving = 0; |
brass_phoenix | 16:9c5ef6fe6780 | 104 | sec_iterations_not_moving = 0; |
brass_phoenix | 16:9c5ef6fe6780 | 105 | main_is_calibrated = false; |
brass_phoenix | 16:9c5ef6fe6780 | 106 | sec_is_calibrated = false; |
brass_phoenix | 2:141cfcafe72b | 107 | } |
brass_phoenix | 41:8640b5782bc7 | 108 | |
brass_phoenix | 16:9c5ef6fe6780 | 109 | if (!main_is_calibrated) { |
brass_phoenix | 33:543debddb3a9 | 110 | led_green = 1; |
brass_phoenix | 16:9c5ef6fe6780 | 111 | main_is_calibrated = calibrate_motor(main_motor, main_last_angle, main_iterations_not_moving); |
brass_phoenix | 16:9c5ef6fe6780 | 112 | if (main_is_calibrated) { |
brass_phoenix | 33:543debddb3a9 | 113 | main_motor.define_current_angle_as_x_radians(main_motor_calibration_angle); |
brass_phoenix | 33:543debddb3a9 | 114 | //main_motor.set_target_angle(main_motor_calibration_angle - 0.2); // Give the arm some breathing space. |
brass_phoenix | 28:25917b26022c | 115 | led_green = 0; |
brass_phoenix | 16:9c5ef6fe6780 | 116 | } |
brass_phoenix | 16:9c5ef6fe6780 | 117 | } |
brass_phoenix | 16:9c5ef6fe6780 | 118 | if (!sec_is_calibrated) { |
brass_phoenix | 33:543debddb3a9 | 119 | led_blue = 1; |
brass_phoenix | 16:9c5ef6fe6780 | 120 | sec_is_calibrated = calibrate_motor(sec_motor, sec_last_angle, sec_iterations_not_moving); |
brass_phoenix | 16:9c5ef6fe6780 | 121 | if (sec_is_calibrated) { |
brass_phoenix | 33:543debddb3a9 | 122 | sec_motor.define_current_angle_as_x_radians(sec_motor_calibration_angle); // -42 degrees. |
brass_phoenix | 33:543debddb3a9 | 123 | //main_motor.set_target_angle(sec_motor_calibration_angle + 0.2); // Give the arm some breathing space. |
brass_phoenix | 28:25917b26022c | 124 | led_blue = 0; |
brass_phoenix | 16:9c5ef6fe6780 | 125 | } |
brass_phoenix | 16:9c5ef6fe6780 | 126 | } |
brass_phoenix | 41:8640b5782bc7 | 127 | |
brass_phoenix | 28:25917b26022c | 128 | screen.get_screen_handle()->setTextCursor(0, 8); |
brass_phoenix | 28:25917b26022c | 129 | screen.get_screen_handle()->printf("M: %i \n", main_iterations_not_moving); |
brass_phoenix | 28:25917b26022c | 130 | screen.get_screen_handle()->printf("S: %i \n", sec_iterations_not_moving); |
brass_phoenix | 28:25917b26022c | 131 | screen.display(); |
brass_phoenix | 41:8640b5782bc7 | 132 | |
brass_phoenix | 16:9c5ef6fe6780 | 133 | if (main_is_calibrated && sec_is_calibrated) { |
brass_phoenix | 19:53b9729fbab5 | 134 | current_state = homing; |
brass_phoenix | 9:27d00b64076e | 135 | } |
brass_phoenix | 2:141cfcafe72b | 136 | } |
brass_phoenix | 2:141cfcafe72b | 137 | |
brass_phoenix | 25:cc81f2120eda | 138 | void do_state_homing() |
brass_phoenix | 25:cc81f2120eda | 139 | { |
MAHCSnijders | 50:5d2176b93a65 | 140 | // Position end-effector |
brass_phoenix | 25:cc81f2120eda | 141 | const double home_x = 0.6524; // Meters. |
MAHCSnijders | 50:5d2176b93a65 | 142 | const double home_y = 0.3409; // Meters |
brass_phoenix | 41:8640b5782bc7 | 143 | |
brass_phoenix | 26:a8f4a117cc0d | 144 | double main_home; |
brass_phoenix | 26:a8f4a117cc0d | 145 | double sec_home; |
brass_phoenix | 41:8640b5782bc7 | 146 | |
brass_phoenix | 25:cc81f2120eda | 147 | if(last_state != current_state) { |
brass_phoenix | 25:cc81f2120eda | 148 | last_state = current_state; |
brass_phoenix | 25:cc81f2120eda | 149 | // State just changed to this one. |
brass_phoenix | 25:cc81f2120eda | 150 | screen.clear_display(); |
brass_phoenix | 25:cc81f2120eda | 151 | screen.display_state_name("Homing"); |
brass_phoenix | 41:8640b5782bc7 | 152 | |
brass_phoenix | 26:a8f4a117cc0d | 153 | inverse_kinematics(home_x, home_y, main_home, sec_home); |
brass_phoenix | 41:8640b5782bc7 | 154 | |
brass_phoenix | 25:cc81f2120eda | 155 | main_motor.set_target_angle(main_home); |
brass_phoenix | 25:cc81f2120eda | 156 | sec_motor.set_target_angle(sec_home); |
brass_phoenix | 41:8640b5782bc7 | 157 | |
brass_phoenix | 33:543debddb3a9 | 158 | screen.get_screen_handle()->setTextCursor(0, 8); |
brass_phoenix | 33:543debddb3a9 | 159 | screen.get_screen_handle()->printf("Ma: %.6f \n", main_home); |
brass_phoenix | 33:543debddb3a9 | 160 | screen.get_screen_handle()->printf("Sa: %.6f \n", sec_home); |
brass_phoenix | 33:543debddb3a9 | 161 | screen.display(); |
brass_phoenix | 25:cc81f2120eda | 162 | } |
brass_phoenix | 41:8640b5782bc7 | 163 | |
brass_phoenix | 25:cc81f2120eda | 164 | if (ud_button.has_just_been_pressed()) { |
brass_phoenix | 25:cc81f2120eda | 165 | current_state = calib_bicep1; |
brass_phoenix | 25:cc81f2120eda | 166 | } |
brass_phoenix | 32:b63b5837bcb1 | 167 | if (lr_button.has_just_been_pressed()) { |
MAHCSnijders | 50:5d2176b93a65 | 168 | current_state = demo; // Back-up for if EMG does not work |
brass_phoenix | 32:b63b5837bcb1 | 169 | } |
brass_phoenix | 25:cc81f2120eda | 170 | } |
brass_phoenix | 25:cc81f2120eda | 171 | |
brass_phoenix | 3:4b19b6cf6cc7 | 172 | void do_state_calib_bicep1() |
brass_phoenix | 3:4b19b6cf6cc7 | 173 | { |
brass_phoenix | 39:f119ca6fc821 | 174 | static EMG_calibration calibration = EMG_calibration(&screen, &emg_1); |
brass_phoenix | 41:8640b5782bc7 | 175 | |
brass_phoenix | 2:141cfcafe72b | 176 | if(last_state != current_state) { |
brass_phoenix | 2:141cfcafe72b | 177 | last_state = current_state; |
brass_phoenix | 2:141cfcafe72b | 178 | // State just changed to this one. |
brass_phoenix | 6:bfc6e68774f5 | 179 | screen.clear_display(); |
brass_phoenix | 5:2632dfc8454c | 180 | screen.display_state_name("EMG 1 calibration"); |
brass_phoenix | 41:8640b5782bc7 | 181 | |
MAHCSnijders | 50:5d2176b93a65 | 182 | calibration.start(); // Starts calibration of bicep 1 |
brass_phoenix | 2:141cfcafe72b | 183 | } |
brass_phoenix | 41:8640b5782bc7 | 184 | |
brass_phoenix | 39:f119ca6fc821 | 185 | if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) { |
brass_phoenix | 9:27d00b64076e | 186 | current_state = calib_bicep2; |
brass_phoenix | 9:27d00b64076e | 187 | } |
brass_phoenix | 2:141cfcafe72b | 188 | } |
brass_phoenix | 2:141cfcafe72b | 189 | |
brass_phoenix | 3:4b19b6cf6cc7 | 190 | void do_state_calib_bicep2() |
brass_phoenix | 3:4b19b6cf6cc7 | 191 | { |
brass_phoenix | 39:f119ca6fc821 | 192 | static EMG_calibration calibration = EMG_calibration(&screen, &emg_2); |
brass_phoenix | 41:8640b5782bc7 | 193 | |
brass_phoenix | 2:141cfcafe72b | 194 | if(last_state != current_state) { |
brass_phoenix | 2:141cfcafe72b | 195 | last_state = current_state; |
brass_phoenix | 2:141cfcafe72b | 196 | // State just changed to this one. |
brass_phoenix | 6:bfc6e68774f5 | 197 | screen.clear_display(); |
brass_phoenix | 5:2632dfc8454c | 198 | screen.display_state_name("EMG 2 calibration"); |
brass_phoenix | 41:8640b5782bc7 | 199 | |
brass_phoenix | 39:f119ca6fc821 | 200 | calibration.start(); |
brass_phoenix | 2:141cfcafe72b | 201 | } |
brass_phoenix | 41:8640b5782bc7 | 202 | |
brass_phoenix | 39:f119ca6fc821 | 203 | if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) { |
brass_phoenix | 39:f119ca6fc821 | 204 | current_state = operation; |
brass_phoenix | 9:27d00b64076e | 205 | } |
brass_phoenix | 2:141cfcafe72b | 206 | } |
brass_phoenix | 2:141cfcafe72b | 207 | |
brass_phoenix | 3:4b19b6cf6cc7 | 208 | void do_state_operation() |
brass_phoenix | 3:4b19b6cf6cc7 | 209 | { |
MAHCSnijders | 50:5d2176b93a65 | 210 | static const double max_speed = 0.01; // 1 cm per iteration (as long as EMG signals are received, setpoint is set 1 cm away) |
brass_phoenix | 41:8640b5782bc7 | 211 | static double speed_x; |
brass_phoenix | 41:8640b5782bc7 | 212 | static double speed_y; |
brass_phoenix | 44:0056118c01b2 | 213 | |
brass_phoenix | 44:0056118c01b2 | 214 | static bool last_state_1; |
brass_phoenix | 44:0056118c01b2 | 215 | static bool last_state_2; |
brass_phoenix | 41:8640b5782bc7 | 216 | |
brass_phoenix | 2:141cfcafe72b | 217 | if(last_state != current_state) { |
brass_phoenix | 2:141cfcafe72b | 218 | last_state = current_state; |
brass_phoenix | 2:141cfcafe72b | 219 | // State just changed to this one. |
brass_phoenix | 6:bfc6e68774f5 | 220 | screen.clear_display(); |
brass_phoenix | 5:2632dfc8454c | 221 | screen.display_state_name("Normal operation"); |
brass_phoenix | 41:8640b5782bc7 | 222 | |
brass_phoenix | 15:f65b4566193e | 223 | control_goes_up = true; |
brass_phoenix | 15:f65b4566193e | 224 | control_goes_right = true; |
brass_phoenix | 15:f65b4566193e | 225 | |
brass_phoenix | 41:8640b5782bc7 | 226 | speed_x = 0; |
brass_phoenix | 41:8640b5782bc7 | 227 | speed_y = 0; |
brass_phoenix | 41:8640b5782bc7 | 228 | |
brass_phoenix | 15:f65b4566193e | 229 | screen.display_up_down_arrow(control_goes_up); |
brass_phoenix | 15:f65b4566193e | 230 | screen.display_left_right_arrow(control_goes_right); |
brass_phoenix | 44:0056118c01b2 | 231 | |
brass_phoenix | 44:0056118c01b2 | 232 | last_state_1 = false; |
brass_phoenix | 44:0056118c01b2 | 233 | last_state_2 = false; |
brass_phoenix | 2:141cfcafe72b | 234 | } |
brass_phoenix | 9:27d00b64076e | 235 | |
brass_phoenix | 42:cafa56da9ed2 | 236 | bool emg_1_state = emg_1.get_is_envelope_over_threshold(); |
brass_phoenix | 42:cafa56da9ed2 | 237 | bool emg_2_state = emg_2.get_is_envelope_over_threshold(); |
brass_phoenix | 42:cafa56da9ed2 | 238 | |
brass_phoenix | 45:f0066593c174 | 239 | if (emg_1_state) { |
brass_phoenix | 41:8640b5782bc7 | 240 | led_green = 0; |
brass_phoenix | 41:8640b5782bc7 | 241 | |
brass_phoenix | 46:0be634ee10e8 | 242 | if (control_goes_right) { |
brass_phoenix | 46:0be634ee10e8 | 243 | speed_x = max_speed; |
brass_phoenix | 46:0be634ee10e8 | 244 | } else { |
brass_phoenix | 46:0be634ee10e8 | 245 | speed_x = -max_speed; |
brass_phoenix | 46:0be634ee10e8 | 246 | } |
brass_phoenix | 46:0be634ee10e8 | 247 | } else { |
brass_phoenix | 46:0be634ee10e8 | 248 | led_green = 1; |
brass_phoenix | 46:0be634ee10e8 | 249 | speed_x = 0; |
brass_phoenix | 46:0be634ee10e8 | 250 | } |
brass_phoenix | 46:0be634ee10e8 | 251 | |
brass_phoenix | 46:0be634ee10e8 | 252 | if (emg_2_state) { |
brass_phoenix | 46:0be634ee10e8 | 253 | led_blue = 0; |
brass_phoenix | 41:8640b5782bc7 | 254 | if (control_goes_up) { |
brass_phoenix | 41:8640b5782bc7 | 255 | speed_y = max_speed; |
brass_phoenix | 41:8640b5782bc7 | 256 | } else { |
brass_phoenix | 41:8640b5782bc7 | 257 | speed_y = -max_speed; |
brass_phoenix | 41:8640b5782bc7 | 258 | } |
brass_phoenix | 41:8640b5782bc7 | 259 | } else { |
brass_phoenix | 46:0be634ee10e8 | 260 | led_blue = 1; |
brass_phoenix | 44:0056118c01b2 | 261 | speed_y = 0; |
brass_phoenix | 41:8640b5782bc7 | 262 | } |
brass_phoenix | 41:8640b5782bc7 | 263 | |
brass_phoenix | 44:0056118c01b2 | 264 | last_state_1 = emg_1_state; |
brass_phoenix | 44:0056118c01b2 | 265 | last_state_2 = emg_2_state; |
MAHCSnijders | 50:5d2176b93a65 | 266 | |
MAHCSnijders | 50:5d2176b93a65 | 267 | if (speed_x || speed_y) { // Ensures that if the end-effector is moved externally, the motors will resist (setpoint changes) |
brass_phoenix | 49:b4fd52d244f9 | 268 | |
brass_phoenix | 49:b4fd52d244f9 | 269 | double main_cur_angle = main_motor.get_current_angle(); |
brass_phoenix | 49:b4fd52d244f9 | 270 | double sec_cur_angle = sec_motor.get_current_angle(); |
brass_phoenix | 49:b4fd52d244f9 | 271 | |
brass_phoenix | 49:b4fd52d244f9 | 272 | double main_target, sec_target; |
brass_phoenix | 49:b4fd52d244f9 | 273 | |
brass_phoenix | 49:b4fd52d244f9 | 274 | end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target); |
brass_phoenix | 49:b4fd52d244f9 | 275 | |
brass_phoenix | 49:b4fd52d244f9 | 276 | main_motor.set_target_angle(main_target); |
brass_phoenix | 49:b4fd52d244f9 | 277 | sec_motor.set_target_angle(sec_target); |
brass_phoenix | 49:b4fd52d244f9 | 278 | } |
brass_phoenix | 41:8640b5782bc7 | 279 | |
brass_phoenix | 41:8640b5782bc7 | 280 | |
brass_phoenix | 9:27d00b64076e | 281 | if (ud_button.has_just_been_pressed()) { |
brass_phoenix | 12:0c10396d0615 | 282 | control_goes_up = !control_goes_up; |
brass_phoenix | 41:8640b5782bc7 | 283 | screen.display_up_down_arrow(control_goes_up); |
brass_phoenix | 41:8640b5782bc7 | 284 | } |
brass_phoenix | 41:8640b5782bc7 | 285 | |
brass_phoenix | 41:8640b5782bc7 | 286 | if (lr_button.has_just_been_pressed()) { |
brass_phoenix | 25:cc81f2120eda | 287 | control_goes_right = !control_goes_right; |
brass_phoenix | 15:f65b4566193e | 288 | screen.display_left_right_arrow(control_goes_right); |
brass_phoenix | 9:27d00b64076e | 289 | } |
brass_phoenix | 2:141cfcafe72b | 290 | } |
brass_phoenix | 2:141cfcafe72b | 291 | |
brass_phoenix | 41:8640b5782bc7 | 292 | void do_state_demo() |
brass_phoenix | 41:8640b5782bc7 | 293 | { |
brass_phoenix | 34:ae62ebf4d494 | 294 | static DebugControlDirection control_direction; |
brass_phoenix | 34:ae62ebf4d494 | 295 | static const double max_speed = 0.01; |
brass_phoenix | 34:ae62ebf4d494 | 296 | static double speed_x; |
brass_phoenix | 34:ae62ebf4d494 | 297 | static double speed_y; |
brass_phoenix | 41:8640b5782bc7 | 298 | |
brass_phoenix | 32:b63b5837bcb1 | 299 | if(last_state != current_state) { |
brass_phoenix | 32:b63b5837bcb1 | 300 | last_state = current_state; |
brass_phoenix | 32:b63b5837bcb1 | 301 | // State just changed. |
brass_phoenix | 32:b63b5837bcb1 | 302 | // Update the display. |
brass_phoenix | 32:b63b5837bcb1 | 303 | led_red = 1; |
brass_phoenix | 32:b63b5837bcb1 | 304 | led_green = 0; |
brass_phoenix | 32:b63b5837bcb1 | 305 | led_blue = 1; |
brass_phoenix | 32:b63b5837bcb1 | 306 | screen.clear_display(); |
brass_phoenix | 32:b63b5837bcb1 | 307 | screen.display_state_name("Demo mode!"); |
brass_phoenix | 41:8640b5782bc7 | 308 | |
brass_phoenix | 34:ae62ebf4d494 | 309 | control_direction = debug_up; |
brass_phoenix | 41:8640b5782bc7 | 310 | |
brass_phoenix | 34:ae62ebf4d494 | 311 | speed_x = 0; |
brass_phoenix | 34:ae62ebf4d494 | 312 | speed_y = 0; |
brass_phoenix | 41:8640b5782bc7 | 313 | |
brass_phoenix | 32:b63b5837bcb1 | 314 | screen.display_up_down_arrow(control_goes_up); |
brass_phoenix | 32:b63b5837bcb1 | 315 | screen.display_left_right_arrow(control_goes_right); |
brass_phoenix | 32:b63b5837bcb1 | 316 | } |
brass_phoenix | 41:8640b5782bc7 | 317 | |
brass_phoenix | 32:b63b5837bcb1 | 318 | if (lr_button.has_just_been_pressed()) { |
brass_phoenix | 34:ae62ebf4d494 | 319 | switch (control_direction) { |
brass_phoenix | 34:ae62ebf4d494 | 320 | case debug_up: |
brass_phoenix | 34:ae62ebf4d494 | 321 | control_direction = debug_right; |
brass_phoenix | 34:ae62ebf4d494 | 322 | speed_x = max_speed; |
brass_phoenix | 34:ae62ebf4d494 | 323 | speed_y = 0; |
brass_phoenix | 34:ae62ebf4d494 | 324 | break; |
brass_phoenix | 34:ae62ebf4d494 | 325 | case debug_right: |
brass_phoenix | 34:ae62ebf4d494 | 326 | control_direction = debug_down; |
brass_phoenix | 34:ae62ebf4d494 | 327 | speed_x = 0; |
brass_phoenix | 34:ae62ebf4d494 | 328 | speed_y = -max_speed; |
brass_phoenix | 34:ae62ebf4d494 | 329 | break; |
brass_phoenix | 34:ae62ebf4d494 | 330 | case debug_down: |
brass_phoenix | 34:ae62ebf4d494 | 331 | control_direction = debug_left; |
brass_phoenix | 34:ae62ebf4d494 | 332 | speed_x = -max_speed; |
brass_phoenix | 34:ae62ebf4d494 | 333 | speed_y = 0; |
brass_phoenix | 34:ae62ebf4d494 | 334 | break; |
brass_phoenix | 34:ae62ebf4d494 | 335 | case debug_left: |
brass_phoenix | 34:ae62ebf4d494 | 336 | control_direction = debug_up; |
brass_phoenix | 34:ae62ebf4d494 | 337 | speed_x = 0; |
brass_phoenix | 34:ae62ebf4d494 | 338 | speed_y = max_speed; |
brass_phoenix | 34:ae62ebf4d494 | 339 | break; |
brass_phoenix | 34:ae62ebf4d494 | 340 | } |
brass_phoenix | 32:b63b5837bcb1 | 341 | } |
brass_phoenix | 41:8640b5782bc7 | 342 | |
brass_phoenix | 34:ae62ebf4d494 | 343 | if (ud_button.is_pressed()) { |
brass_phoenix | 41:8640b5782bc7 | 344 | |
brass_phoenix | 32:b63b5837bcb1 | 345 | led_blue = 0; |
brass_phoenix | 41:8640b5782bc7 | 346 | |
brass_phoenix | 41:8640b5782bc7 | 347 | |
brass_phoenix | 32:b63b5837bcb1 | 348 | double main_cur_angle = main_motor.get_current_angle(); |
brass_phoenix | 32:b63b5837bcb1 | 349 | double sec_cur_angle = sec_motor.get_current_angle(); |
brass_phoenix | 41:8640b5782bc7 | 350 | |
brass_phoenix | 32:b63b5837bcb1 | 351 | double main_target, sec_target; |
brass_phoenix | 41:8640b5782bc7 | 352 | |
brass_phoenix | 32:b63b5837bcb1 | 353 | end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target); |
brass_phoenix | 41:8640b5782bc7 | 354 | |
brass_phoenix | 32:b63b5837bcb1 | 355 | main_motor.set_target_angle(main_target); |
brass_phoenix | 32:b63b5837bcb1 | 356 | sec_motor.set_target_angle(sec_target); |
brass_phoenix | 41:8640b5782bc7 | 357 | |
brass_phoenix | 32:b63b5837bcb1 | 358 | screen.get_screen_handle()->setTextCursor(0, 0); |
brass_phoenix | 32:b63b5837bcb1 | 359 | screen.get_screen_handle()->printf("M_a: %.6f \n", main_cur_angle); |
brass_phoenix | 32:b63b5837bcb1 | 360 | screen.get_screen_handle()->printf("S_a: %.6f \n", sec_cur_angle); |
brass_phoenix | 32:b63b5837bcb1 | 361 | screen.get_screen_handle()->printf("Vx: %.6f \n", main_target); |
brass_phoenix | 32:b63b5837bcb1 | 362 | screen.get_screen_handle()->printf("Vy: %.6f ", sec_target); |
brass_phoenix | 32:b63b5837bcb1 | 363 | screen.display(); |
brass_phoenix | 32:b63b5837bcb1 | 364 | } |
brass_phoenix | 32:b63b5837bcb1 | 365 | } |
brass_phoenix | 32:b63b5837bcb1 | 366 | |
brass_phoenix | 3:4b19b6cf6cc7 | 367 | void do_state_failure() |
brass_phoenix | 3:4b19b6cf6cc7 | 368 | { |
brass_phoenix | 2:141cfcafe72b | 369 | if(last_state != current_state) { |
brass_phoenix | 2:141cfcafe72b | 370 | last_state = current_state; |
brass_phoenix | 2:141cfcafe72b | 371 | // State just changed. |
brass_phoenix | 2:141cfcafe72b | 372 | // Update the display. |
brass_phoenix | 2:141cfcafe72b | 373 | led_red = 0; |
brass_phoenix | 2:141cfcafe72b | 374 | led_green = 1; |
brass_phoenix | 30:a45bbfa6bd22 | 375 | led_blue = 1; |
brass_phoenix | 6:bfc6e68774f5 | 376 | screen.clear_display(); |
brass_phoenix | 11:d980e0e581db | 377 | screen.display_state_name("STOP"); |
brass_phoenix | 2:141cfcafe72b | 378 | } |
brass_phoenix | 3:4b19b6cf6cc7 | 379 | |
brass_phoenix | 2:141cfcafe72b | 380 | // Stop the motors! |
brass_phoenix | 12:0c10396d0615 | 381 | main_motor.stop(); |
brass_phoenix | 12:0c10396d0615 | 382 | sec_motor.stop(); |
brass_phoenix | 2:141cfcafe72b | 383 | } |
brass_phoenix | 2:141cfcafe72b | 384 | |
brass_phoenix | 2:141cfcafe72b | 385 | |
brass_phoenix | 1:cfa5abca6d43 | 386 | void main_loop() |
brass_phoenix | 1:cfa5abca6d43 | 387 | { |
MAHCSnijders | 50:5d2176b93a65 | 388 | // Update buttons |
brass_phoenix | 1:cfa5abca6d43 | 389 | ud_button.update(); |
brass_phoenix | 1:cfa5abca6d43 | 390 | lr_button.update(); |
brass_phoenix | 1:cfa5abca6d43 | 391 | p_button.update(); |
brass_phoenix | 3:4b19b6cf6cc7 | 392 | |
brass_phoenix | 1:cfa5abca6d43 | 393 | switch (current_state) { |
brass_phoenix | 1:cfa5abca6d43 | 394 | case waiting: |
brass_phoenix | 2:141cfcafe72b | 395 | do_state_waiting(); |
brass_phoenix | 1:cfa5abca6d43 | 396 | break; |
brass_phoenix | 1:cfa5abca6d43 | 397 | case calib_motor: |
brass_phoenix | 2:141cfcafe72b | 398 | do_state_calib_motor(); |
brass_phoenix | 1:cfa5abca6d43 | 399 | break; |
brass_phoenix | 1:cfa5abca6d43 | 400 | case calib_bicep1: |
brass_phoenix | 2:141cfcafe72b | 401 | do_state_calib_bicep1(); |
brass_phoenix | 1:cfa5abca6d43 | 402 | break; |
brass_phoenix | 1:cfa5abca6d43 | 403 | case calib_bicep2: |
brass_phoenix | 2:141cfcafe72b | 404 | do_state_calib_bicep2(); |
brass_phoenix | 1:cfa5abca6d43 | 405 | break; |
brass_phoenix | 1:cfa5abca6d43 | 406 | case homing: |
brass_phoenix | 2:141cfcafe72b | 407 | do_state_homing(); |
brass_phoenix | 1:cfa5abca6d43 | 408 | break; |
brass_phoenix | 1:cfa5abca6d43 | 409 | case operation: |
brass_phoenix | 2:141cfcafe72b | 410 | do_state_operation(); |
brass_phoenix | 1:cfa5abca6d43 | 411 | break; |
brass_phoenix | 32:b63b5837bcb1 | 412 | case demo: |
brass_phoenix | 32:b63b5837bcb1 | 413 | do_state_demo(); |
brass_phoenix | 32:b63b5837bcb1 | 414 | break; |
brass_phoenix | 1:cfa5abca6d43 | 415 | case failure: |
brass_phoenix | 2:141cfcafe72b | 416 | do_state_failure(); |
brass_phoenix | 1:cfa5abca6d43 | 417 | break; |
brass_phoenix | 1:cfa5abca6d43 | 418 | } |
brass_phoenix | 3:4b19b6cf6cc7 | 419 | |
brass_phoenix | 2:141cfcafe72b | 420 | // Check if the panic button was pressed. |
brass_phoenix | 2:141cfcafe72b | 421 | // Doesn't matter in which state we are, we need to go to failure. |
brass_phoenix | 2:141cfcafe72b | 422 | if (p_button.is_pressed()) { |
brass_phoenix | 2:141cfcafe72b | 423 | current_state = failure; |
brass_phoenix | 3:4b19b6cf6cc7 | 424 | } |
brass_phoenix | 1:cfa5abca6d43 | 425 | } |
MAHCSnijders | 0:7d25c2ade6c5 | 426 | |
brass_phoenix | 41:8640b5782bc7 | 427 | void poll_buttons() |
brass_phoenix | 41:8640b5782bc7 | 428 | { |
brass_phoenix | 7:e7f808875bc4 | 429 | // We need to poll the pins periodically. |
brass_phoenix | 7:e7f808875bc4 | 430 | // Normally one would use rise and fall interrupts, so this wouldn't be |
brass_phoenix | 7:e7f808875bc4 | 431 | // needed. But the buttons we use generate so much chatter that |
brass_phoenix | 7:e7f808875bc4 | 432 | // sometimes a rising or a falling edge doesn't get registered. |
brass_phoenix | 7:e7f808875bc4 | 433 | // With all the confusion that accompanies it. |
brass_phoenix | 7:e7f808875bc4 | 434 | ud_button.poll_pin(); |
brass_phoenix | 7:e7f808875bc4 | 435 | lr_button.poll_pin(); |
brass_phoenix | 7:e7f808875bc4 | 436 | p_button.poll_pin(); |
brass_phoenix | 7:e7f808875bc4 | 437 | } |
brass_phoenix | 7:e7f808875bc4 | 438 | |
brass_phoenix | 1:cfa5abca6d43 | 439 | int main() |
brass_phoenix | 1:cfa5abca6d43 | 440 | { |
brass_phoenix | 1:cfa5abca6d43 | 441 | led_red = 1; |
brass_phoenix | 9:27d00b64076e | 442 | led_green = 1; |
brass_phoenix | 9:27d00b64076e | 443 | led_blue = 1; |
brass_phoenix | 41:8640b5782bc7 | 444 | |
brass_phoenix | 10:b165ccd11afd | 445 | screen.clear_display(); |
brass_phoenix | 41:8640b5782bc7 | 446 | |
brass_phoenix | 12:0c10396d0615 | 447 | main_motor.set_pid_k_values(Kp, Ki, Kd); |
brass_phoenix | 12:0c10396d0615 | 448 | sec_motor.set_pid_k_values(Kp, Ki, Kd); |
brass_phoenix | 41:8640b5782bc7 | 449 | |
brass_phoenix | 12:0c10396d0615 | 450 | // One of the motors is reversed in the electronics. |
brass_phoenix | 12:0c10396d0615 | 451 | // This is fixed in the motor controll board, so we have to account |
brass_phoenix | 12:0c10396d0615 | 452 | // for it in software. |
MAHCSnijders | 50:5d2176b93a65 | 453 | // Extra reduction ratio is needed to give the "real" angles that the gears need to make (ratio of both gears is different) |
brass_phoenix | 19:53b9729fbab5 | 454 | main_motor.set_extra_reduction_ratio(-main_gear_ratio); |
brass_phoenix | 19:53b9729fbab5 | 455 | sec_motor.set_extra_reduction_ratio(sec_gear_ratio); |
brass_phoenix | 41:8640b5782bc7 | 456 | |
brass_phoenix | 29:77fee8a01529 | 457 | // Set the maximum speed for both motors. |
brass_phoenix | 29:77fee8a01529 | 458 | main_motor.set_max_speed(0.5); |
brass_phoenix | 29:77fee8a01529 | 459 | sec_motor.set_max_speed(0.5); |
brass_phoenix | 41:8640b5782bc7 | 460 | |
brass_phoenix | 8:9090ab7c19a8 | 461 | // Start the motor controller at the desired frequency. |
brass_phoenix | 12:0c10396d0615 | 462 | main_motor.start(pid_period); |
brass_phoenix | 12:0c10396d0615 | 463 | sec_motor.start(pid_period); |
brass_phoenix | 3:4b19b6cf6cc7 | 464 | |
brass_phoenix | 2:141cfcafe72b | 465 | // Start in the waiting state. |
brass_phoenix | 1:cfa5abca6d43 | 466 | current_state = waiting; |
brass_phoenix | 4:5a44ab7e94b3 | 467 | // Pretend we come from the operation state. |
brass_phoenix | 4:5a44ab7e94b3 | 468 | // So that the waiting state knows it just got started. |
brass_phoenix | 4:5a44ab7e94b3 | 469 | last_state = operation; |
brass_phoenix | 41:8640b5782bc7 | 470 | |
brass_phoenix | 39:f119ca6fc821 | 471 | emg_1.start(emg_period); |
brass_phoenix | 39:f119ca6fc821 | 472 | emg_2.start(emg_period); |
brass_phoenix | 41:8640b5782bc7 | 473 | |
brass_phoenix | 7:e7f808875bc4 | 474 | // Start the button polling ticker. |
brass_phoenix | 7:e7f808875bc4 | 475 | button_ticker.attach(&poll_buttons, button_poll_interval); |
brass_phoenix | 3:4b19b6cf6cc7 | 476 | |
brass_phoenix | 1:cfa5abca6d43 | 477 | while (true) { |
MAHCSnijders | 50:5d2176b93a65 | 478 | main_loop(); // In while(true) to keep from stalling the interrupts (around 0.01 times per seconds) |
brass_phoenix | 41:8640b5782bc7 | 479 | |
brass_phoenix | 1:cfa5abca6d43 | 480 | wait(main_loop_wait_time); |
brass_phoenix | 1:cfa5abca6d43 | 481 | } |
brass_phoenix | 1:cfa5abca6d43 | 482 | } |