Fabrice Tshilumba / Mbed 2 deprecated state_program

Dependencies:   FastPWM MODSERIAL QEI biquadFilter mbed

Fork of state_program by Jesse Lohman

main.cpp@0:2a5dd6cc0008, 2018-10-23 (annotated)

Committer:
JesseLohman
Date:
Tue Oct 23 13:04:18 2018 +0000
Revision:
0:2a5dd6cc0008
Child:
1:4cb9af313c26
Created state system architecture (WIP)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JesseLohman 0:2a5dd6cc0008 1 #include "mbed.h"
JesseLohman 0:2a5dd6cc0008 2 #include "FastPWM.h"
JesseLohman 0:2a5dd6cc0008 3 #include "QEI.h"
JesseLohman 0:2a5dd6cc0008 4 #include "MODSERIAL.h"
JesseLohman 0:2a5dd6cc0008 5 #include "BiQuad.h"
JesseLohman 0:2a5dd6cc0008 6
JesseLohman 0:2a5dd6cc0008 7 enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState};
JesseLohman 0:2a5dd6cc0008 8 States currentState = WaitState;
JesseLohman 0:2a5dd6cc0008 9
JesseLohman 0:2a5dd6cc0008 10 DigitalIn startButton(D0);
JesseLohman 0:2a5dd6cc0008 11 InterruptIn failureButton(D1);
JesseLohman 0:2a5dd6cc0008 12 DigitalIn grippingSwitch(D2);
JesseLohman 0:2a5dd6cc0008 13 DigitalIn screwingSwitch(D3);
JesseLohman 0:2a5dd6cc0008 14 MODSERIAL pc(USBTX, USBRX);
JesseLohman 0:2a5dd6cc0008 15 DigitalOut led1(LED1);
JesseLohman 0:2a5dd6cc0008 16 DigitalOut led2(LED2);
JesseLohman 0:2a5dd6cc0008 17 DigitalOut led3(LED3);
JesseLohman 0:2a5dd6cc0008 18
JesseLohman 0:2a5dd6cc0008 19 bool stateChanged = true;
JesseLohman 0:2a5dd6cc0008 20
JesseLohman 0:2a5dd6cc0008 21 Ticker mainTicker;
JesseLohman 0:2a5dd6cc0008 22 Timer stateTimer;
JesseLohman 0:2a5dd6cc0008 23 double sampleTime = 0.01;
JesseLohman 0:2a5dd6cc0008 24
JesseLohman 0:2a5dd6cc0008 25 void switchToFailureState ()
JesseLohman 0:2a5dd6cc0008 26 {
JesseLohman 0:2a5dd6cc0008 27 failureButton.fall(NULL); // Detaches button, so it can only be activated once
JesseLohman 0:2a5dd6cc0008 28 pc.printf("SYSTEM FAILED");
JesseLohman 0:2a5dd6cc0008 29 currentState = FailureState;
JesseLohman 0:2a5dd6cc0008 30 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 31 }
JesseLohman 0:2a5dd6cc0008 32
JesseLohman 0:2a5dd6cc0008 33 void stateMachine ()
JesseLohman 0:2a5dd6cc0008 34 {
JesseLohman 0:2a5dd6cc0008 35 switch (currentState) {
JesseLohman 0:2a5dd6cc0008 36 case WaitState:
JesseLohman 0:2a5dd6cc0008 37 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 38 // Entry action: all the things you do once in this state
JesseLohman 0:2a5dd6cc0008 39 // Set output motor to 0
JesseLohman 0:2a5dd6cc0008 40 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 41 }
JesseLohman 0:2a5dd6cc0008 42
JesseLohman 0:2a5dd6cc0008 43 if (startButton.read() == true) {
JesseLohman 0:2a5dd6cc0008 44 pc.printf("Switching to motor calibration");
JesseLohman 0:2a5dd6cc0008 45 led1 = 0;
JesseLohman 0:2a5dd6cc0008 46 currentState = MotorCalState;
JesseLohman 0:2a5dd6cc0008 47 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 48 }
JesseLohman 0:2a5dd6cc0008 49
JesseLohman 0:2a5dd6cc0008 50 break;
JesseLohman 0:2a5dd6cc0008 51 case MotorCalState:
JesseLohman 0:2a5dd6cc0008 52 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 53 // Entry action: all the things you do once in this state
JesseLohman 0:2a5dd6cc0008 54 led2 = 1;
JesseLohman 0:2a5dd6cc0008 55 // Set motorpwm to 'low' value
JesseLohman 0:2a5dd6cc0008 56 stateTimer.reset();
JesseLohman 0:2a5dd6cc0008 57 stateTimer.start();
JesseLohman 0:2a5dd6cc0008 58 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 59 }
JesseLohman 0:2a5dd6cc0008 60
JesseLohman 0:2a5dd6cc0008 61 // Add stuff you do every loop
JesseLohman 0:2a5dd6cc0008 62
JesseLohman 0:2a5dd6cc0008 63 if (stateTimer >= 3.0f) { //TODO: add && motor velocity < 0.1f
JesseLohman 0:2a5dd6cc0008 64 pc.printf("Starting EMG calibration...\n");
JesseLohman 0:2a5dd6cc0008 65 led2 = 0;
JesseLohman 0:2a5dd6cc0008 66 currentState = EMGCalState;
JesseLohman 0:2a5dd6cc0008 67 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 68 }
JesseLohman 0:2a5dd6cc0008 69 break;
JesseLohman 0:2a5dd6cc0008 70 case EMGCalState:
JesseLohman 0:2a5dd6cc0008 71 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 72 // Entry action: all the things you do once in this state;
JesseLohman 0:2a5dd6cc0008 73 led3 = 1;
JesseLohman 0:2a5dd6cc0008 74 stateTimer.reset();
JesseLohman 0:2a5dd6cc0008 75 stateTimer.start();
JesseLohman 0:2a5dd6cc0008 76 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 77 }
JesseLohman 0:2a5dd6cc0008 78
JesseLohman 0:2a5dd6cc0008 79 // Add stuff you do every loop
JesseLohman 0:2a5dd6cc0008 80
JesseLohman 0:2a5dd6cc0008 81 if (stateTimer >= 3.0f) {
JesseLohman 0:2a5dd6cc0008 82 pc.printf("Starting homing...\n");
JesseLohman 0:2a5dd6cc0008 83 led3 = 0;
JesseLohman 0:2a5dd6cc0008 84 currentState = HomingState;
JesseLohman 0:2a5dd6cc0008 85 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 86 }
JesseLohman 0:2a5dd6cc0008 87 break;
JesseLohman 0:2a5dd6cc0008 88 case HomingState:
JesseLohman 0:2a5dd6cc0008 89 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 90 // Entry action: all the things you do once in this state;
JesseLohman 0:2a5dd6cc0008 91 led1 = 1;
JesseLohman 0:2a5dd6cc0008 92 led2 = 1;
JesseLohman 0:2a5dd6cc0008 93 // Set intended position
JesseLohman 0:2a5dd6cc0008 94 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 95 }
JesseLohman 0:2a5dd6cc0008 96
JesseLohman 0:2a5dd6cc0008 97 // Nothing extra happens till robot reaches starting position and button is pressed
JesseLohman 0:2a5dd6cc0008 98
JesseLohman 0:2a5dd6cc0008 99 if (startButton.read() == true) { // Also add position condition
JesseLohman 0:2a5dd6cc0008 100 pc.printf("Start moving...\n");
JesseLohman 0:2a5dd6cc0008 101 led1 = 0;
JesseLohman 0:2a5dd6cc0008 102 led2 = 0;
JesseLohman 0:2a5dd6cc0008 103 currentState = MovingState;
JesseLohman 0:2a5dd6cc0008 104 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 105 }
JesseLohman 0:2a5dd6cc0008 106 break;
JesseLohman 0:2a5dd6cc0008 107 case MovingState:
JesseLohman 0:2a5dd6cc0008 108 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 109 // Entry action: all the things you do once in this state;
JesseLohman 0:2a5dd6cc0008 110 led1 = 1;
JesseLohman 0:2a5dd6cc0008 111 led2 = 1;
JesseLohman 0:2a5dd6cc0008 112 led3 = 1;
JesseLohman 0:2a5dd6cc0008 113 // Set intended position
JesseLohman 0:2a5dd6cc0008 114 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 115 }
JesseLohman 0:2a5dd6cc0008 116 break;
JesseLohman 0:2a5dd6cc0008 117 case GrippingState:
JesseLohman 0:2a5dd6cc0008 118 break;
JesseLohman 0:2a5dd6cc0008 119 case ScrewingState:
JesseLohman 0:2a5dd6cc0008 120 break;
JesseLohman 0:2a5dd6cc0008 121 case FailureState:
JesseLohman 0:2a5dd6cc0008 122 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 123 // Entry action: all the things you do once in this state
JesseLohman 0:2a5dd6cc0008 124 // Turn all motors off
JesseLohman 0:2a5dd6cc0008 125 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 126 break;
JesseLohman 0:2a5dd6cc0008 127 }
JesseLohman 0:2a5dd6cc0008 128 }
JesseLohman 0:2a5dd6cc0008 129 }
JesseLohman 0:2a5dd6cc0008 130
JesseLohman 0:2a5dd6cc0008 131 void mainLoop ()
JesseLohman 0:2a5dd6cc0008 132 {
JesseLohman 0:2a5dd6cc0008 133 // Add measure, motor controller and output function
JesseLohman 0:2a5dd6cc0008 134 stateMachine();
JesseLohman 0:2a5dd6cc0008 135 }
JesseLohman 0:2a5dd6cc0008 136
JesseLohman 0:2a5dd6cc0008 137 int main()
JesseLohman 0:2a5dd6cc0008 138 {
JesseLohman 0:2a5dd6cc0008 139 pc.baud(115200);
JesseLohman 0:2a5dd6cc0008 140 mainTicker.attach(mainLoop, sampleTime);
JesseLohman 0:2a5dd6cc0008 141 failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated
JesseLohman 0:2a5dd6cc0008 142
JesseLohman 0:2a5dd6cc0008 143 while (true) {
JesseLohman 0:2a5dd6cc0008 144 pc.printf("State = %i\n", currentState);
JesseLohman 0:2a5dd6cc0008 145 wait(1);
JesseLohman 0:2a5dd6cc0008 146 }
JesseLohman 0:2a5dd6cc0008 147 }