Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@33:543debddb3a9, 2018-11-01 (annotated)

Committer:
brass_phoenix
Date:
Thu Nov 01 14:39:55 2018 +0000
Revision:
33:543debddb3a9
Parent:
32:b63b5837bcb1
Child:
34:ae62ebf4d494
+ Motor restriction angles are now correcter than before.

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 16:9c5ef6fe6780 8 #include "motor_calibration.h"
brass_phoenix 20:af1a6cd7469b 9 #include "forward_kinematics.h"
brass_phoenix 21:d541303a2ea6 10 #include "inverse_kinematics.h"
brass_phoenix 32:b63b5837bcb1 11 #include "end_effector_control.h"
MAHCSnijders 0:7d25c2ade6c5 12
brass_phoenix 1:cfa5abca6d43 13
brass_phoenix 32:b63b5837bcb1 14 enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, demo, failure}; // The possible states of the state machine
MAHCSnijders 0:7d25c2ade6c5 15
MAHCSnijders 0:7d25c2ade6c5 16 // Global variables
brass_phoenix 1:cfa5abca6d43 17
brass_phoenix 8:9090ab7c19a8 18
brass_phoenix 12:0c10396d0615 19 Motor main_motor(D6, D7, D13, D12);
brass_phoenix 12:0c10396d0615 20 Motor sec_motor(D5, D4, D10, D11);
brass_phoenix 12:0c10396d0615 21
brass_phoenix 1:cfa5abca6d43 22
brass_phoenix 13:88967c004446 23 AnalogIn potmeter1(A5); // Analoge input van potmeter 1 -> Motor 1
brass_phoenix 13:88967c004446 24 AnalogIn potmeter2(A4); // Analoge input van potmeter 2 -> Motor 2
brass_phoenix 13:88967c004446 25
brass_phoenix 13:88967c004446 26
brass_phoenix 1:cfa5abca6d43 27 States current_state; // Defining the state we are currently in
brass_phoenix 2:141cfcafe72b 28 States last_state; // To detect state changes.
MAHCSnijders 0:7d25c2ade6c5 29 Ticker loop_ticker; // Ticker for the loop function
brass_phoenix 1:cfa5abca6d43 30
brass_phoenix 1:cfa5abca6d43 31 // Order of buttons: up_down, left_right, panic
brass_phoenix 1:cfa5abca6d43 32 // D2, D3, D8
brass_phoenix 7:e7f808875bc4 33 Button ud_button(D2);
brass_phoenix 2:141cfcafe72b 34 Button lr_button(D3);
brass_phoenix 7:e7f808875bc4 35 Button p_button(D8);
brass_phoenix 7:e7f808875bc4 36
brass_phoenix 7:e7f808875bc4 37 Ticker button_ticker;
MAHCSnijders 0:7d25c2ade6c5 38
brass_phoenix 1:cfa5abca6d43 39 DigitalOut led_red(LED_RED);
brass_phoenix 1:cfa5abca6d43 40 DigitalOut led_green(LED_GREEN);
brass_phoenix 9:27d00b64076e 41 DigitalOut led_blue(LED_BLUE);
brass_phoenix 1:cfa5abca6d43 42
brass_phoenix 3:4b19b6cf6cc7 43 // The last arguent is the reset pin.
brass_phoenix 3:4b19b6cf6cc7 44 // The screen doesn't use it, but the library requires it
brass_phoenix 3:4b19b6cf6cc7 45 // So pick a pin we don't use.
brass_phoenix 3:4b19b6cf6cc7 46 Screen screen(D14, D15, D9);
brass_phoenix 2:141cfcafe72b 47
brass_phoenix 12:0c10396d0615 48 // Which direction the emg will control the arm.
brass_phoenix 12:0c10396d0615 49 // Up or down.
brass_phoenix 12:0c10396d0615 50 // Left or right.
brass_phoenix 12:0c10396d0615 51 bool control_goes_up = false;
brass_phoenix 12:0c10396d0615 52 bool control_goes_right = false;
brass_phoenix 12:0c10396d0615 53
brass_phoenix 3:4b19b6cf6cc7 54
brass_phoenix 3:4b19b6cf6cc7 55 void do_state_waiting()
brass_phoenix 3:4b19b6cf6cc7 56 {
brass_phoenix 4:5a44ab7e94b3 57 if(last_state != current_state) {
brass_phoenix 4:5a44ab7e94b3 58 last_state = current_state;
brass_phoenix 4:5a44ab7e94b3 59 // State just changed to this one.
brass_phoenix 4:5a44ab7e94b3 60
brass_phoenix 4:5a44ab7e94b3 61 led_green = 1;
brass_phoenix 6:bfc6e68774f5 62 screen.clear_display();
brass_phoenix 4:5a44ab7e94b3 63 screen.display_state_name("Waiting");
brass_phoenix 6:bfc6e68774f5 64 screen.get_screen_handle()->printf(" Press to start ");
brass_phoenix 6:bfc6e68774f5 65 screen.get_screen_handle()->printf(" | ");
brass_phoenix 6:bfc6e68774f5 66 screen.get_screen_handle()->printf(" V ");
brass_phoenix 6:bfc6e68774f5 67 screen.display();
brass_phoenix 4:5a44ab7e94b3 68 }
brass_phoenix 4:5a44ab7e94b3 69
brass_phoenix 9:27d00b64076e 70 if (ud_button.has_just_been_pressed()) {
brass_phoenix 2:141cfcafe72b 71 current_state = calib_motor;
brass_phoenix 2:141cfcafe72b 72 }
brass_phoenix 2:141cfcafe72b 73 }
brass_phoenix 2:141cfcafe72b 74
brass_phoenix 3:4b19b6cf6cc7 75 void do_state_calib_motor()
brass_phoenix 3:4b19b6cf6cc7 76 {
brass_phoenix 16:9c5ef6fe6780 77 static double main_last_angle;
brass_phoenix 16:9c5ef6fe6780 78 static double sec_last_angle;
brass_phoenix 16:9c5ef6fe6780 79 static int main_iterations_not_moving;
brass_phoenix 16:9c5ef6fe6780 80 static int sec_iterations_not_moving;
brass_phoenix 16:9c5ef6fe6780 81 static bool main_is_calibrated;
brass_phoenix 16:9c5ef6fe6780 82 static bool sec_is_calibrated;
brass_phoenix 16:9c5ef6fe6780 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 28:25917b26022c 88 led_green = 1;
brass_phoenix 28:25917b26022c 89 led_blue = 1;
brass_phoenix 6:bfc6e68774f5 90 screen.clear_display();
brass_phoenix 5:2632dfc8454c 91 screen.display_state_name("Motor calibration");
brass_phoenix 16:9c5ef6fe6780 92
brass_phoenix 16:9c5ef6fe6780 93 main_last_angle = -10;
brass_phoenix 16:9c5ef6fe6780 94 sec_last_angle = -10;
brass_phoenix 16:9c5ef6fe6780 95 main_iterations_not_moving = 0;
brass_phoenix 16:9c5ef6fe6780 96 sec_iterations_not_moving = 0;
brass_phoenix 16:9c5ef6fe6780 97 main_is_calibrated = false;
brass_phoenix 16:9c5ef6fe6780 98 sec_is_calibrated = false;
brass_phoenix 2:141cfcafe72b 99 }
brass_phoenix 9:27d00b64076e 100
brass_phoenix 16:9c5ef6fe6780 101 if (!main_is_calibrated) {
brass_phoenix 33:543debddb3a9 102 led_green = 1;
brass_phoenix 16:9c5ef6fe6780 103 main_is_calibrated = calibrate_motor(main_motor, main_last_angle, main_iterations_not_moving);
brass_phoenix 16:9c5ef6fe6780 104 if (main_is_calibrated) {
brass_phoenix 33:543debddb3a9 105 main_motor.define_current_angle_as_x_radians(main_motor_calibration_angle);
brass_phoenix 33:543debddb3a9 106 //main_motor.set_target_angle(main_motor_calibration_angle - 0.2); // Give the arm some breathing space.
brass_phoenix 28:25917b26022c 107 led_green = 0;
brass_phoenix 16:9c5ef6fe6780 108 }
brass_phoenix 16:9c5ef6fe6780 109 }
brass_phoenix 16:9c5ef6fe6780 110 if (!sec_is_calibrated) {
brass_phoenix 33:543debddb3a9 111 led_blue = 1;
brass_phoenix 16:9c5ef6fe6780 112 sec_is_calibrated = calibrate_motor(sec_motor, sec_last_angle, sec_iterations_not_moving);
brass_phoenix 16:9c5ef6fe6780 113 if (sec_is_calibrated) {
brass_phoenix 33:543debddb3a9 114 sec_motor.define_current_angle_as_x_radians(sec_motor_calibration_angle); // -42 degrees.
brass_phoenix 33:543debddb3a9 115 //main_motor.set_target_angle(sec_motor_calibration_angle + 0.2); // Give the arm some breathing space.
brass_phoenix 28:25917b26022c 116 led_blue = 0;
brass_phoenix 16:9c5ef6fe6780 117 }
brass_phoenix 16:9c5ef6fe6780 118 }
brass_phoenix 16:9c5ef6fe6780 119
brass_phoenix 28:25917b26022c 120 screen.get_screen_handle()->setTextCursor(0, 8);
brass_phoenix 28:25917b26022c 121 screen.get_screen_handle()->printf("M: %i \n", main_iterations_not_moving);
brass_phoenix 28:25917b26022c 122 screen.get_screen_handle()->printf("S: %i \n", sec_iterations_not_moving);
brass_phoenix 28:25917b26022c 123 screen.display();
brass_phoenix 28:25917b26022c 124
brass_phoenix 16:9c5ef6fe6780 125 if (main_is_calibrated && sec_is_calibrated) {
brass_phoenix 19:53b9729fbab5 126 current_state = homing;
brass_phoenix 9:27d00b64076e 127 }
brass_phoenix 2:141cfcafe72b 128 }
brass_phoenix 2:141cfcafe72b 129
brass_phoenix 25:cc81f2120eda 130 void do_state_homing()
brass_phoenix 25:cc81f2120eda 131 {
brass_phoenix 25:cc81f2120eda 132 const double home_x = 0.6524; // Meters.
brass_phoenix 25:cc81f2120eda 133 const double home_y = 0.3409;
brass_phoenix 25:cc81f2120eda 134
brass_phoenix 26:a8f4a117cc0d 135 double main_home;
brass_phoenix 26:a8f4a117cc0d 136 double sec_home;
brass_phoenix 25:cc81f2120eda 137
brass_phoenix 25:cc81f2120eda 138 if(last_state != current_state) {
brass_phoenix 25:cc81f2120eda 139 last_state = current_state;
brass_phoenix 25:cc81f2120eda 140 // State just changed to this one.
brass_phoenix 25:cc81f2120eda 141 screen.clear_display();
brass_phoenix 25:cc81f2120eda 142 screen.display_state_name("Homing");
brass_phoenix 25:cc81f2120eda 143
brass_phoenix 26:a8f4a117cc0d 144 inverse_kinematics(home_x, home_y, main_home, sec_home);
brass_phoenix 25:cc81f2120eda 145
brass_phoenix 25:cc81f2120eda 146 main_motor.set_target_angle(main_home);
brass_phoenix 25:cc81f2120eda 147 sec_motor.set_target_angle(sec_home);
brass_phoenix 33:543debddb3a9 148
brass_phoenix 33:543debddb3a9 149 screen.get_screen_handle()->setTextCursor(0, 8);
brass_phoenix 33:543debddb3a9 150 screen.get_screen_handle()->printf("Ma: %.6f \n", main_home);
brass_phoenix 33:543debddb3a9 151 screen.get_screen_handle()->printf("Sa: %.6f \n", sec_home);
brass_phoenix 33:543debddb3a9 152 screen.display();
brass_phoenix 25:cc81f2120eda 153 }
brass_phoenix 25:cc81f2120eda 154
brass_phoenix 25:cc81f2120eda 155 if (ud_button.has_just_been_pressed()) {
brass_phoenix 25:cc81f2120eda 156 current_state = calib_bicep1;
brass_phoenix 25:cc81f2120eda 157 }
brass_phoenix 32:b63b5837bcb1 158 if (lr_button.has_just_been_pressed()) {
brass_phoenix 32:b63b5837bcb1 159 current_state = demo;
brass_phoenix 32:b63b5837bcb1 160 }
brass_phoenix 25:cc81f2120eda 161 }
brass_phoenix 25:cc81f2120eda 162
brass_phoenix 3:4b19b6cf6cc7 163 void do_state_calib_bicep1()
brass_phoenix 3:4b19b6cf6cc7 164 {
brass_phoenix 2:141cfcafe72b 165 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 166 last_state = current_state;
brass_phoenix 2:141cfcafe72b 167 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 168 screen.clear_display();
brass_phoenix 5:2632dfc8454c 169 screen.display_state_name("EMG 1 calibration");
brass_phoenix 2:141cfcafe72b 170 }
brass_phoenix 9:27d00b64076e 171
brass_phoenix 9:27d00b64076e 172 if (ud_button.has_just_been_pressed()) {
brass_phoenix 9:27d00b64076e 173 current_state = calib_bicep2;
brass_phoenix 9:27d00b64076e 174 }
brass_phoenix 2:141cfcafe72b 175 }
brass_phoenix 2:141cfcafe72b 176
brass_phoenix 3:4b19b6cf6cc7 177 void do_state_calib_bicep2()
brass_phoenix 3:4b19b6cf6cc7 178 {
brass_phoenix 2:141cfcafe72b 179 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 180 last_state = current_state;
brass_phoenix 2:141cfcafe72b 181 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 182 screen.clear_display();
brass_phoenix 5:2632dfc8454c 183 screen.display_state_name("EMG 2 calibration");
brass_phoenix 2:141cfcafe72b 184 }
brass_phoenix 9:27d00b64076e 185
brass_phoenix 9:27d00b64076e 186 if (ud_button.has_just_been_pressed()) {
brass_phoenix 9:27d00b64076e 187 current_state = homing;
brass_phoenix 9:27d00b64076e 188 }
brass_phoenix 2:141cfcafe72b 189 }
brass_phoenix 2:141cfcafe72b 190
brass_phoenix 3:4b19b6cf6cc7 191 void do_state_operation()
brass_phoenix 3:4b19b6cf6cc7 192 {
brass_phoenix 21:d541303a2ea6 193 static bool debug_forward_kinematics;
brass_phoenix 21:d541303a2ea6 194
brass_phoenix 2:141cfcafe72b 195 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 196 last_state = current_state;
brass_phoenix 2:141cfcafe72b 197 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 198 screen.clear_display();
brass_phoenix 5:2632dfc8454c 199 screen.display_state_name("Normal operation");
brass_phoenix 15:f65b4566193e 200
brass_phoenix 15:f65b4566193e 201 control_goes_up = true;
brass_phoenix 15:f65b4566193e 202 control_goes_right = true;
brass_phoenix 15:f65b4566193e 203
brass_phoenix 15:f65b4566193e 204 screen.display_up_down_arrow(control_goes_up);
brass_phoenix 15:f65b4566193e 205 screen.display_left_right_arrow(control_goes_right);
brass_phoenix 21:d541303a2ea6 206
brass_phoenix 21:d541303a2ea6 207 debug_forward_kinematics = true;
brass_phoenix 2:141cfcafe72b 208 }
brass_phoenix 9:27d00b64076e 209
brass_phoenix 21:d541303a2ea6 210 if (debug_forward_kinematics) {
brass_phoenix 21:d541303a2ea6 211 // Using potmeters for debugging purposes;
brass_phoenix 21:d541303a2ea6 212 double main_angle = ((potmeter1.read() * 2) - 1) * PI;
brass_phoenix 21:d541303a2ea6 213 double sec_angle = ((potmeter2.read() * 2) - 1) * PI;
brass_phoenix 21:d541303a2ea6 214
brass_phoenix 21:d541303a2ea6 215 double e_x = 0.0;
brass_phoenix 21:d541303a2ea6 216 double e_y = 0.0;
brass_phoenix 21:d541303a2ea6 217
brass_phoenix 21:d541303a2ea6 218 forward_kinematics(main_angle, sec_angle, e_x, e_y);
brass_phoenix 21:d541303a2ea6 219
brass_phoenix 21:d541303a2ea6 220 screen.get_screen_handle()->setTextCursor(0, 0);
brass_phoenix 21:d541303a2ea6 221 screen.get_screen_handle()->printf("M_a: %.6f \n", main_angle);
brass_phoenix 21:d541303a2ea6 222 screen.get_screen_handle()->printf("S_a: %.6f \n", sec_angle);
brass_phoenix 21:d541303a2ea6 223 screen.get_screen_handle()->printf("X: %.6f \n", e_x);
brass_phoenix 21:d541303a2ea6 224 screen.get_screen_handle()->printf("Y: %.6f ", e_y);
brass_phoenix 21:d541303a2ea6 225 screen.display();
brass_phoenix 20:af1a6cd7469b 226
brass_phoenix 21:d541303a2ea6 227 } else {
brass_phoenix 21:d541303a2ea6 228 // Using potmeters for debugging purposes;
brass_phoenix 21:d541303a2ea6 229 double e_x = potmeter1.read();
brass_phoenix 21:d541303a2ea6 230 double e_y = potmeter2.read();
brass_phoenix 21:d541303a2ea6 231
brass_phoenix 21:d541303a2ea6 232 double main_angle = 0.0;
brass_phoenix 21:d541303a2ea6 233 double sec_angle = 0.0;
brass_phoenix 21:d541303a2ea6 234
brass_phoenix 21:d541303a2ea6 235 inverse_kinematics(e_x, e_y, main_angle, sec_angle);
brass_phoenix 21:d541303a2ea6 236
brass_phoenix 21:d541303a2ea6 237 screen.get_screen_handle()->setTextCursor(0, 0);
brass_phoenix 21:d541303a2ea6 238 screen.get_screen_handle()->printf("E_x: %.6f \n", e_x);
brass_phoenix 21:d541303a2ea6 239 screen.get_screen_handle()->printf("E_y: %.6f \n", e_y);
brass_phoenix 21:d541303a2ea6 240 screen.get_screen_handle()->printf("M_a: %.6f \n", main_angle);
brass_phoenix 21:d541303a2ea6 241 screen.get_screen_handle()->printf("S_a: %.6f ", sec_angle);
brass_phoenix 21:d541303a2ea6 242 screen.display();
brass_phoenix 21:d541303a2ea6 243 }
brass_phoenix 20:af1a6cd7469b 244
brass_phoenix 21:d541303a2ea6 245 if (lr_button.has_just_been_pressed()) {
brass_phoenix 21:d541303a2ea6 246 debug_forward_kinematics = !debug_forward_kinematics;
brass_phoenix 21:d541303a2ea6 247 }
brass_phoenix 20:af1a6cd7469b 248
brass_phoenix 19:53b9729fbab5 249 /*
brass_phoenix 14:b97e7a41ec23 250 double main_target = ((potmeter1.read() * 2) - 1) * PI;
brass_phoenix 13:88967c004446 251 main_motor.set_target_angle(main_target);
brass_phoenix 14:b97e7a41ec23 252 double sec_target = ((potmeter2.read() * 2) - 1) * PI;
brass_phoenix 13:88967c004446 253 sec_motor.set_target_angle(sec_target);
brass_phoenix 13:88967c004446 254
brass_phoenix 25:cc81f2120eda 255 if (lr_button.has_just_been_pressed()) {
brass_phoenix 25:cc81f2120eda 256 control_goes_right = !control_goes_right;
brass_phoenix 25:cc81f2120eda 257 screen.display_left_right_arrow(control_goes_right);
brass_phoenix 25:cc81f2120eda 258 }
brass_phoenix 25:cc81f2120eda 259 */
brass_phoenix 25:cc81f2120eda 260
brass_phoenix 9:27d00b64076e 261 if (ud_button.has_just_been_pressed()) {
brass_phoenix 12:0c10396d0615 262 control_goes_up = !control_goes_up;
brass_phoenix 25:cc81f2120eda 263 control_goes_right = !control_goes_right;
brass_phoenix 15:f65b4566193e 264 screen.display_up_down_arrow(control_goes_up);
brass_phoenix 15:f65b4566193e 265 screen.display_left_right_arrow(control_goes_right);
brass_phoenix 9:27d00b64076e 266 }
brass_phoenix 2:141cfcafe72b 267 }
brass_phoenix 2:141cfcafe72b 268
brass_phoenix 32:b63b5837bcb1 269 void do_state_demo() {
brass_phoenix 32:b63b5837bcb1 270 if(last_state != current_state) {
brass_phoenix 32:b63b5837bcb1 271 last_state = current_state;
brass_phoenix 32:b63b5837bcb1 272 // State just changed.
brass_phoenix 32:b63b5837bcb1 273 // Update the display.
brass_phoenix 32:b63b5837bcb1 274 led_red = 1;
brass_phoenix 32:b63b5837bcb1 275 led_green = 0;
brass_phoenix 32:b63b5837bcb1 276 led_blue = 1;
brass_phoenix 32:b63b5837bcb1 277 screen.clear_display();
brass_phoenix 32:b63b5837bcb1 278 screen.display_state_name("Demo mode!");
brass_phoenix 32:b63b5837bcb1 279
brass_phoenix 32:b63b5837bcb1 280 screen.display_up_down_arrow(control_goes_up);
brass_phoenix 32:b63b5837bcb1 281 screen.display_left_right_arrow(control_goes_right);
brass_phoenix 32:b63b5837bcb1 282 }
brass_phoenix 32:b63b5837bcb1 283
brass_phoenix 32:b63b5837bcb1 284 if (lr_button.has_just_been_pressed()) {
brass_phoenix 32:b63b5837bcb1 285 control_goes_up = !control_goes_up;
brass_phoenix 32:b63b5837bcb1 286 control_goes_right = !control_goes_right;
brass_phoenix 32:b63b5837bcb1 287 screen.display_up_down_arrow(control_goes_up);
brass_phoenix 32:b63b5837bcb1 288 screen.display_left_right_arrow(control_goes_right);
brass_phoenix 32:b63b5837bcb1 289 }
brass_phoenix 32:b63b5837bcb1 290
brass_phoenix 32:b63b5837bcb1 291 if (ud_button.has_just_been_pressed()) {
brass_phoenix 32:b63b5837bcb1 292
brass_phoenix 32:b63b5837bcb1 293 led_blue = 0;
brass_phoenix 32:b63b5837bcb1 294
brass_phoenix 32:b63b5837bcb1 295 double speed_x = 0.01;
brass_phoenix 32:b63b5837bcb1 296 double speed_y = 0.01;
brass_phoenix 32:b63b5837bcb1 297
brass_phoenix 32:b63b5837bcb1 298 if (!control_goes_right) {
brass_phoenix 32:b63b5837bcb1 299 speed_x = -speed_x;
brass_phoenix 32:b63b5837bcb1 300 }
brass_phoenix 32:b63b5837bcb1 301 if (!control_goes_up) {
brass_phoenix 32:b63b5837bcb1 302 speed_y = -speed_y;
brass_phoenix 32:b63b5837bcb1 303 }
brass_phoenix 32:b63b5837bcb1 304
brass_phoenix 32:b63b5837bcb1 305
brass_phoenix 32:b63b5837bcb1 306 double main_cur_angle = main_motor.get_current_angle();
brass_phoenix 32:b63b5837bcb1 307 double sec_cur_angle = sec_motor.get_current_angle();
brass_phoenix 32:b63b5837bcb1 308
brass_phoenix 32:b63b5837bcb1 309 double main_target, sec_target;
brass_phoenix 32:b63b5837bcb1 310
brass_phoenix 32:b63b5837bcb1 311 end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target);
brass_phoenix 32:b63b5837bcb1 312
brass_phoenix 32:b63b5837bcb1 313 main_motor.set_target_angle(main_target);
brass_phoenix 32:b63b5837bcb1 314 sec_motor.set_target_angle(sec_target);
brass_phoenix 32:b63b5837bcb1 315
brass_phoenix 32:b63b5837bcb1 316 screen.get_screen_handle()->setTextCursor(0, 0);
brass_phoenix 32:b63b5837bcb1 317 screen.get_screen_handle()->printf("M_a: %.6f \n", main_cur_angle);
brass_phoenix 32:b63b5837bcb1 318 screen.get_screen_handle()->printf("S_a: %.6f \n", sec_cur_angle);
brass_phoenix 32:b63b5837bcb1 319 screen.get_screen_handle()->printf("Vx: %.6f \n", main_target);
brass_phoenix 32:b63b5837bcb1 320 screen.get_screen_handle()->printf("Vy: %.6f ", sec_target);
brass_phoenix 32:b63b5837bcb1 321 screen.display();
brass_phoenix 32:b63b5837bcb1 322 }
brass_phoenix 32:b63b5837bcb1 323 }
brass_phoenix 32:b63b5837bcb1 324
brass_phoenix 3:4b19b6cf6cc7 325 void do_state_failure()
brass_phoenix 3:4b19b6cf6cc7 326 {
brass_phoenix 2:141cfcafe72b 327 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 328 last_state = current_state;
brass_phoenix 2:141cfcafe72b 329 // State just changed.
brass_phoenix 2:141cfcafe72b 330 // Update the display.
brass_phoenix 2:141cfcafe72b 331 led_red = 0;
brass_phoenix 2:141cfcafe72b 332 led_green = 1;
brass_phoenix 30:a45bbfa6bd22 333 led_blue = 1;
brass_phoenix 6:bfc6e68774f5 334 screen.clear_display();
brass_phoenix 11:d980e0e581db 335 screen.display_state_name("STOP");
brass_phoenix 2:141cfcafe72b 336 }
brass_phoenix 3:4b19b6cf6cc7 337
brass_phoenix 2:141cfcafe72b 338 // Stop the motors!
brass_phoenix 12:0c10396d0615 339 main_motor.stop();
brass_phoenix 12:0c10396d0615 340 sec_motor.stop();
brass_phoenix 2:141cfcafe72b 341 }
brass_phoenix 2:141cfcafe72b 342
brass_phoenix 2:141cfcafe72b 343
brass_phoenix 1:cfa5abca6d43 344 void main_loop()
brass_phoenix 1:cfa5abca6d43 345 {
brass_phoenix 1:cfa5abca6d43 346 ud_button.update();
brass_phoenix 1:cfa5abca6d43 347 lr_button.update();
brass_phoenix 1:cfa5abca6d43 348 p_button.update();
brass_phoenix 3:4b19b6cf6cc7 349
brass_phoenix 1:cfa5abca6d43 350 switch (current_state) {
brass_phoenix 1:cfa5abca6d43 351 case waiting:
brass_phoenix 2:141cfcafe72b 352 do_state_waiting();
brass_phoenix 1:cfa5abca6d43 353 break;
brass_phoenix 1:cfa5abca6d43 354 case calib_motor:
brass_phoenix 2:141cfcafe72b 355 do_state_calib_motor();
brass_phoenix 1:cfa5abca6d43 356 break;
brass_phoenix 1:cfa5abca6d43 357 case calib_bicep1:
brass_phoenix 2:141cfcafe72b 358 do_state_calib_bicep1();
brass_phoenix 1:cfa5abca6d43 359 break;
brass_phoenix 1:cfa5abca6d43 360 case calib_bicep2:
brass_phoenix 2:141cfcafe72b 361 do_state_calib_bicep2();
brass_phoenix 1:cfa5abca6d43 362 break;
brass_phoenix 1:cfa5abca6d43 363 case homing:
brass_phoenix 2:141cfcafe72b 364 do_state_homing();
brass_phoenix 1:cfa5abca6d43 365 break;
brass_phoenix 1:cfa5abca6d43 366 case operation:
brass_phoenix 2:141cfcafe72b 367 do_state_operation();
brass_phoenix 1:cfa5abca6d43 368 break;
brass_phoenix 32:b63b5837bcb1 369 case demo:
brass_phoenix 32:b63b5837bcb1 370 do_state_demo();
brass_phoenix 32:b63b5837bcb1 371 break;
brass_phoenix 1:cfa5abca6d43 372 case failure:
brass_phoenix 2:141cfcafe72b 373 do_state_failure();
brass_phoenix 1:cfa5abca6d43 374 break;
brass_phoenix 1:cfa5abca6d43 375 }
brass_phoenix 3:4b19b6cf6cc7 376
brass_phoenix 2:141cfcafe72b 377 // Check if the panic button was pressed.
brass_phoenix 2:141cfcafe72b 378 // Doesn't matter in which state we are, we need to go to failure.
brass_phoenix 2:141cfcafe72b 379 if (p_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 380 current_state = failure;
brass_phoenix 3:4b19b6cf6cc7 381 }
brass_phoenix 1:cfa5abca6d43 382 }
MAHCSnijders 0:7d25c2ade6c5 383
brass_phoenix 7:e7f808875bc4 384 void poll_buttons() {
brass_phoenix 7:e7f808875bc4 385 // We need to poll the pins periodically.
brass_phoenix 7:e7f808875bc4 386 // Normally one would use rise and fall interrupts, so this wouldn't be
brass_phoenix 7:e7f808875bc4 387 // needed. But the buttons we use generate so much chatter that
brass_phoenix 7:e7f808875bc4 388 // sometimes a rising or a falling edge doesn't get registered.
brass_phoenix 7:e7f808875bc4 389 // With all the confusion that accompanies it.
brass_phoenix 7:e7f808875bc4 390 ud_button.poll_pin();
brass_phoenix 7:e7f808875bc4 391 lr_button.poll_pin();
brass_phoenix 7:e7f808875bc4 392 p_button.poll_pin();
brass_phoenix 7:e7f808875bc4 393 }
brass_phoenix 7:e7f808875bc4 394
brass_phoenix 1:cfa5abca6d43 395 int main()
brass_phoenix 1:cfa5abca6d43 396 {
brass_phoenix 1:cfa5abca6d43 397 led_red = 1;
brass_phoenix 9:27d00b64076e 398 led_green = 1;
brass_phoenix 9:27d00b64076e 399 led_blue = 1;
brass_phoenix 8:9090ab7c19a8 400
brass_phoenix 10:b165ccd11afd 401 screen.clear_display();
brass_phoenix 10:b165ccd11afd 402
brass_phoenix 12:0c10396d0615 403 main_motor.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 12:0c10396d0615 404 sec_motor.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 12:0c10396d0615 405
brass_phoenix 12:0c10396d0615 406 // One of the motors is reversed in the electronics.
brass_phoenix 12:0c10396d0615 407 // This is fixed in the motor controll board, so we have to account
brass_phoenix 12:0c10396d0615 408 // for it in software.
brass_phoenix 19:53b9729fbab5 409 main_motor.set_extra_reduction_ratio(-main_gear_ratio);
brass_phoenix 19:53b9729fbab5 410 sec_motor.set_extra_reduction_ratio(sec_gear_ratio);
brass_phoenix 19:53b9729fbab5 411
brass_phoenix 29:77fee8a01529 412 // Set the maximum speed for both motors.
brass_phoenix 29:77fee8a01529 413 main_motor.set_max_speed(0.5);
brass_phoenix 29:77fee8a01529 414 sec_motor.set_max_speed(0.5);
brass_phoenix 12:0c10396d0615 415
brass_phoenix 8:9090ab7c19a8 416 // Start the motor controller at the desired frequency.
brass_phoenix 12:0c10396d0615 417 main_motor.start(pid_period);
brass_phoenix 12:0c10396d0615 418 sec_motor.start(pid_period);
brass_phoenix 3:4b19b6cf6cc7 419
brass_phoenix 2:141cfcafe72b 420 // Start in the waiting state.
brass_phoenix 1:cfa5abca6d43 421 current_state = waiting;
brass_phoenix 4:5a44ab7e94b3 422 // Pretend we come from the operation state.
brass_phoenix 4:5a44ab7e94b3 423 // So that the waiting state knows it just got started.
brass_phoenix 4:5a44ab7e94b3 424 last_state = operation;
brass_phoenix 7:e7f808875bc4 425
brass_phoenix 7:e7f808875bc4 426 // Start the button polling ticker.
brass_phoenix 7:e7f808875bc4 427 button_ticker.attach(&poll_buttons, button_poll_interval);
brass_phoenix 3:4b19b6cf6cc7 428
brass_phoenix 1:cfa5abca6d43 429 while (true) {
brass_phoenix 1:cfa5abca6d43 430 main_loop();
brass_phoenix 9:27d00b64076e 431
brass_phoenix 1:cfa5abca6d43 432 wait(main_loop_wait_time);
brass_phoenix 1:cfa5abca6d43 433 }
brass_phoenix 1:cfa5abca6d43 434 }