Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@45:f0066593c174, 2018-11-01 (annotated)

Committer:
brass_phoenix
Date:
Thu Nov 01 17:35:58 2018 +0000
Revision:
45:f0066593c174
Parent:
44:0056118c01b2
Child:
46:0be634ee10e8
+ WORKS! (It was the jumpers on the emg board.)

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
brass_phoenix 1:cfa5abca6d43 27
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
brass_phoenix 39:f119ca6fc821 31 EMGFilter emg_1(A0);
brass_phoenix 39:f119ca6fc821 32 EMGFilter emg_2(A1);
brass_phoenix 39:f119ca6fc821 33
brass_phoenix 13:88967c004446 34
brass_phoenix 1:cfa5abca6d43 35 States current_state; // Defining the state we are currently in
brass_phoenix 2:141cfcafe72b 36 States last_state; // To detect state changes.
MAHCSnijders 0:7d25c2ade6c5 37 Ticker loop_ticker; // Ticker for the loop function
brass_phoenix 1:cfa5abca6d43 38
brass_phoenix 1:cfa5abca6d43 39 // Order of buttons: up_down, left_right, panic
brass_phoenix 1:cfa5abca6d43 40 // D2, D3, D8
brass_phoenix 7:e7f808875bc4 41 Button ud_button(D2);
brass_phoenix 2:141cfcafe72b 42 Button lr_button(D3);
brass_phoenix 7:e7f808875bc4 43 Button p_button(D8);
brass_phoenix 7:e7f808875bc4 44
brass_phoenix 7:e7f808875bc4 45 Ticker button_ticker;
MAHCSnijders 0:7d25c2ade6c5 46
brass_phoenix 1:cfa5abca6d43 47 DigitalOut led_red(LED_RED);
brass_phoenix 1:cfa5abca6d43 48 DigitalOut led_green(LED_GREEN);
brass_phoenix 9:27d00b64076e 49 DigitalOut led_blue(LED_BLUE);
brass_phoenix 1:cfa5abca6d43 50
brass_phoenix 3:4b19b6cf6cc7 51 // The last arguent is the reset pin.
brass_phoenix 3:4b19b6cf6cc7 52 // The screen doesn't use it, but the library requires it
brass_phoenix 3:4b19b6cf6cc7 53 // So pick a pin we don't use.
brass_phoenix 3:4b19b6cf6cc7 54 Screen screen(D14, D15, D9);
brass_phoenix 2:141cfcafe72b 55
brass_phoenix 12:0c10396d0615 56 // Which direction the emg 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 {
brass_phoenix 25:cc81f2120eda 140 const double home_x = 0.6524; // Meters.
brass_phoenix 25:cc81f2120eda 141 const double home_y = 0.3409;
brass_phoenix 41:8640b5782bc7 142
brass_phoenix 26:a8f4a117cc0d 143 double main_home;
brass_phoenix 26:a8f4a117cc0d 144 double sec_home;
brass_phoenix 41:8640b5782bc7 145
brass_phoenix 25:cc81f2120eda 146 if(last_state != current_state) {
brass_phoenix 25:cc81f2120eda 147 last_state = current_state;
brass_phoenix 25:cc81f2120eda 148 // State just changed to this one.
brass_phoenix 25:cc81f2120eda 149 screen.clear_display();
brass_phoenix 25:cc81f2120eda 150 screen.display_state_name("Homing");
brass_phoenix 41:8640b5782bc7 151
brass_phoenix 26:a8f4a117cc0d 152 inverse_kinematics(home_x, home_y, main_home, sec_home);
brass_phoenix 41:8640b5782bc7 153
brass_phoenix 25:cc81f2120eda 154 main_motor.set_target_angle(main_home);
brass_phoenix 25:cc81f2120eda 155 sec_motor.set_target_angle(sec_home);
brass_phoenix 41:8640b5782bc7 156
brass_phoenix 33:543debddb3a9 157 screen.get_screen_handle()->setTextCursor(0, 8);
brass_phoenix 33:543debddb3a9 158 screen.get_screen_handle()->printf("Ma: %.6f \n", main_home);
brass_phoenix 33:543debddb3a9 159 screen.get_screen_handle()->printf("Sa: %.6f \n", sec_home);
brass_phoenix 33:543debddb3a9 160 screen.display();
brass_phoenix 25:cc81f2120eda 161 }
brass_phoenix 41:8640b5782bc7 162
brass_phoenix 25:cc81f2120eda 163 if (ud_button.has_just_been_pressed()) {
brass_phoenix 25:cc81f2120eda 164 current_state = calib_bicep1;
brass_phoenix 25:cc81f2120eda 165 }
brass_phoenix 32:b63b5837bcb1 166 if (lr_button.has_just_been_pressed()) {
brass_phoenix 32:b63b5837bcb1 167 current_state = demo;
brass_phoenix 32:b63b5837bcb1 168 }
brass_phoenix 25:cc81f2120eda 169 }
brass_phoenix 25:cc81f2120eda 170
brass_phoenix 3:4b19b6cf6cc7 171 void do_state_calib_bicep1()
brass_phoenix 3:4b19b6cf6cc7 172 {
brass_phoenix 39:f119ca6fc821 173 static EMG_calibration calibration = EMG_calibration(&screen, &emg_1);
brass_phoenix 41:8640b5782bc7 174
brass_phoenix 2:141cfcafe72b 175 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 176 last_state = current_state;
brass_phoenix 2:141cfcafe72b 177 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 178 screen.clear_display();
brass_phoenix 5:2632dfc8454c 179 screen.display_state_name("EMG 1 calibration");
brass_phoenix 41:8640b5782bc7 180
brass_phoenix 39:f119ca6fc821 181 calibration.start();
brass_phoenix 2:141cfcafe72b 182 }
brass_phoenix 41:8640b5782bc7 183
brass_phoenix 39:f119ca6fc821 184 if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) {
brass_phoenix 9:27d00b64076e 185 current_state = calib_bicep2;
brass_phoenix 9:27d00b64076e 186 }
brass_phoenix 2:141cfcafe72b 187 }
brass_phoenix 2:141cfcafe72b 188
brass_phoenix 3:4b19b6cf6cc7 189 void do_state_calib_bicep2()
brass_phoenix 3:4b19b6cf6cc7 190 {
brass_phoenix 39:f119ca6fc821 191 static EMG_calibration calibration = EMG_calibration(&screen, &emg_2);
brass_phoenix 41:8640b5782bc7 192
brass_phoenix 2:141cfcafe72b 193 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 194 last_state = current_state;
brass_phoenix 2:141cfcafe72b 195 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 196 screen.clear_display();
brass_phoenix 5:2632dfc8454c 197 screen.display_state_name("EMG 2 calibration");
brass_phoenix 41:8640b5782bc7 198
brass_phoenix 39:f119ca6fc821 199 calibration.start();
brass_phoenix 2:141cfcafe72b 200 }
brass_phoenix 41:8640b5782bc7 201
brass_phoenix 39:f119ca6fc821 202 if (ud_button.has_just_been_pressed() && calibration.is_calibrated()) {
brass_phoenix 39:f119ca6fc821 203 current_state = operation;
brass_phoenix 9:27d00b64076e 204 }
brass_phoenix 2:141cfcafe72b 205 }
brass_phoenix 2:141cfcafe72b 206
brass_phoenix 3:4b19b6cf6cc7 207 void do_state_operation()
brass_phoenix 3:4b19b6cf6cc7 208 {
brass_phoenix 21:d541303a2ea6 209 static bool debug_forward_kinematics;
brass_phoenix 41:8640b5782bc7 210
brass_phoenix 41:8640b5782bc7 211 static const double max_speed = 0.01;
brass_phoenix 41:8640b5782bc7 212 static double speed_x;
brass_phoenix 41:8640b5782bc7 213 static double speed_y;
brass_phoenix 44:0056118c01b2 214
brass_phoenix 44:0056118c01b2 215 static bool last_state_1;
brass_phoenix 44:0056118c01b2 216 static bool last_state_2;
brass_phoenix 41:8640b5782bc7 217
brass_phoenix 2:141cfcafe72b 218 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 219 last_state = current_state;
brass_phoenix 2:141cfcafe72b 220 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 221 screen.clear_display();
brass_phoenix 5:2632dfc8454c 222 screen.display_state_name("Normal operation");
brass_phoenix 41:8640b5782bc7 223
brass_phoenix 15:f65b4566193e 224 control_goes_up = true;
brass_phoenix 15:f65b4566193e 225 control_goes_right = true;
brass_phoenix 15:f65b4566193e 226
brass_phoenix 41:8640b5782bc7 227 speed_x = 0;
brass_phoenix 41:8640b5782bc7 228 speed_y = 0;
brass_phoenix 41:8640b5782bc7 229
brass_phoenix 15:f65b4566193e 230 screen.display_up_down_arrow(control_goes_up);
brass_phoenix 15:f65b4566193e 231 screen.display_left_right_arrow(control_goes_right);
brass_phoenix 41:8640b5782bc7 232
brass_phoenix 44:0056118c01b2 233 // debug_forward_kinematics = true;
brass_phoenix 44:0056118c01b2 234
brass_phoenix 44:0056118c01b2 235 last_state_1 = false;
brass_phoenix 44:0056118c01b2 236 last_state_2 = false;
brass_phoenix 2:141cfcafe72b 237 }
brass_phoenix 9:27d00b64076e 238
brass_phoenix 42:cafa56da9ed2 239 bool emg_1_state = emg_1.get_is_envelope_over_threshold();
brass_phoenix 42:cafa56da9ed2 240 bool emg_2_state = emg_2.get_is_envelope_over_threshold();
brass_phoenix 42:cafa56da9ed2 241
brass_phoenix 45:f0066593c174 242 if (emg_1_state) {
brass_phoenix 41:8640b5782bc7 243 led_green = 0;
brass_phoenix 41:8640b5782bc7 244
brass_phoenix 41:8640b5782bc7 245 if (control_goes_up) {
brass_phoenix 41:8640b5782bc7 246 speed_y = max_speed;
brass_phoenix 41:8640b5782bc7 247 } else {
brass_phoenix 41:8640b5782bc7 248 speed_y = -max_speed;
brass_phoenix 41:8640b5782bc7 249 }
brass_phoenix 41:8640b5782bc7 250 } else {
brass_phoenix 41:8640b5782bc7 251 led_green = 1;
brass_phoenix 44:0056118c01b2 252 speed_y = 0;
brass_phoenix 41:8640b5782bc7 253 }
brass_phoenix 41:8640b5782bc7 254
brass_phoenix 45:f0066593c174 255 if (emg_2_state) {
brass_phoenix 41:8640b5782bc7 256 led_blue = 0;
brass_phoenix 41:8640b5782bc7 257 if (control_goes_right) {
brass_phoenix 41:8640b5782bc7 258 speed_x = max_speed;
brass_phoenix 41:8640b5782bc7 259 } else {
brass_phoenix 41:8640b5782bc7 260 speed_x = -max_speed;
brass_phoenix 41:8640b5782bc7 261 }
brass_phoenix 41:8640b5782bc7 262 } else {
brass_phoenix 41:8640b5782bc7 263 led_blue = 1;
brass_phoenix 44:0056118c01b2 264 speed_x = 0;
brass_phoenix 41:8640b5782bc7 265 }
brass_phoenix 44:0056118c01b2 266
brass_phoenix 44:0056118c01b2 267 last_state_1 = emg_1_state;
brass_phoenix 44:0056118c01b2 268 last_state_2 = emg_2_state;
brass_phoenix 41:8640b5782bc7 269
brass_phoenix 40:7e8d0632757c 270 /*
brass_phoenix 21:d541303a2ea6 271 if (debug_forward_kinematics) {
brass_phoenix 21:d541303a2ea6 272 // Using potmeters for debugging purposes;
brass_phoenix 21:d541303a2ea6 273 double main_angle = ((potmeter1.read() * 2) - 1) * PI;
brass_phoenix 21:d541303a2ea6 274 double sec_angle = ((potmeter2.read() * 2) - 1) * PI;
brass_phoenix 41:8640b5782bc7 275
brass_phoenix 21:d541303a2ea6 276 double e_x = 0.0;
brass_phoenix 21:d541303a2ea6 277 double e_y = 0.0;
brass_phoenix 41:8640b5782bc7 278
brass_phoenix 21:d541303a2ea6 279 forward_kinematics(main_angle, sec_angle, e_x, e_y);
brass_phoenix 41:8640b5782bc7 280
brass_phoenix 21:d541303a2ea6 281 screen.get_screen_handle()->setTextCursor(0, 0);
brass_phoenix 21:d541303a2ea6 282 screen.get_screen_handle()->printf("M_a: %.6f \n", main_angle);
brass_phoenix 21:d541303a2ea6 283 screen.get_screen_handle()->printf("S_a: %.6f \n", sec_angle);
brass_phoenix 21:d541303a2ea6 284 screen.get_screen_handle()->printf("X: %.6f \n", e_x);
brass_phoenix 21:d541303a2ea6 285 screen.get_screen_handle()->printf("Y: %.6f ", e_y);
brass_phoenix 21:d541303a2ea6 286 screen.display();
brass_phoenix 41:8640b5782bc7 287
brass_phoenix 21:d541303a2ea6 288 } else {
brass_phoenix 21:d541303a2ea6 289 // Using potmeters for debugging purposes;
brass_phoenix 21:d541303a2ea6 290 double e_x = potmeter1.read();
brass_phoenix 21:d541303a2ea6 291 double e_y = potmeter2.read();
brass_phoenix 41:8640b5782bc7 292
brass_phoenix 21:d541303a2ea6 293 double main_angle = 0.0;
brass_phoenix 21:d541303a2ea6 294 double sec_angle = 0.0;
brass_phoenix 41:8640b5782bc7 295
brass_phoenix 21:d541303a2ea6 296 inverse_kinematics(e_x, e_y, main_angle, sec_angle);
brass_phoenix 41:8640b5782bc7 297
brass_phoenix 21:d541303a2ea6 298 screen.get_screen_handle()->setTextCursor(0, 0);
brass_phoenix 21:d541303a2ea6 299 screen.get_screen_handle()->printf("E_x: %.6f \n", e_x);
brass_phoenix 21:d541303a2ea6 300 screen.get_screen_handle()->printf("E_y: %.6f \n", e_y);
brass_phoenix 21:d541303a2ea6 301 screen.get_screen_handle()->printf("M_a: %.6f \n", main_angle);
brass_phoenix 21:d541303a2ea6 302 screen.get_screen_handle()->printf("S_a: %.6f ", sec_angle);
brass_phoenix 21:d541303a2ea6 303 screen.display();
brass_phoenix 21:d541303a2ea6 304 }
brass_phoenix 41:8640b5782bc7 305
brass_phoenix 21:d541303a2ea6 306 if (lr_button.has_just_been_pressed()) {
brass_phoenix 21:d541303a2ea6 307 debug_forward_kinematics = !debug_forward_kinematics;
brass_phoenix 40:7e8d0632757c 308 }*/
brass_phoenix 40:7e8d0632757c 309
brass_phoenix 20:af1a6cd7469b 310
brass_phoenix 19:53b9729fbab5 311 /*
brass_phoenix 14:b97e7a41ec23 312 double main_target = ((potmeter1.read() * 2) - 1) * PI;
brass_phoenix 13:88967c004446 313 main_motor.set_target_angle(main_target);
brass_phoenix 14:b97e7a41ec23 314 double sec_target = ((potmeter2.read() * 2) - 1) * PI;
brass_phoenix 13:88967c004446 315 sec_motor.set_target_angle(sec_target);
brass_phoenix 41:8640b5782bc7 316
brass_phoenix 25:cc81f2120eda 317 if (lr_button.has_just_been_pressed()) {
brass_phoenix 25:cc81f2120eda 318 control_goes_right = !control_goes_right;
brass_phoenix 25:cc81f2120eda 319 screen.display_left_right_arrow(control_goes_right);
brass_phoenix 25:cc81f2120eda 320 }
brass_phoenix 25:cc81f2120eda 321 */
brass_phoenix 25:cc81f2120eda 322
brass_phoenix 41:8640b5782bc7 323 double main_cur_angle = main_motor.get_current_angle();
brass_phoenix 41:8640b5782bc7 324 double sec_cur_angle = sec_motor.get_current_angle();
brass_phoenix 41:8640b5782bc7 325
brass_phoenix 41:8640b5782bc7 326 double main_target, sec_target;
brass_phoenix 41:8640b5782bc7 327
brass_phoenix 41:8640b5782bc7 328 end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target);
brass_phoenix 41:8640b5782bc7 329
brass_phoenix 41:8640b5782bc7 330 main_motor.set_target_angle(main_target);
brass_phoenix 41:8640b5782bc7 331 sec_motor.set_target_angle(sec_target);
brass_phoenix 41:8640b5782bc7 332
brass_phoenix 42:cafa56da9ed2 333 /*
brass_phoenix 41:8640b5782bc7 334 screen.get_screen_handle()->setTextCursor(0, 0);
brass_phoenix 41:8640b5782bc7 335 screen.get_screen_handle()->printf("M_a: %.6f \n", main_cur_angle);
brass_phoenix 41:8640b5782bc7 336 screen.get_screen_handle()->printf("S_a: %.6f \n", sec_cur_angle);
brass_phoenix 41:8640b5782bc7 337 screen.get_screen_handle()->printf("Vx: %.6f \n", main_target);
brass_phoenix 41:8640b5782bc7 338 screen.get_screen_handle()->printf("Vy: %.6f ", sec_target);
brass_phoenix 41:8640b5782bc7 339 screen.display();
brass_phoenix 42:cafa56da9ed2 340 */
brass_phoenix 42:cafa56da9ed2 341
brass_phoenix 42:cafa56da9ed2 342 screen.display_emg_state(emg_1_state, emg_2_state);
brass_phoenix 42:cafa56da9ed2 343 screen.display();
brass_phoenix 41:8640b5782bc7 344
brass_phoenix 9:27d00b64076e 345 if (ud_button.has_just_been_pressed()) {
brass_phoenix 12:0c10396d0615 346 control_goes_up = !control_goes_up;
brass_phoenix 41:8640b5782bc7 347 screen.display_up_down_arrow(control_goes_up);
brass_phoenix 41:8640b5782bc7 348 }
brass_phoenix 41:8640b5782bc7 349
brass_phoenix 41:8640b5782bc7 350 if (lr_button.has_just_been_pressed()) {
brass_phoenix 25:cc81f2120eda 351 control_goes_right = !control_goes_right;
brass_phoenix 15:f65b4566193e 352 screen.display_left_right_arrow(control_goes_right);
brass_phoenix 9:27d00b64076e 353 }
brass_phoenix 2:141cfcafe72b 354 }
brass_phoenix 2:141cfcafe72b 355
brass_phoenix 41:8640b5782bc7 356 void do_state_demo()
brass_phoenix 41:8640b5782bc7 357 {
brass_phoenix 34:ae62ebf4d494 358 static DebugControlDirection control_direction;
brass_phoenix 34:ae62ebf4d494 359 static const double max_speed = 0.01;
brass_phoenix 34:ae62ebf4d494 360 static double speed_x;
brass_phoenix 34:ae62ebf4d494 361 static double speed_y;
brass_phoenix 41:8640b5782bc7 362
brass_phoenix 32:b63b5837bcb1 363 if(last_state != current_state) {
brass_phoenix 32:b63b5837bcb1 364 last_state = current_state;
brass_phoenix 32:b63b5837bcb1 365 // State just changed.
brass_phoenix 32:b63b5837bcb1 366 // Update the display.
brass_phoenix 32:b63b5837bcb1 367 led_red = 1;
brass_phoenix 32:b63b5837bcb1 368 led_green = 0;
brass_phoenix 32:b63b5837bcb1 369 led_blue = 1;
brass_phoenix 32:b63b5837bcb1 370 screen.clear_display();
brass_phoenix 32:b63b5837bcb1 371 screen.display_state_name("Demo mode!");
brass_phoenix 41:8640b5782bc7 372
brass_phoenix 34:ae62ebf4d494 373 control_direction = debug_up;
brass_phoenix 41:8640b5782bc7 374
brass_phoenix 34:ae62ebf4d494 375 speed_x = 0;
brass_phoenix 34:ae62ebf4d494 376 speed_y = 0;
brass_phoenix 41:8640b5782bc7 377
brass_phoenix 32:b63b5837bcb1 378 screen.display_up_down_arrow(control_goes_up);
brass_phoenix 32:b63b5837bcb1 379 screen.display_left_right_arrow(control_goes_right);
brass_phoenix 32:b63b5837bcb1 380 }
brass_phoenix 41:8640b5782bc7 381
brass_phoenix 32:b63b5837bcb1 382 if (lr_button.has_just_been_pressed()) {
brass_phoenix 34:ae62ebf4d494 383 switch (control_direction) {
brass_phoenix 34:ae62ebf4d494 384 case debug_up:
brass_phoenix 34:ae62ebf4d494 385 control_direction = debug_right;
brass_phoenix 34:ae62ebf4d494 386 speed_x = max_speed;
brass_phoenix 34:ae62ebf4d494 387 speed_y = 0;
brass_phoenix 34:ae62ebf4d494 388 break;
brass_phoenix 34:ae62ebf4d494 389 case debug_right:
brass_phoenix 34:ae62ebf4d494 390 control_direction = debug_down;
brass_phoenix 34:ae62ebf4d494 391 speed_x = 0;
brass_phoenix 34:ae62ebf4d494 392 speed_y = -max_speed;
brass_phoenix 34:ae62ebf4d494 393 break;
brass_phoenix 34:ae62ebf4d494 394 case debug_down:
brass_phoenix 34:ae62ebf4d494 395 control_direction = debug_left;
brass_phoenix 34:ae62ebf4d494 396 speed_x = -max_speed;
brass_phoenix 34:ae62ebf4d494 397 speed_y = 0;
brass_phoenix 34:ae62ebf4d494 398 break;
brass_phoenix 34:ae62ebf4d494 399 case debug_left:
brass_phoenix 34:ae62ebf4d494 400 control_direction = debug_up;
brass_phoenix 34:ae62ebf4d494 401 speed_x = 0;
brass_phoenix 34:ae62ebf4d494 402 speed_y = max_speed;
brass_phoenix 34:ae62ebf4d494 403 break;
brass_phoenix 34:ae62ebf4d494 404 }
brass_phoenix 32:b63b5837bcb1 405 }
brass_phoenix 41:8640b5782bc7 406
brass_phoenix 34:ae62ebf4d494 407 if (ud_button.is_pressed()) {
brass_phoenix 41:8640b5782bc7 408
brass_phoenix 32:b63b5837bcb1 409 led_blue = 0;
brass_phoenix 41:8640b5782bc7 410
brass_phoenix 41:8640b5782bc7 411
brass_phoenix 32:b63b5837bcb1 412 double main_cur_angle = main_motor.get_current_angle();
brass_phoenix 32:b63b5837bcb1 413 double sec_cur_angle = sec_motor.get_current_angle();
brass_phoenix 41:8640b5782bc7 414
brass_phoenix 32:b63b5837bcb1 415 double main_target, sec_target;
brass_phoenix 41:8640b5782bc7 416
brass_phoenix 32:b63b5837bcb1 417 end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target);
brass_phoenix 41:8640b5782bc7 418
brass_phoenix 32:b63b5837bcb1 419 main_motor.set_target_angle(main_target);
brass_phoenix 32:b63b5837bcb1 420 sec_motor.set_target_angle(sec_target);
brass_phoenix 41:8640b5782bc7 421
brass_phoenix 32:b63b5837bcb1 422 screen.get_screen_handle()->setTextCursor(0, 0);
brass_phoenix 32:b63b5837bcb1 423 screen.get_screen_handle()->printf("M_a: %.6f \n", main_cur_angle);
brass_phoenix 32:b63b5837bcb1 424 screen.get_screen_handle()->printf("S_a: %.6f \n", sec_cur_angle);
brass_phoenix 32:b63b5837bcb1 425 screen.get_screen_handle()->printf("Vx: %.6f \n", main_target);
brass_phoenix 32:b63b5837bcb1 426 screen.get_screen_handle()->printf("Vy: %.6f ", sec_target);
brass_phoenix 32:b63b5837bcb1 427 screen.display();
brass_phoenix 32:b63b5837bcb1 428 }
brass_phoenix 32:b63b5837bcb1 429 }
brass_phoenix 32:b63b5837bcb1 430
brass_phoenix 3:4b19b6cf6cc7 431 void do_state_failure()
brass_phoenix 3:4b19b6cf6cc7 432 {
brass_phoenix 2:141cfcafe72b 433 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 434 last_state = current_state;
brass_phoenix 2:141cfcafe72b 435 // State just changed.
brass_phoenix 2:141cfcafe72b 436 // Update the display.
brass_phoenix 2:141cfcafe72b 437 led_red = 0;
brass_phoenix 2:141cfcafe72b 438 led_green = 1;
brass_phoenix 30:a45bbfa6bd22 439 led_blue = 1;
brass_phoenix 6:bfc6e68774f5 440 screen.clear_display();
brass_phoenix 11:d980e0e581db 441 screen.display_state_name("STOP");
brass_phoenix 2:141cfcafe72b 442 }
brass_phoenix 3:4b19b6cf6cc7 443
brass_phoenix 2:141cfcafe72b 444 // Stop the motors!
brass_phoenix 12:0c10396d0615 445 main_motor.stop();
brass_phoenix 12:0c10396d0615 446 sec_motor.stop();
brass_phoenix 2:141cfcafe72b 447 }
brass_phoenix 2:141cfcafe72b 448
brass_phoenix 2:141cfcafe72b 449
brass_phoenix 1:cfa5abca6d43 450 void main_loop()
brass_phoenix 1:cfa5abca6d43 451 {
brass_phoenix 1:cfa5abca6d43 452 ud_button.update();
brass_phoenix 1:cfa5abca6d43 453 lr_button.update();
brass_phoenix 1:cfa5abca6d43 454 p_button.update();
brass_phoenix 3:4b19b6cf6cc7 455
brass_phoenix 1:cfa5abca6d43 456 switch (current_state) {
brass_phoenix 1:cfa5abca6d43 457 case waiting:
brass_phoenix 2:141cfcafe72b 458 do_state_waiting();
brass_phoenix 1:cfa5abca6d43 459 break;
brass_phoenix 1:cfa5abca6d43 460 case calib_motor:
brass_phoenix 2:141cfcafe72b 461 do_state_calib_motor();
brass_phoenix 1:cfa5abca6d43 462 break;
brass_phoenix 1:cfa5abca6d43 463 case calib_bicep1:
brass_phoenix 2:141cfcafe72b 464 do_state_calib_bicep1();
brass_phoenix 1:cfa5abca6d43 465 break;
brass_phoenix 1:cfa5abca6d43 466 case calib_bicep2:
brass_phoenix 2:141cfcafe72b 467 do_state_calib_bicep2();
brass_phoenix 1:cfa5abca6d43 468 break;
brass_phoenix 1:cfa5abca6d43 469 case homing:
brass_phoenix 2:141cfcafe72b 470 do_state_homing();
brass_phoenix 1:cfa5abca6d43 471 break;
brass_phoenix 1:cfa5abca6d43 472 case operation:
brass_phoenix 2:141cfcafe72b 473 do_state_operation();
brass_phoenix 1:cfa5abca6d43 474 break;
brass_phoenix 32:b63b5837bcb1 475 case demo:
brass_phoenix 32:b63b5837bcb1 476 do_state_demo();
brass_phoenix 32:b63b5837bcb1 477 break;
brass_phoenix 1:cfa5abca6d43 478 case failure:
brass_phoenix 2:141cfcafe72b 479 do_state_failure();
brass_phoenix 1:cfa5abca6d43 480 break;
brass_phoenix 1:cfa5abca6d43 481 }
brass_phoenix 3:4b19b6cf6cc7 482
brass_phoenix 2:141cfcafe72b 483 // Check if the panic button was pressed.
brass_phoenix 2:141cfcafe72b 484 // Doesn't matter in which state we are, we need to go to failure.
brass_phoenix 2:141cfcafe72b 485 if (p_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 486 current_state = failure;
brass_phoenix 3:4b19b6cf6cc7 487 }
brass_phoenix 1:cfa5abca6d43 488 }
MAHCSnijders 0:7d25c2ade6c5 489
brass_phoenix 41:8640b5782bc7 490 void poll_buttons()
brass_phoenix 41:8640b5782bc7 491 {
brass_phoenix 7:e7f808875bc4 492 // We need to poll the pins periodically.
brass_phoenix 7:e7f808875bc4 493 // Normally one would use rise and fall interrupts, so this wouldn't be
brass_phoenix 7:e7f808875bc4 494 // needed. But the buttons we use generate so much chatter that
brass_phoenix 7:e7f808875bc4 495 // sometimes a rising or a falling edge doesn't get registered.
brass_phoenix 7:e7f808875bc4 496 // With all the confusion that accompanies it.
brass_phoenix 7:e7f808875bc4 497 ud_button.poll_pin();
brass_phoenix 7:e7f808875bc4 498 lr_button.poll_pin();
brass_phoenix 7:e7f808875bc4 499 p_button.poll_pin();
brass_phoenix 7:e7f808875bc4 500 }
brass_phoenix 7:e7f808875bc4 501
brass_phoenix 1:cfa5abca6d43 502 int main()
brass_phoenix 1:cfa5abca6d43 503 {
brass_phoenix 1:cfa5abca6d43 504 led_red = 1;
brass_phoenix 9:27d00b64076e 505 led_green = 1;
brass_phoenix 9:27d00b64076e 506 led_blue = 1;
brass_phoenix 41:8640b5782bc7 507
brass_phoenix 10:b165ccd11afd 508 screen.clear_display();
brass_phoenix 41:8640b5782bc7 509
brass_phoenix 12:0c10396d0615 510 main_motor.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 12:0c10396d0615 511 sec_motor.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 41:8640b5782bc7 512
brass_phoenix 12:0c10396d0615 513 // One of the motors is reversed in the electronics.
brass_phoenix 12:0c10396d0615 514 // This is fixed in the motor controll board, so we have to account
brass_phoenix 12:0c10396d0615 515 // for it in software.
brass_phoenix 19:53b9729fbab5 516 main_motor.set_extra_reduction_ratio(-main_gear_ratio);
brass_phoenix 19:53b9729fbab5 517 sec_motor.set_extra_reduction_ratio(sec_gear_ratio);
brass_phoenix 41:8640b5782bc7 518
brass_phoenix 29:77fee8a01529 519 // Set the maximum speed for both motors.
brass_phoenix 29:77fee8a01529 520 main_motor.set_max_speed(0.5);
brass_phoenix 29:77fee8a01529 521 sec_motor.set_max_speed(0.5);
brass_phoenix 41:8640b5782bc7 522
brass_phoenix 8:9090ab7c19a8 523 // Start the motor controller at the desired frequency.
brass_phoenix 12:0c10396d0615 524 main_motor.start(pid_period);
brass_phoenix 12:0c10396d0615 525 sec_motor.start(pid_period);
brass_phoenix 3:4b19b6cf6cc7 526
brass_phoenix 2:141cfcafe72b 527 // Start in the waiting state.
brass_phoenix 1:cfa5abca6d43 528 current_state = waiting;
brass_phoenix 4:5a44ab7e94b3 529 // Pretend we come from the operation state.
brass_phoenix 4:5a44ab7e94b3 530 // So that the waiting state knows it just got started.
brass_phoenix 4:5a44ab7e94b3 531 last_state = operation;
brass_phoenix 41:8640b5782bc7 532
brass_phoenix 39:f119ca6fc821 533 emg_1.start(emg_period);
brass_phoenix 39:f119ca6fc821 534 emg_2.start(emg_period);
brass_phoenix 41:8640b5782bc7 535
brass_phoenix 7:e7f808875bc4 536 // Start the button polling ticker.
brass_phoenix 7:e7f808875bc4 537 button_ticker.attach(&poll_buttons, button_poll_interval);
brass_phoenix 3:4b19b6cf6cc7 538
brass_phoenix 1:cfa5abca6d43 539 while (true) {
brass_phoenix 1:cfa5abca6d43 540 main_loop();
brass_phoenix 41:8640b5782bc7 541
brass_phoenix 1:cfa5abca6d43 542 wait(main_loop_wait_time);
brass_phoenix 1:cfa5abca6d43 543 }
brass_phoenix 1:cfa5abca6d43 544 }