State machine
Dependencies: mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter
main.cpp@19:53b9729fbab5, 2018-10-31 (annotated)
- 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?
User | Revision | Line number | New 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 | } |