Biorobotics 7 / Mbed 2 deprecated State_Machine

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?

UserRevisionLine numberNew 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 }