Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@30:a45bbfa6bd22, 2018-11-01 (annotated)

Committer:
brass_phoenix
Date:
Thu Nov 01 13:01:54 2018 +0000
Revision:
30:a45bbfa6bd22
Parent:
29:77fee8a01529
Child:
32:b63b5837bcb1
+ Added a bit of breathing room for the motors while they wait for the other motor to be calibrated.

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