Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@15:f65b4566193e, 2018-10-31 (annotated)

Committer:
brass_phoenix
Date:
Wed Oct 31 11:05:47 2018 +0000
Revision:
15:f65b4566193e
Parent:
14:b97e7a41ec23
Child:
16:9c5ef6fe6780
+ Should be displaying arrows in normal operation.

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