Jesse Lohman / Mbed 2 deprecated state_program1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FastPWM MODSERIAL QEI biquadFilter mbed

Fork of state_program by Fabrice Tshilumba

main.cpp@1:4cb9af313c26, 2018-10-26 (annotated)

Committer:
JesseLohman
Date:
Fri Oct 26 09:12:26 2018 +0000
Revision:
1:4cb9af313c26
Parent:
0:2a5dd6cc0008
Child:
2:5cace74299e7
Added all states, state transitions and if condition for closing/opening gripper and screwing/unscrewing.

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 1:4cb9af313c26 12 DigitalIn grippingSwitch(SW2);
JesseLohman 1:4cb9af313c26 13 DigitalIn screwingSwitch(SW3);
JesseLohman 1:4cb9af313c26 14 DigitalIn gripDirection(D2);
JesseLohman 1:4cb9af313c26 15 DigitalIn screwDirection(D3);
JesseLohman 0:2a5dd6cc0008 16 MODSERIAL pc(USBTX, USBRX);
JesseLohman 1:4cb9af313c26 17 DigitalOut led1(LED1); // Red led
JesseLohman 1:4cb9af313c26 18 DigitalOut led2(LED2); // Green led
JesseLohman 1:4cb9af313c26 19 DigitalOut led3(LED3); // Blue led
JesseLohman 0:2a5dd6cc0008 20
JesseLohman 0:2a5dd6cc0008 21 bool stateChanged = true;
JesseLohman 0:2a5dd6cc0008 22
JesseLohman 0:2a5dd6cc0008 23 Ticker mainTicker;
JesseLohman 0:2a5dd6cc0008 24 Timer stateTimer;
JesseLohman 0:2a5dd6cc0008 25 double sampleTime = 0.01;
JesseLohman 0:2a5dd6cc0008 26
JesseLohman 0:2a5dd6cc0008 27 void switchToFailureState ()
JesseLohman 0:2a5dd6cc0008 28 {
JesseLohman 0:2a5dd6cc0008 29 failureButton.fall(NULL); // Detaches button, so it can only be activated once
JesseLohman 1:4cb9af313c26 30 led1 = 0;
JesseLohman 1:4cb9af313c26 31 led2 = 1;
JesseLohman 1:4cb9af313c26 32 led3 = 1;
JesseLohman 1:4cb9af313c26 33 pc.printf("SYSTEM FAILED\n");
JesseLohman 0:2a5dd6cc0008 34 currentState = FailureState;
JesseLohman 0:2a5dd6cc0008 35 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 36 }
JesseLohman 0:2a5dd6cc0008 37
JesseLohman 0:2a5dd6cc0008 38 void stateMachine ()
JesseLohman 0:2a5dd6cc0008 39 {
JesseLohman 0:2a5dd6cc0008 40 switch (currentState) {
JesseLohman 0:2a5dd6cc0008 41 case WaitState:
JesseLohman 0:2a5dd6cc0008 42 if (stateChanged == true) {
JesseLohman 1:4cb9af313c26 43 led1 = 0;
JesseLohman 1:4cb9af313c26 44 led2 = 1;
JesseLohman 1:4cb9af313c26 45 led3 = 1;
JesseLohman 0:2a5dd6cc0008 46 // Entry action: all the things you do once in this state
JesseLohman 0:2a5dd6cc0008 47 // Set output motor to 0
JesseLohman 0:2a5dd6cc0008 48 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 49 }
JesseLohman 0:2a5dd6cc0008 50
JesseLohman 1:4cb9af313c26 51 if (startButton.read() == false) {
JesseLohman 1:4cb9af313c26 52 //pc.printf("Switching to motor calibration");
JesseLohman 1:4cb9af313c26 53 led1 = 1;
JesseLohman 0:2a5dd6cc0008 54 currentState = MotorCalState;
JesseLohman 0:2a5dd6cc0008 55 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 56 }
JesseLohman 0:2a5dd6cc0008 57
JesseLohman 0:2a5dd6cc0008 58 break;
JesseLohman 0:2a5dd6cc0008 59 case MotorCalState:
JesseLohman 0:2a5dd6cc0008 60 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 61 // Entry action: all the things you do once in this state
JesseLohman 1:4cb9af313c26 62 led2 = 0;
JesseLohman 0:2a5dd6cc0008 63 // Set motorpwm to 'low' value
JesseLohman 0:2a5dd6cc0008 64 stateTimer.reset();
JesseLohman 0:2a5dd6cc0008 65 stateTimer.start();
JesseLohman 0:2a5dd6cc0008 66 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 67 }
JesseLohman 0:2a5dd6cc0008 68
JesseLohman 0:2a5dd6cc0008 69 // Add stuff you do every loop
JesseLohman 0:2a5dd6cc0008 70
JesseLohman 0:2a5dd6cc0008 71 if (stateTimer >= 3.0f) { //TODO: add && motor velocity < 0.1f
JesseLohman 1:4cb9af313c26 72 led2 = 1;
JesseLohman 0:2a5dd6cc0008 73 currentState = EMGCalState;
JesseLohman 0:2a5dd6cc0008 74 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 75 }
JesseLohman 0:2a5dd6cc0008 76 break;
JesseLohman 0:2a5dd6cc0008 77 case EMGCalState:
JesseLohman 0:2a5dd6cc0008 78 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 79 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 80 led3 = 0;
JesseLohman 0:2a5dd6cc0008 81 stateTimer.reset();
JesseLohman 0:2a5dd6cc0008 82 stateTimer.start();
JesseLohman 0:2a5dd6cc0008 83 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 84 }
JesseLohman 0:2a5dd6cc0008 85
JesseLohman 0:2a5dd6cc0008 86 // Add stuff you do every loop
JesseLohman 0:2a5dd6cc0008 87
JesseLohman 0:2a5dd6cc0008 88 if (stateTimer >= 3.0f) {
JesseLohman 1:4cb9af313c26 89 //pc.printf("Starting homing...\n");
JesseLohman 1:4cb9af313c26 90 led3 = 1;
JesseLohman 0:2a5dd6cc0008 91 currentState = HomingState;
JesseLohman 0:2a5dd6cc0008 92 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 93 }
JesseLohman 0:2a5dd6cc0008 94 break;
JesseLohman 0:2a5dd6cc0008 95 case HomingState:
JesseLohman 0:2a5dd6cc0008 96 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 97 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 98 led1 = 0;
JesseLohman 1:4cb9af313c26 99 led2 = 0; // Emits yellow together
JesseLohman 1:4cb9af313c26 100 //TODO: Set intended position
JesseLohman 0:2a5dd6cc0008 101 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 102 }
JesseLohman 0:2a5dd6cc0008 103
JesseLohman 0:2a5dd6cc0008 104 // Nothing extra happens till robot reaches starting position and button is pressed
JesseLohman 0:2a5dd6cc0008 105
JesseLohman 1:4cb9af313c26 106 if (startButton.read() == false) { //TODO: Also add position condition
JesseLohman 1:4cb9af313c26 107 led1 = 1;
JesseLohman 1:4cb9af313c26 108 led2 = 1;
JesseLohman 0:2a5dd6cc0008 109 currentState = MovingState;
JesseLohman 0:2a5dd6cc0008 110 stateChanged = true;
JesseLohman 0:2a5dd6cc0008 111 }
JesseLohman 0:2a5dd6cc0008 112 break;
JesseLohman 0:2a5dd6cc0008 113 case MovingState:
JesseLohman 1:4cb9af313c26 114 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 115 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 116 led1 = 0;
JesseLohman 1:4cb9af313c26 117 led2 = 0;
JesseLohman 1:4cb9af313c26 118 led3 = 0; // Emits white together
JesseLohman 1:4cb9af313c26 119 stateChanged = false;
JesseLohman 1:4cb9af313c26 120 }
JesseLohman 1:4cb9af313c26 121
JesseLohman 1:4cb9af313c26 122 if (grippingSwitch.read() == false) {
JesseLohman 0:2a5dd6cc0008 123 led1 = 1;
JesseLohman 0:2a5dd6cc0008 124 led2 = 1;
JesseLohman 0:2a5dd6cc0008 125 led3 = 1;
JesseLohman 1:4cb9af313c26 126 currentState = GrippingState;
JesseLohman 1:4cb9af313c26 127 stateChanged = true;
JesseLohman 1:4cb9af313c26 128 }
JesseLohman 1:4cb9af313c26 129
JesseLohman 1:4cb9af313c26 130 break;
JesseLohman 1:4cb9af313c26 131 case GrippingState:
JesseLohman 1:4cb9af313c26 132 if (stateChanged == true) {
JesseLohman 1:4cb9af313c26 133 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 134 led2 = 0;
JesseLohman 1:4cb9af313c26 135 led3 = 0; // Emits x together
JesseLohman 0:2a5dd6cc0008 136 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 137 }
JesseLohman 1:4cb9af313c26 138
JesseLohman 1:4cb9af313c26 139 if (gripDirection == true) {
JesseLohman 1:4cb9af313c26 140 // Close gripper
JesseLohman 1:4cb9af313c26 141 } else {
JesseLohman 1:4cb9af313c26 142 // Open gripper
JesseLohman 1:4cb9af313c26 143 }
JesseLohman 1:4cb9af313c26 144
JesseLohman 1:4cb9af313c26 145 if (screwingSwitch.read() == false) {
JesseLohman 1:4cb9af313c26 146 led2 = 1;
JesseLohman 1:4cb9af313c26 147 led3 = 1;
JesseLohman 1:4cb9af313c26 148 currentState = ScrewingState;
JesseLohman 1:4cb9af313c26 149 stateChanged = true;
JesseLohman 1:4cb9af313c26 150 }
JesseLohman 1:4cb9af313c26 151 if (startButton.read() == false) {
JesseLohman 1:4cb9af313c26 152 led2 = 1;
JesseLohman 1:4cb9af313c26 153 led3 = 1;
JesseLohman 1:4cb9af313c26 154 currentState = MovingState;
JesseLohman 1:4cb9af313c26 155 stateChanged = true;
JesseLohman 1:4cb9af313c26 156 }
JesseLohman 0:2a5dd6cc0008 157 break;
JesseLohman 0:2a5dd6cc0008 158 case ScrewingState:
JesseLohman 1:4cb9af313c26 159 if (stateChanged == true) {
JesseLohman 1:4cb9af313c26 160 // Entry action: all the things you do once in this state;
JesseLohman 1:4cb9af313c26 161 led1 = 0;
JesseLohman 1:4cb9af313c26 162 led3 = 0; // Emits pink together
JesseLohman 1:4cb9af313c26 163 stateChanged = false;
JesseLohman 1:4cb9af313c26 164 }
JesseLohman 1:4cb9af313c26 165
JesseLohman 1:4cb9af313c26 166 if (screwDirection == true) {
JesseLohman 1:4cb9af313c26 167 // Screw
JesseLohman 1:4cb9af313c26 168 } else {
JesseLohman 1:4cb9af313c26 169 // Unscrew
JesseLohman 1:4cb9af313c26 170 }
JesseLohman 1:4cb9af313c26 171
JesseLohman 1:4cb9af313c26 172 if (startButton.read() == false) {
JesseLohman 1:4cb9af313c26 173 led1 = 1;
JesseLohman 1:4cb9af313c26 174 led3 = 1;
JesseLohman 1:4cb9af313c26 175 currentState = MovingState;
JesseLohman 1:4cb9af313c26 176 stateChanged = true;
JesseLohman 1:4cb9af313c26 177 }
JesseLohman 0:2a5dd6cc0008 178 break;
JesseLohman 0:2a5dd6cc0008 179 case FailureState:
JesseLohman 0:2a5dd6cc0008 180 if (stateChanged == true) {
JesseLohman 0:2a5dd6cc0008 181 // Entry action: all the things you do once in this state
JesseLohman 1:4cb9af313c26 182 //TODO: Turn all motors off
JesseLohman 0:2a5dd6cc0008 183 stateChanged = false;
JesseLohman 0:2a5dd6cc0008 184 }
JesseLohman 1:4cb9af313c26 185
JesseLohman 1:4cb9af313c26 186 static double blinkTimer = 0;
JesseLohman 1:4cb9af313c26 187 if (blinkTimer >= 0.5) {
JesseLohman 1:4cb9af313c26 188 led1 = !led1;
JesseLohman 1:4cb9af313c26 189 blinkTimer = 0;
JesseLohman 1:4cb9af313c26 190 }
JesseLohman 1:4cb9af313c26 191 blinkTimer += sampleTime;
JesseLohman 1:4cb9af313c26 192
JesseLohman 1:4cb9af313c26 193 break;
JesseLohman 0:2a5dd6cc0008 194 }
JesseLohman 0:2a5dd6cc0008 195 }
JesseLohman 0:2a5dd6cc0008 196
JesseLohman 0:2a5dd6cc0008 197 void mainLoop ()
JesseLohman 0:2a5dd6cc0008 198 {
JesseLohman 0:2a5dd6cc0008 199 // Add measure, motor controller and output function
JesseLohman 0:2a5dd6cc0008 200 stateMachine();
JesseLohman 0:2a5dd6cc0008 201 }
JesseLohman 0:2a5dd6cc0008 202
JesseLohman 0:2a5dd6cc0008 203 int main()
JesseLohman 0:2a5dd6cc0008 204 {
JesseLohman 0:2a5dd6cc0008 205 pc.baud(115200);
JesseLohman 0:2a5dd6cc0008 206 mainTicker.attach(mainLoop, sampleTime);
JesseLohman 0:2a5dd6cc0008 207 failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated
JesseLohman 0:2a5dd6cc0008 208
JesseLohman 0:2a5dd6cc0008 209 while (true) {
JesseLohman 0:2a5dd6cc0008 210 pc.printf("State = %i\n", currentState);
JesseLohman 0:2a5dd6cc0008 211 wait(1);
JesseLohman 0:2a5dd6cc0008 212 }
JesseLohman 0:2a5dd6cc0008 213 }