State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

Committer:
brass_phoenix
Date:
Wed Oct 31 16:19:28 2018 +0000
Revision:
19:53b9729fbab5
Parent:
16:9c5ef6fe6780
Child:
20:af1a6cd7469b
+ Added maximum pwm fraction for the motors.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MAHCSnijders 0:7d25c2ade6c5 1 #include "mbed.h"
MAHCSnijders 0:7d25c2ade6c5 2
brass_phoenix 1:cfa5abca6d43 3 #include "Button.h"
brass_phoenix 3:4b19b6cf6cc7 4 #include "Screen.h"
brass_phoenix 8:9090ab7c19a8 5 #include "motor.h"
brass_phoenix 16:9c5ef6fe6780 6 #include "motor_calibration.h"
MAHCSnijders 0:7d25c2ade6c5 7
brass_phoenix 1:cfa5abca6d43 8
brass_phoenix 1:cfa5abca6d43 9 enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, failure}; // The possible states of the state machine
MAHCSnijders 0:7d25c2ade6c5 10
MAHCSnijders 0:7d25c2ade6c5 11 // Global variables
MAHCSnijders 0:7d25c2ade6c5 12 const double PI = 3.14159265359;
brass_phoenix 1:cfa5abca6d43 13 // Main loop wait time per cycle. This does not influence the motor PID or EMG reading frequencies.
brass_phoenix 12:0c10396d0615 14 const float main_loop_wait_time = 0.01;
brass_phoenix 1:cfa5abca6d43 15
brass_phoenix 7:e7f808875bc4 16 // Time between two button polls. Used to debounce the button presses.
brass_phoenix 12:0c10396d0615 17 const float button_poll_interval = 0.05;
brass_phoenix 7:e7f808875bc4 18
brass_phoenix 8:9090ab7c19a8 19 const float pid_period = 0.001; // PID sample period in seconds.
brass_phoenix 8:9090ab7c19a8 20
brass_phoenix 12:0c10396d0615 21 const float Kp = 10.0;
brass_phoenix 12:0c10396d0615 22 const float Ki = 0.1;
brass_phoenix 12:0c10396d0615 23 const float Kd = 0.5;
brass_phoenix 8:9090ab7c19a8 24
brass_phoenix 12:0c10396d0615 25 Motor main_motor(D6, D7, D13, D12);
brass_phoenix 12:0c10396d0615 26 Motor sec_motor(D5, D4, D10, D11);
brass_phoenix 12:0c10396d0615 27
brass_phoenix 12:0c10396d0615 28 // The motor -> main gear ratio is 25 / 60.
brass_phoenix 12:0c10396d0615 29 // The motor -> secondary gear ratio is 25/50
brass_phoenix 12:0c10396d0615 30 const float main_gear_ratio = 25.0/60.0;
brass_phoenix 12:0c10396d0615 31 const float sec_gear_ratio = 25.0/50.0;
brass_phoenix 8:9090ab7c19a8 32
brass_phoenix 1:cfa5abca6d43 33
brass_phoenix 13:88967c004446 34 AnalogIn potmeter1(A5); // Analoge input van potmeter 1 -> Motor 1
brass_phoenix 13:88967c004446 35 AnalogIn potmeter2(A4); // Analoge input van potmeter 2 -> Motor 2
brass_phoenix 13:88967c004446 36
brass_phoenix 13:88967c004446 37
brass_phoenix 1:cfa5abca6d43 38 States current_state; // Defining the state we are currently in
brass_phoenix 2:141cfcafe72b 39 States last_state; // To detect state changes.
MAHCSnijders 0:7d25c2ade6c5 40 Ticker loop_ticker; // Ticker for the loop function
brass_phoenix 1:cfa5abca6d43 41
brass_phoenix 1:cfa5abca6d43 42 // Order of buttons: up_down, left_right, panic
brass_phoenix 1:cfa5abca6d43 43 // D2, D3, D8
brass_phoenix 7:e7f808875bc4 44 Button ud_button(D2);
brass_phoenix 2:141cfcafe72b 45 Button lr_button(D3);
brass_phoenix 7:e7f808875bc4 46 Button p_button(D8);
brass_phoenix 7:e7f808875bc4 47
brass_phoenix 7:e7f808875bc4 48 Ticker button_ticker;
MAHCSnijders 0:7d25c2ade6c5 49
brass_phoenix 1:cfa5abca6d43 50 DigitalOut led_red(LED_RED);
brass_phoenix 1:cfa5abca6d43 51 DigitalOut led_green(LED_GREEN);
brass_phoenix 9:27d00b64076e 52 DigitalOut led_blue(LED_BLUE);
brass_phoenix 1:cfa5abca6d43 53
brass_phoenix 3:4b19b6cf6cc7 54 // The last arguent is the reset pin.
brass_phoenix 3:4b19b6cf6cc7 55 // The screen doesn't use it, but the library requires it
brass_phoenix 3:4b19b6cf6cc7 56 // So pick a pin we don't use.
brass_phoenix 3:4b19b6cf6cc7 57 Screen screen(D14, D15, D9);
brass_phoenix 2:141cfcafe72b 58
brass_phoenix 12:0c10396d0615 59 // Which direction the emg will control the arm.
brass_phoenix 12:0c10396d0615 60 // Up or down.
brass_phoenix 12:0c10396d0615 61 // Left or right.
brass_phoenix 12:0c10396d0615 62 bool control_goes_up = false;
brass_phoenix 12:0c10396d0615 63 bool control_goes_right = false;
brass_phoenix 12:0c10396d0615 64
brass_phoenix 3:4b19b6cf6cc7 65
brass_phoenix 3:4b19b6cf6cc7 66 void do_state_waiting()
brass_phoenix 3:4b19b6cf6cc7 67 {
brass_phoenix 4:5a44ab7e94b3 68 if(last_state != current_state) {
brass_phoenix 4:5a44ab7e94b3 69 last_state = current_state;
brass_phoenix 4:5a44ab7e94b3 70 // State just changed to this one.
brass_phoenix 4:5a44ab7e94b3 71
brass_phoenix 4:5a44ab7e94b3 72 led_green = 1;
brass_phoenix 6:bfc6e68774f5 73 screen.clear_display();
brass_phoenix 4:5a44ab7e94b3 74 screen.display_state_name("Waiting");
brass_phoenix 6:bfc6e68774f5 75 screen.get_screen_handle()->printf(" Press to start ");
brass_phoenix 6:bfc6e68774f5 76 screen.get_screen_handle()->printf(" | ");
brass_phoenix 6:bfc6e68774f5 77 screen.get_screen_handle()->printf(" V ");
brass_phoenix 6:bfc6e68774f5 78 screen.display();
brass_phoenix 4:5a44ab7e94b3 79 }
brass_phoenix 4:5a44ab7e94b3 80
brass_phoenix 9:27d00b64076e 81 if (ud_button.has_just_been_pressed()) {
brass_phoenix 2:141cfcafe72b 82 current_state = calib_motor;
brass_phoenix 2:141cfcafe72b 83 }
brass_phoenix 2:141cfcafe72b 84 }
brass_phoenix 2:141cfcafe72b 85
brass_phoenix 3:4b19b6cf6cc7 86 void do_state_calib_motor()
brass_phoenix 3:4b19b6cf6cc7 87 {
brass_phoenix 16:9c5ef6fe6780 88 static double main_last_angle;
brass_phoenix 16:9c5ef6fe6780 89 static double sec_last_angle;
brass_phoenix 16:9c5ef6fe6780 90 static int main_iterations_not_moving;
brass_phoenix 16:9c5ef6fe6780 91 static int sec_iterations_not_moving;
brass_phoenix 16:9c5ef6fe6780 92 static bool main_is_calibrated;
brass_phoenix 16:9c5ef6fe6780 93 static bool sec_is_calibrated;
brass_phoenix 16:9c5ef6fe6780 94
brass_phoenix 2:141cfcafe72b 95 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 96 last_state = current_state;
brass_phoenix 2:141cfcafe72b 97 // State just changed to this one.
brass_phoenix 3:4b19b6cf6cc7 98
brass_phoenix 2:141cfcafe72b 99 led_green = 0;
brass_phoenix 6:bfc6e68774f5 100 screen.clear_display();
brass_phoenix 5:2632dfc8454c 101 screen.display_state_name("Motor calibration");
brass_phoenix 16:9c5ef6fe6780 102
brass_phoenix 16:9c5ef6fe6780 103 main_last_angle = -10;
brass_phoenix 16:9c5ef6fe6780 104 sec_last_angle = -10;
brass_phoenix 16:9c5ef6fe6780 105 main_iterations_not_moving = 0;
brass_phoenix 16:9c5ef6fe6780 106 sec_iterations_not_moving = 0;
brass_phoenix 16:9c5ef6fe6780 107 main_is_calibrated = false;
brass_phoenix 16:9c5ef6fe6780 108 sec_is_calibrated = false;
brass_phoenix 2:141cfcafe72b 109 }
brass_phoenix 9:27d00b64076e 110
brass_phoenix 16:9c5ef6fe6780 111 if (!main_is_calibrated) {
brass_phoenix 16:9c5ef6fe6780 112 main_is_calibrated = calibrate_motor(main_motor, main_last_angle, main_iterations_not_moving);
brass_phoenix 16:9c5ef6fe6780 113 if (main_is_calibrated) {
brass_phoenix 16:9c5ef6fe6780 114 main_motor.define_current_angle_as_x_radians(0.785398); // 45 degrees.
brass_phoenix 16:9c5ef6fe6780 115 }
brass_phoenix 16:9c5ef6fe6780 116 }
brass_phoenix 16:9c5ef6fe6780 117 if (!sec_is_calibrated) {
brass_phoenix 16:9c5ef6fe6780 118 sec_is_calibrated = calibrate_motor(sec_motor, sec_last_angle, sec_iterations_not_moving);
brass_phoenix 16:9c5ef6fe6780 119 if (sec_is_calibrated) {
brass_phoenix 16:9c5ef6fe6780 120 sec_motor.define_current_angle_as_x_radians(-0.733038); // -42 degrees.
brass_phoenix 16:9c5ef6fe6780 121 }
brass_phoenix 16:9c5ef6fe6780 122 }
brass_phoenix 16:9c5ef6fe6780 123
brass_phoenix 16:9c5ef6fe6780 124 if (main_is_calibrated && sec_is_calibrated) {
brass_phoenix 19:53b9729fbab5 125 current_state = homing;
brass_phoenix 9:27d00b64076e 126 }
brass_phoenix 2:141cfcafe72b 127 }
brass_phoenix 2:141cfcafe72b 128
brass_phoenix 3:4b19b6cf6cc7 129 void do_state_calib_bicep1()
brass_phoenix 3:4b19b6cf6cc7 130 {
brass_phoenix 2:141cfcafe72b 131 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 132 last_state = current_state;
brass_phoenix 2:141cfcafe72b 133 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 134 screen.clear_display();
brass_phoenix 5:2632dfc8454c 135 screen.display_state_name("EMG 1 calibration");
brass_phoenix 2:141cfcafe72b 136 }
brass_phoenix 9:27d00b64076e 137
brass_phoenix 9:27d00b64076e 138 if (ud_button.has_just_been_pressed()) {
brass_phoenix 9:27d00b64076e 139 current_state = calib_bicep2;
brass_phoenix 9:27d00b64076e 140 }
brass_phoenix 2:141cfcafe72b 141 }
brass_phoenix 2:141cfcafe72b 142
brass_phoenix 3:4b19b6cf6cc7 143 void do_state_calib_bicep2()
brass_phoenix 3:4b19b6cf6cc7 144 {
brass_phoenix 2:141cfcafe72b 145 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 146 last_state = current_state;
brass_phoenix 2:141cfcafe72b 147 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 148 screen.clear_display();
brass_phoenix 5:2632dfc8454c 149 screen.display_state_name("EMG 2 calibration");
brass_phoenix 2:141cfcafe72b 150 }
brass_phoenix 9:27d00b64076e 151
brass_phoenix 9:27d00b64076e 152 if (ud_button.has_just_been_pressed()) {
brass_phoenix 9:27d00b64076e 153 current_state = homing;
brass_phoenix 9:27d00b64076e 154 }
brass_phoenix 2:141cfcafe72b 155 }
brass_phoenix 2:141cfcafe72b 156
brass_phoenix 3:4b19b6cf6cc7 157 void do_state_homing()
brass_phoenix 3:4b19b6cf6cc7 158 {
brass_phoenix 2:141cfcafe72b 159 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 160 last_state = current_state;
brass_phoenix 2:141cfcafe72b 161 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 162 screen.clear_display();
brass_phoenix 5:2632dfc8454c 163 screen.display_state_name("Homing");
brass_phoenix 2:141cfcafe72b 164 }
brass_phoenix 9:27d00b64076e 165
brass_phoenix 19:53b9729fbab5 166 main_motor.set_target_angle(PI*0.5);
brass_phoenix 19:53b9729fbab5 167 sec_motor.set_target_angle(0);
brass_phoenix 19:53b9729fbab5 168
brass_phoenix 9:27d00b64076e 169 if (ud_button.has_just_been_pressed()) {
brass_phoenix 19:53b9729fbab5 170 current_state = calib_bicep1;
brass_phoenix 9:27d00b64076e 171 }
brass_phoenix 2:141cfcafe72b 172 }
brass_phoenix 2:141cfcafe72b 173
brass_phoenix 3:4b19b6cf6cc7 174 void do_state_operation()
brass_phoenix 3:4b19b6cf6cc7 175 {
brass_phoenix 2:141cfcafe72b 176 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 177 last_state = current_state;
brass_phoenix 2:141cfcafe72b 178 // State just changed to this one.
brass_phoenix 6:bfc6e68774f5 179 screen.clear_display();
brass_phoenix 5:2632dfc8454c 180 screen.display_state_name("Normal operation");
brass_phoenix 15:f65b4566193e 181
brass_phoenix 15:f65b4566193e 182 control_goes_up = true;
brass_phoenix 15:f65b4566193e 183 control_goes_right = true;
brass_phoenix 15:f65b4566193e 184
brass_phoenix 15:f65b4566193e 185 screen.display_up_down_arrow(control_goes_up);
brass_phoenix 15:f65b4566193e 186 screen.display_left_right_arrow(control_goes_right);
brass_phoenix 2:141cfcafe72b 187 }
brass_phoenix 9:27d00b64076e 188
brass_phoenix 19:53b9729fbab5 189 /*
brass_phoenix 14:b97e7a41ec23 190 double main_target = ((potmeter1.read() * 2) - 1) * PI;
brass_phoenix 13:88967c004446 191 main_motor.set_target_angle(main_target);
brass_phoenix 14:b97e7a41ec23 192 double sec_target = ((potmeter2.read() * 2) - 1) * PI;
brass_phoenix 13:88967c004446 193 sec_motor.set_target_angle(sec_target);
brass_phoenix 13:88967c004446 194
brass_phoenix 9:27d00b64076e 195 if (ud_button.has_just_been_pressed()) {
brass_phoenix 12:0c10396d0615 196 control_goes_up = !control_goes_up;
brass_phoenix 15:f65b4566193e 197 screen.display_up_down_arrow(control_goes_up);
brass_phoenix 12:0c10396d0615 198 }
brass_phoenix 12:0c10396d0615 199 if (lr_button.has_just_been_pressed()) {
brass_phoenix 12:0c10396d0615 200 control_goes_right = !control_goes_right;
brass_phoenix 15:f65b4566193e 201 screen.display_left_right_arrow(control_goes_right);
brass_phoenix 9:27d00b64076e 202 }
brass_phoenix 19:53b9729fbab5 203 */
brass_phoenix 2:141cfcafe72b 204 }
brass_phoenix 2:141cfcafe72b 205
brass_phoenix 3:4b19b6cf6cc7 206 void do_state_failure()
brass_phoenix 3:4b19b6cf6cc7 207 {
brass_phoenix 2:141cfcafe72b 208 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 209 last_state = current_state;
brass_phoenix 2:141cfcafe72b 210 // State just changed.
brass_phoenix 2:141cfcafe72b 211 // Update the display.
brass_phoenix 2:141cfcafe72b 212 led_red = 0;
brass_phoenix 2:141cfcafe72b 213 led_green = 1;
brass_phoenix 6:bfc6e68774f5 214 screen.clear_display();
brass_phoenix 11:d980e0e581db 215 screen.display_state_name("STOP");
brass_phoenix 2:141cfcafe72b 216 }
brass_phoenix 3:4b19b6cf6cc7 217
brass_phoenix 2:141cfcafe72b 218 // Stop the motors!
brass_phoenix 12:0c10396d0615 219 main_motor.stop();
brass_phoenix 12:0c10396d0615 220 sec_motor.stop();
brass_phoenix 2:141cfcafe72b 221 }
brass_phoenix 2:141cfcafe72b 222
brass_phoenix 2:141cfcafe72b 223
brass_phoenix 1:cfa5abca6d43 224 void main_loop()
brass_phoenix 1:cfa5abca6d43 225 {
brass_phoenix 1:cfa5abca6d43 226 ud_button.update();
brass_phoenix 1:cfa5abca6d43 227 lr_button.update();
brass_phoenix 1:cfa5abca6d43 228 p_button.update();
brass_phoenix 3:4b19b6cf6cc7 229
brass_phoenix 1:cfa5abca6d43 230 switch (current_state) {
brass_phoenix 1:cfa5abca6d43 231 case waiting:
brass_phoenix 2:141cfcafe72b 232 do_state_waiting();
brass_phoenix 1:cfa5abca6d43 233 break;
brass_phoenix 1:cfa5abca6d43 234 case calib_motor:
brass_phoenix 2:141cfcafe72b 235 do_state_calib_motor();
brass_phoenix 1:cfa5abca6d43 236 break;
brass_phoenix 1:cfa5abca6d43 237 case calib_bicep1:
brass_phoenix 2:141cfcafe72b 238 do_state_calib_bicep1();
brass_phoenix 1:cfa5abca6d43 239 break;
brass_phoenix 1:cfa5abca6d43 240 case calib_bicep2:
brass_phoenix 2:141cfcafe72b 241 do_state_calib_bicep2();
brass_phoenix 1:cfa5abca6d43 242 break;
brass_phoenix 1:cfa5abca6d43 243 case homing:
brass_phoenix 2:141cfcafe72b 244 do_state_homing();
brass_phoenix 1:cfa5abca6d43 245 break;
brass_phoenix 1:cfa5abca6d43 246 case operation:
brass_phoenix 2:141cfcafe72b 247 do_state_operation();
brass_phoenix 1:cfa5abca6d43 248 break;
brass_phoenix 1:cfa5abca6d43 249 case failure:
brass_phoenix 2:141cfcafe72b 250 do_state_failure();
brass_phoenix 1:cfa5abca6d43 251 break;
brass_phoenix 1:cfa5abca6d43 252 }
brass_phoenix 3:4b19b6cf6cc7 253
brass_phoenix 2:141cfcafe72b 254 // Check if the panic button was pressed.
brass_phoenix 2:141cfcafe72b 255 // Doesn't matter in which state we are, we need to go to failure.
brass_phoenix 2:141cfcafe72b 256 if (p_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 257 current_state = failure;
brass_phoenix 3:4b19b6cf6cc7 258 }
brass_phoenix 1:cfa5abca6d43 259 }
MAHCSnijders 0:7d25c2ade6c5 260
brass_phoenix 7:e7f808875bc4 261 void poll_buttons() {
brass_phoenix 7:e7f808875bc4 262 // We need to poll the pins periodically.
brass_phoenix 7:e7f808875bc4 263 // Normally one would use rise and fall interrupts, so this wouldn't be
brass_phoenix 7:e7f808875bc4 264 // needed. But the buttons we use generate so much chatter that
brass_phoenix 7:e7f808875bc4 265 // sometimes a rising or a falling edge doesn't get registered.
brass_phoenix 7:e7f808875bc4 266 // With all the confusion that accompanies it.
brass_phoenix 7:e7f808875bc4 267 ud_button.poll_pin();
brass_phoenix 7:e7f808875bc4 268 lr_button.poll_pin();
brass_phoenix 7:e7f808875bc4 269 p_button.poll_pin();
brass_phoenix 7:e7f808875bc4 270 }
brass_phoenix 7:e7f808875bc4 271
brass_phoenix 1:cfa5abca6d43 272 int main()
brass_phoenix 1:cfa5abca6d43 273 {
brass_phoenix 1:cfa5abca6d43 274 led_red = 1;
brass_phoenix 9:27d00b64076e 275 led_green = 1;
brass_phoenix 9:27d00b64076e 276 led_blue = 1;
brass_phoenix 8:9090ab7c19a8 277
brass_phoenix 10:b165ccd11afd 278 screen.clear_display();
brass_phoenix 10:b165ccd11afd 279
brass_phoenix 12:0c10396d0615 280 main_motor.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 12:0c10396d0615 281 sec_motor.set_pid_k_values(Kp, Ki, Kd);
brass_phoenix 12:0c10396d0615 282
brass_phoenix 12:0c10396d0615 283 // One of the motors is reversed in the electronics.
brass_phoenix 12:0c10396d0615 284 // This is fixed in the motor controll board, so we have to account
brass_phoenix 12:0c10396d0615 285 // for it in software.
brass_phoenix 19:53b9729fbab5 286 main_motor.set_extra_reduction_ratio(-main_gear_ratio);
brass_phoenix 19:53b9729fbab5 287 sec_motor.set_extra_reduction_ratio(sec_gear_ratio);
brass_phoenix 19:53b9729fbab5 288
brass_phoenix 19:53b9729fbab5 289 // Set the maximum pwm fraction for both motors.
brass_phoenix 19:53b9729fbab5 290 main_motor.set_max_pwm_fraction(0.5);
brass_phoenix 19:53b9729fbab5 291 sec_motor.set_max_pwm_fraction(0.5);
brass_phoenix 12:0c10396d0615 292
brass_phoenix 8:9090ab7c19a8 293 // Start the motor controller at the desired frequency.
brass_phoenix 12:0c10396d0615 294 main_motor.start(pid_period);
brass_phoenix 12:0c10396d0615 295 sec_motor.start(pid_period);
brass_phoenix 3:4b19b6cf6cc7 296
brass_phoenix 2:141cfcafe72b 297 // Start in the waiting state.
brass_phoenix 1:cfa5abca6d43 298 current_state = waiting;
brass_phoenix 4:5a44ab7e94b3 299 // Pretend we come from the operation state.
brass_phoenix 4:5a44ab7e94b3 300 // So that the waiting state knows it just got started.
brass_phoenix 4:5a44ab7e94b3 301 last_state = operation;
brass_phoenix 7:e7f808875bc4 302
brass_phoenix 7:e7f808875bc4 303 // Start the button polling ticker.
brass_phoenix 7:e7f808875bc4 304 button_ticker.attach(&poll_buttons, button_poll_interval);
brass_phoenix 3:4b19b6cf6cc7 305
brass_phoenix 1:cfa5abca6d43 306 while (true) {
brass_phoenix 1:cfa5abca6d43 307 main_loop();
brass_phoenix 9:27d00b64076e 308
brass_phoenix 9:27d00b64076e 309 // Button debugging.
brass_phoenix 9:27d00b64076e 310 if (ud_button.has_just_been_pressed() || lr_button.has_just_been_pressed() || p_button.has_just_been_pressed()) {
brass_phoenix 9:27d00b64076e 311 led_blue = 0;
brass_phoenix 9:27d00b64076e 312 } else {
brass_phoenix 9:27d00b64076e 313 led_blue = 1;
brass_phoenix 9:27d00b64076e 314 }
brass_phoenix 9:27d00b64076e 315
brass_phoenix 1:cfa5abca6d43 316 wait(main_loop_wait_time);
brass_phoenix 1:cfa5abca6d43 317 }
brass_phoenix 1:cfa5abca6d43 318 }