Biorobotics 7 / Mbed 2 deprecated State_Machine

State machine

Dependencies:   mbed Adafruit_GFX BioroboticsMotorControl MODSERIAL BioroboticsEMGFilter

main.cpp@3:4b19b6cf6cc7, 2018-10-30 (annotated)

Committer:
brass_phoenix
Date:
Tue Oct 30 11:40:00 2018 +0000
Revision:
3:4b19b6cf6cc7
Parent:
2:141cfcafe72b
Child:
4:5a44ab7e94b3
+ Added screen basics.

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 2:141cfcafe72b 36 if (ud_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 37 current_state = calib_motor;
brass_phoenix 2:141cfcafe72b 38 }
brass_phoenix 3:4b19b6cf6cc7 39
brass_phoenix 2:141cfcafe72b 40 led_green = 1;
brass_phoenix 2:141cfcafe72b 41 }
brass_phoenix 2:141cfcafe72b 42
brass_phoenix 3:4b19b6cf6cc7 43 void do_state_calib_motor()
brass_phoenix 3:4b19b6cf6cc7 44 {
brass_phoenix 2:141cfcafe72b 45 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 46 last_state = current_state;
brass_phoenix 2:141cfcafe72b 47 // State just changed to this one.
brass_phoenix 3:4b19b6cf6cc7 48
brass_phoenix 2:141cfcafe72b 49 led_green = 0;
brass_phoenix 2:141cfcafe72b 50 }
brass_phoenix 2:141cfcafe72b 51 }
brass_phoenix 2:141cfcafe72b 52
brass_phoenix 3:4b19b6cf6cc7 53 void do_state_calib_bicep1()
brass_phoenix 3:4b19b6cf6cc7 54 {
brass_phoenix 2:141cfcafe72b 55 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 56 last_state = current_state;
brass_phoenix 2:141cfcafe72b 57 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 58 }
brass_phoenix 2:141cfcafe72b 59 }
brass_phoenix 2:141cfcafe72b 60
brass_phoenix 3:4b19b6cf6cc7 61 void do_state_calib_bicep2()
brass_phoenix 3:4b19b6cf6cc7 62 {
brass_phoenix 2:141cfcafe72b 63 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 64 last_state = current_state;
brass_phoenix 2:141cfcafe72b 65 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 66 }
brass_phoenix 2:141cfcafe72b 67 }
brass_phoenix 2:141cfcafe72b 68
brass_phoenix 3:4b19b6cf6cc7 69 void do_state_homing()
brass_phoenix 3:4b19b6cf6cc7 70 {
brass_phoenix 2:141cfcafe72b 71 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 72 last_state = current_state;
brass_phoenix 2:141cfcafe72b 73 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 74 }
brass_phoenix 2:141cfcafe72b 75 }
brass_phoenix 2:141cfcafe72b 76
brass_phoenix 3:4b19b6cf6cc7 77 void do_state_operation()
brass_phoenix 3:4b19b6cf6cc7 78 {
brass_phoenix 2:141cfcafe72b 79 if(last_state != current_state) {
brass_phoenix 2:141cfcafe72b 80 last_state = current_state;
brass_phoenix 2:141cfcafe72b 81 // State just changed to this one.
brass_phoenix 2:141cfcafe72b 82 }
brass_phoenix 2:141cfcafe72b 83 }
brass_phoenix 2:141cfcafe72b 84
brass_phoenix 3:4b19b6cf6cc7 85 void do_state_failure()
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.
brass_phoenix 2:141cfcafe72b 90 // Update the display.
brass_phoenix 2:141cfcafe72b 91 led_red = 0;
brass_phoenix 2:141cfcafe72b 92 led_green = 1;
brass_phoenix 2:141cfcafe72b 93 }
brass_phoenix 3:4b19b6cf6cc7 94
brass_phoenix 2:141cfcafe72b 95 // Stop the motors!
brass_phoenix 2:141cfcafe72b 96 }
brass_phoenix 2:141cfcafe72b 97
brass_phoenix 2:141cfcafe72b 98
brass_phoenix 1:cfa5abca6d43 99 void main_loop()
brass_phoenix 1:cfa5abca6d43 100 {
brass_phoenix 1:cfa5abca6d43 101 ud_button.update();
brass_phoenix 1:cfa5abca6d43 102 lr_button.update();
brass_phoenix 1:cfa5abca6d43 103 p_button.update();
brass_phoenix 3:4b19b6cf6cc7 104
brass_phoenix 1:cfa5abca6d43 105 switch (current_state) {
brass_phoenix 1:cfa5abca6d43 106 case waiting:
brass_phoenix 2:141cfcafe72b 107 do_state_waiting();
brass_phoenix 1:cfa5abca6d43 108 break;
brass_phoenix 1:cfa5abca6d43 109 case calib_motor:
brass_phoenix 2:141cfcafe72b 110 do_state_calib_motor();
brass_phoenix 1:cfa5abca6d43 111 break;
brass_phoenix 1:cfa5abca6d43 112 case calib_bicep1:
brass_phoenix 2:141cfcafe72b 113 do_state_calib_bicep1();
brass_phoenix 1:cfa5abca6d43 114 break;
brass_phoenix 1:cfa5abca6d43 115 case calib_bicep2:
brass_phoenix 2:141cfcafe72b 116 do_state_calib_bicep2();
brass_phoenix 1:cfa5abca6d43 117 break;
brass_phoenix 1:cfa5abca6d43 118 case homing:
brass_phoenix 2:141cfcafe72b 119 do_state_homing();
brass_phoenix 1:cfa5abca6d43 120 break;
brass_phoenix 1:cfa5abca6d43 121 case operation:
brass_phoenix 2:141cfcafe72b 122 do_state_operation();
brass_phoenix 1:cfa5abca6d43 123 break;
brass_phoenix 1:cfa5abca6d43 124 case failure:
brass_phoenix 2:141cfcafe72b 125 do_state_failure();
brass_phoenix 1:cfa5abca6d43 126 break;
brass_phoenix 1:cfa5abca6d43 127 }
brass_phoenix 3:4b19b6cf6cc7 128
brass_phoenix 2:141cfcafe72b 129 // Check if the panic button was pressed.
brass_phoenix 2:141cfcafe72b 130 // Doesn't matter in which state we are, we need to go to failure.
brass_phoenix 2:141cfcafe72b 131 if (p_button.is_pressed()) {
brass_phoenix 2:141cfcafe72b 132 current_state = failure;
brass_phoenix 3:4b19b6cf6cc7 133 }
brass_phoenix 1:cfa5abca6d43 134 }
MAHCSnijders 0:7d25c2ade6c5 135
brass_phoenix 1:cfa5abca6d43 136 int main()
brass_phoenix 1:cfa5abca6d43 137 {
brass_phoenix 1:cfa5abca6d43 138 led_red = 1;
brass_phoenix 3:4b19b6cf6cc7 139
brass_phoenix 2:141cfcafe72b 140 // Start in the waiting state.
brass_phoenix 1:cfa5abca6d43 141 current_state = waiting;
brass_phoenix 2:141cfcafe72b 142 last_state = waiting;
brass_phoenix 2:141cfcafe72b 143
brass_phoenix 3:4b19b6cf6cc7 144 screen.get_screen_handle().printf("OLED Display\r\n");
brass_phoenix 3:4b19b6cf6cc7 145 screen.display();
brass_phoenix 3:4b19b6cf6cc7 146
brass_phoenix 1:cfa5abca6d43 147 while (true) {
brass_phoenix 1:cfa5abca6d43 148 main_loop();
brass_phoenix 1:cfa5abca6d43 149 wait(main_loop_wait_time);
brass_phoenix 1:cfa5abca6d43 150 }
brass_phoenix 1:cfa5abca6d43 151 }