Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@34:ae62ebf4d494, 2018-11-01 (annotated)

Committer:
brass_phoenix
Date:
Thu Nov 01 14:55:47 2018 +0000
Revision:
34:ae62ebf4d494
Parent:
33:543debddb3a9
Child:
39:f119ca6fc821
+ DEMO MODE WORKS (without screen arrows)

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