Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@4:5a44ab7e94b3, 2018-10-30 (annotated)

Committer:
brass_phoenix
Date:
Tue Oct 30 11:52:50 2018 +0000
Revision:
4:5a44ab7e94b3
Parent:
3:4b19b6cf6cc7
Child:
5:2632dfc8454c
+ States can now print their name to the screen.

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"
MAHCSnijders 0:7d25c2ade6c5 5
brass_phoenix 1:cfa5abca6d43 6
brass_phoenix 1:cfa5abca6d43 7 enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, failure}; // The possible states of the state machine
MAHCSnijders 0:7d25c2ade6c5 8
MAHCSnijders 0:7d25c2ade6c5 9 // Global variables
MAHCSnijders 0:7d25c2ade6c5 10 const double PI = 3.14159265359;
brass_phoenix 1:cfa5abca6d43 11 // Main loop wait time per cycle. This does not influence the motor PID or EMG reading frequencies.
brass_phoenix 1:cfa5abca6d43 12 const double main_loop_wait_time = 0.01;
brass_phoenix 1:cfa5abca6d43 13
brass_phoenix 1:cfa5abca6d43 14
brass_phoenix 1:cfa5abca6d43 15 States current_state; // Defining the state we are currently in
brass_phoenix 2:141cfcafe72b 16 States last_state; // To detect state changes.
MAHCSnijders 0:7d25c2ade6c5 17 Ticker loop_ticker; // Ticker for the loop function
brass_phoenix 1:cfa5abca6d43 18
brass_phoenix 1:cfa5abca6d43 19 // Order of buttons: up_down, left_right, panic
brass_phoenix 1:cfa5abca6d43 20 // D2, D3, D8
brass_phoenix 1:cfa5abca6d43 21 Button ud_button(PTA4);
brass_phoenix 2:141cfcafe72b 22 Button lr_button(D3);
brass_phoenix 2:141cfcafe72b 23 Button p_button(PTC6);
MAHCSnijders 0:7d25c2ade6c5 24
brass_phoenix 1:cfa5abca6d43 25 DigitalOut led_red(LED_RED);
brass_phoenix 1:cfa5abca6d43 26 DigitalOut led_green(LED_GREEN);
brass_phoenix 1:cfa5abca6d43 27
brass_phoenix 3:4b19b6cf6cc7 28 // The last arguent is the reset pin.
brass_phoenix 3:4b19b6cf6cc7 29 // The screen doesn't use it, but the library requires it
brass_phoenix 3:4b19b6cf6cc7 30 // So pick a pin we don't use.
brass_phoenix 3:4b19b6cf6cc7 31 Screen screen(D14, D15, D9);
brass_phoenix 2:141cfcafe72b 32
brass_phoenix 3:4b19b6cf6cc7 33
brass_phoenix 3:4b19b6cf6cc7 34 void do_state_waiting()
brass_phoenix 3:4b19b6cf6cc7 35 {
brass_phoenix 4:5a44ab7e94b3 36 if(last_state != current_state) {
brass_phoenix 4:5a44ab7e94b3 37 last_state = current_state;
brass_phoenix 4:5a44ab7e94b3 38 // State just changed to this one.
brass_phoenix 4:5a44ab7e94b3 39
brass_phoenix 4:5a44ab7e94b3 40 led_green = 1;
brass_phoenix 4:5a44ab7e94b3 41 screen.display_state_name("Waiting");
brass_phoenix 4:5a44ab7e94b3 42 }
brass_phoenix 4:5a44ab7e94b3 43
brass_phoenix 2:141cfcafe72b 44 if (ud_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 45 current_state = calib_motor;
brass_phoenix 2:141cfcafe72b 46 }
brass_phoenix 2:141cfcafe72b 47 }
brass_phoenix 2:141cfcafe72b 48
brass_phoenix 3:4b19b6cf6cc7 49 void do_state_calib_motor()
brass_phoenix 3:4b19b6cf6cc7 50 {
brass_phoenix 2:141cfcafe72b 51 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 52 last_state = current_state;
brass_phoenix 2:141cfcafe72b 53 // State just changed to this one.
brass_phoenix 3:4b19b6cf6cc7 54
brass_phoenix 2:141cfcafe72b 55 led_green = 0;
brass_phoenix 4:5a44ab7e94b3 56 screen.display_state_name("Motor calib");
brass_phoenix 2:141cfcafe72b 57 }
brass_phoenix 2:141cfcafe72b 58 }
brass_phoenix 2:141cfcafe72b 59
brass_phoenix 3:4b19b6cf6cc7 60 void do_state_calib_bicep1()
brass_phoenix 3:4b19b6cf6cc7 61 {
brass_phoenix 2:141cfcafe72b 62 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 63 last_state = current_state;
brass_phoenix 2:141cfcafe72b 64 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 65 }
brass_phoenix 2:141cfcafe72b 66 }
brass_phoenix 2:141cfcafe72b 67
brass_phoenix 3:4b19b6cf6cc7 68 void do_state_calib_bicep2()
brass_phoenix 3:4b19b6cf6cc7 69 {
brass_phoenix 2:141cfcafe72b 70 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 71 last_state = current_state;
brass_phoenix 2:141cfcafe72b 72 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 73 }
brass_phoenix 2:141cfcafe72b 74 }
brass_phoenix 2:141cfcafe72b 75
brass_phoenix 3:4b19b6cf6cc7 76 void do_state_homing()
brass_phoenix 3:4b19b6cf6cc7 77 {
brass_phoenix 2:141cfcafe72b 78 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 79 last_state = current_state;
brass_phoenix 2:141cfcafe72b 80 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 81 }
brass_phoenix 2:141cfcafe72b 82 }
brass_phoenix 2:141cfcafe72b 83
brass_phoenix 3:4b19b6cf6cc7 84 void do_state_operation()
brass_phoenix 3:4b19b6cf6cc7 85 {
brass_phoenix 2:141cfcafe72b 86 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 87 last_state = current_state;
brass_phoenix 2:141cfcafe72b 88 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 89 }
brass_phoenix 2:141cfcafe72b 90 }
brass_phoenix 2:141cfcafe72b 91
brass_phoenix 3:4b19b6cf6cc7 92 void do_state_failure()
brass_phoenix 3:4b19b6cf6cc7 93 {
brass_phoenix 2:141cfcafe72b 94 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 95 last_state = current_state;
brass_phoenix 2:141cfcafe72b 96 // State just changed.
brass_phoenix 2:141cfcafe72b 97 // Update the display.
brass_phoenix 2:141cfcafe72b 98 led_red = 0;
brass_phoenix 2:141cfcafe72b 99 led_green = 1;
brass_phoenix 4:5a44ab7e94b3 100 screen.display_state_name("Error!");
brass_phoenix 2:141cfcafe72b 101 }
brass_phoenix 3:4b19b6cf6cc7 102
brass_phoenix 2:141cfcafe72b 103 // Stop the motors!
brass_phoenix 2:141cfcafe72b 104 }
brass_phoenix 2:141cfcafe72b 105
brass_phoenix 2:141cfcafe72b 106
brass_phoenix 1:cfa5abca6d43 107 void main_loop()
brass_phoenix 1:cfa5abca6d43 108 {
brass_phoenix 1:cfa5abca6d43 109 ud_button.update();
brass_phoenix 1:cfa5abca6d43 110 lr_button.update();
brass_phoenix 1:cfa5abca6d43 111 p_button.update();
brass_phoenix 3:4b19b6cf6cc7 112
brass_phoenix 1:cfa5abca6d43 113 switch (current_state) {
brass_phoenix 1:cfa5abca6d43 114 case waiting:
brass_phoenix 2:141cfcafe72b 115 do_state_waiting();
brass_phoenix 1:cfa5abca6d43 116 break;
brass_phoenix 1:cfa5abca6d43 117 case calib_motor:
brass_phoenix 2:141cfcafe72b 118 do_state_calib_motor();
brass_phoenix 1:cfa5abca6d43 119 break;
brass_phoenix 1:cfa5abca6d43 120 case calib_bicep1:
brass_phoenix 2:141cfcafe72b 121 do_state_calib_bicep1();
brass_phoenix 1:cfa5abca6d43 122 break;
brass_phoenix 1:cfa5abca6d43 123 case calib_bicep2:
brass_phoenix 2:141cfcafe72b 124 do_state_calib_bicep2();
brass_phoenix 1:cfa5abca6d43 125 break;
brass_phoenix 1:cfa5abca6d43 126 case homing:
brass_phoenix 2:141cfcafe72b 127 do_state_homing();
brass_phoenix 1:cfa5abca6d43 128 break;
brass_phoenix 1:cfa5abca6d43 129 case operation:
brass_phoenix 2:141cfcafe72b 130 do_state_operation();
brass_phoenix 1:cfa5abca6d43 131 break;
brass_phoenix 1:cfa5abca6d43 132 case failure:
brass_phoenix 2:141cfcafe72b 133 do_state_failure();
brass_phoenix 1:cfa5abca6d43 134 break;
brass_phoenix 1:cfa5abca6d43 135 }
brass_phoenix 3:4b19b6cf6cc7 136
brass_phoenix 2:141cfcafe72b 137 // Check if the panic button was pressed.
brass_phoenix 2:141cfcafe72b 138 // Doesn't matter in which state we are, we need to go to failure.
brass_phoenix 2:141cfcafe72b 139 if (p_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 140 current_state = failure;
brass_phoenix 3:4b19b6cf6cc7 141 }
brass_phoenix 1:cfa5abca6d43 142 }
MAHCSnijders 0:7d25c2ade6c5 143
brass_phoenix 1:cfa5abca6d43 144 int main()
brass_phoenix 1:cfa5abca6d43 145 {
brass_phoenix 1:cfa5abca6d43 146 led_red = 1;
brass_phoenix 3:4b19b6cf6cc7 147
brass_phoenix 2:141cfcafe72b 148 // Start in the waiting state.
brass_phoenix 1:cfa5abca6d43 149 current_state = waiting;
brass_phoenix 4:5a44ab7e94b3 150 // Pretend we come from the operation state.
brass_phoenix 4:5a44ab7e94b3 151 // So that the waiting state knows it just got started.
brass_phoenix 4:5a44ab7e94b3 152 last_state = operation;
brass_phoenix 3:4b19b6cf6cc7 153
brass_phoenix 1:cfa5abca6d43 154 while (true) {
brass_phoenix 1:cfa5abca6d43 155 main_loop();
brass_phoenix 1:cfa5abca6d43 156 wait(main_loop_wait_time);
brass_phoenix 1:cfa5abca6d43 157 }
brass_phoenix 1:cfa5abca6d43 158 }