Eva Krolis / Mbed 2 deprecated StateMachine

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:   biquadFilter FastPWM HIDScope MODSERIAL mbed

Fork of StateMachine by Tommie Verouden

main.cpp@13:abee61d15b5f, 2018-11-01 (annotated)

Committer:
EvaKrolis
Date:
Thu Nov 01 09:26:12 2018 +0000
Revision:
13:abee61d15b5f
Parent:
12:323ac4e27a0d 
State machine with EMG functions and parameters

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tverouden 0:c0c35b95765f 1 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ PREPARATION ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 0:c0c35b95765f 2 // Libraries
tverouden 0:c0c35b95765f 3 #include "mbed.h"
tverouden 2:d70795e4e0bf 4 #include "BiQuad.h"
tverouden 0:c0c35b95765f 5 #include "FastPWM.h"
tverouden 0:c0c35b95765f 6 #include "HIDScope.h"
tverouden 0:c0c35b95765f 7 #include "MODSERIAL.h"
EvaKrolis 13:abee61d15b5f 8 #include <algorithm>
tverouden 0:c0c35b95765f 9
tverouden 8:8cef1050ebd9 10 // LEDs
tverouden 8:8cef1050ebd9 11 DigitalOut ledRed(LED_RED,1); // red LED K64F
tverouden 8:8cef1050ebd9 12 DigitalOut ledGreen(LED_GREEN,1); // green LED K64F
tverouden 8:8cef1050ebd9 13 DigitalOut ledBlue(LED_BLUE,1); // blue LED K64F
tverouden 8:8cef1050ebd9 14 // DigitalOut ledBio1(,1); // led 1 Biorobotics shield // LED pins
tverouden 8:8cef1050ebd9 15 // DigitalOut ledBio2(,1); // led 2 Biorobotics shield
tverouden 8:8cef1050ebd9 16
tverouden 11:2d1dfebae948 17 Ticker blinkTimer; // LED ticker
tverouden 8:8cef1050ebd9 18
tverouden 8:8cef1050ebd9 19 // Buttons/inputs
tverouden 4:5ce2c8864908 20 InterruptIn buttonBio1(D0); // button 1 BioRobotics shield
tverouden 4:5ce2c8864908 21 InterruptIn buttonBio2(D1); // button 2 BioRobotics shield
tverouden 0:c0c35b95765f 22 InterruptIn buttonK64F(SW3); // button on K64F
tverouden 8:8cef1050ebd9 23 InterruptIn buttonEmergency(SW2); // emergency button on K64F
tverouden 0:c0c35b95765f 24
tverouden 8:8cef1050ebd9 25 // Motor pins
tverouden 8:8cef1050ebd9 26
tverouden 8:8cef1050ebd9 27
tverouden 8:8cef1050ebd9 28 // PC communication
tverouden 4:5ce2c8864908 29 MODSERIAL pc(USBTX, USBRX); // communication with pc
tverouden 4:5ce2c8864908 30
tverouden 0:c0c35b95765f 31 // Define & initialise state machine
tverouden 11:2d1dfebae948 32 enum states { calibratingMotors, calibratingEMG,
tverouden 7:ef5966469621 33 homing, operating, reading, failing, demoing
tverouden 2:d70795e4e0bf 34 };
tverouden 11:2d1dfebae948 35 states currentState = calibratingMotors;// start in waiting mode
tverouden 11:2d1dfebae948 36 bool changeState = true; // initialise the first state
tverouden 2:d70795e4e0bf 37
EvaKrolis 13:abee61d15b5f 38 // Parameters for the EMG
tverouden 3:9c63fc5f157e 39
EvaKrolis 13:abee61d15b5f 40 //EMG inputs
EvaKrolis 13:abee61d15b5f 41 AnalogIn EMG0In(A0); //EMG input 0
EvaKrolis 13:abee61d15b5f 42 AnalogIn EMG1In(A1); //EMG input 1
EvaKrolis 13:abee61d15b5f 43
EvaKrolis 13:abee61d15b5f 44 //Timers
EvaKrolis 13:abee61d15b5f 45 Ticker ReadUseEMG0_timer; //Timer to read, filter and use the EMG
EvaKrolis 13:abee61d15b5f 46 Ticker EMGCalibration0_timer; //Timer for the calibration of the EMG
EvaKrolis 13:abee61d15b5f 47 Ticker FindMax0_timer; //Timer for finding the max muscle
EvaKrolis 13:abee61d15b5f 48 Ticker ReadUseEMG1_timer; //Timer to read, filter and use the EMG
EvaKrolis 13:abee61d15b5f 49 Ticker EMGCalibration1_timer; //Timer for the calibration of the EMG
EvaKrolis 13:abee61d15b5f 50 Ticker FindMax1_timer; //Timer for finding the max muscle
EvaKrolis 13:abee61d15b5f 51 Ticker SwitchState_timer; //Timer to switch from the Calibration to the working mode
EvaKrolis 13:abee61d15b5f 52
EvaKrolis 13:abee61d15b5f 53 //Constants
EvaKrolis 13:abee61d15b5f 54 const int Length = 10000; //Length of the array for the calibration
EvaKrolis 13:abee61d15b5f 55 const int Parts = 50; //Mean average filter over 50 values
EvaKrolis 13:abee61d15b5f 56
EvaKrolis 13:abee61d15b5f 57 //Parameters for the first EMG signal
EvaKrolis 13:abee61d15b5f 58 float EMG0; //float for EMG input
EvaKrolis 13:abee61d15b5f 59 float EMG0filt; //float for filtered EMG
EvaKrolis 13:abee61d15b5f 60 float EMG0filtArray[Parts]; //Array for the filtered array
EvaKrolis 13:abee61d15b5f 61 float EMG0Average; //float for the value after Moving Average Filter
EvaKrolis 13:abee61d15b5f 62 float Sum0 = 0; //Sum0 for the moving average filter
EvaKrolis 13:abee61d15b5f 63 float EMG0Calibrate[Length]; //Array for the calibration
EvaKrolis 13:abee61d15b5f 64 int ReadCal0 = 0; //Integer to read over the calibration array
EvaKrolis 13:abee61d15b5f 65 float MaxValue0 = 0; //float to save the max muscle
EvaKrolis 13:abee61d15b5f 66 float Threshold0 = 0; //Threshold for the first EMG signal
EvaKrolis 13:abee61d15b5f 67
EvaKrolis 13:abee61d15b5f 68 //Parameters for the second EMG signal
EvaKrolis 13:abee61d15b5f 69 float EMG1; //float for EMG input
EvaKrolis 13:abee61d15b5f 70 float EMG1filt; //float for filtered EMG
EvaKrolis 13:abee61d15b5f 71 float EMG1filtArray[Parts]; //Array for the filtered array
EvaKrolis 13:abee61d15b5f 72 float EMG1Average; //float for the value after Moving Average Filter
EvaKrolis 13:abee61d15b5f 73 float Sum1 = 0; //Sum0 for the moving average filter
EvaKrolis 13:abee61d15b5f 74 float EMG1Calibrate[Length]; //Array for the calibration
EvaKrolis 13:abee61d15b5f 75 int ReadCal1 = 0; //Integer to read over the calibration array
EvaKrolis 13:abee61d15b5f 76 float MaxValue1 = 0; //float to save the max muscle
EvaKrolis 13:abee61d15b5f 77 float Threshold1 = 0; //Threshold for the second EMG signal
EvaKrolis 13:abee61d15b5f 78
EvaKrolis 13:abee61d15b5f 79 //Filter variables
EvaKrolis 13:abee61d15b5f 80 BiQuad Notch50_0(0.7887,0,0.7887,0,0.5774); //Make Notch filter around 50 Hz
EvaKrolis 13:abee61d15b5f 81 BiQuad Notch50_1(0.7887,0,0.7887,0,0.5774); //Make Notch filter around 50 Hz
EvaKrolis 13:abee61d15b5f 82 BiQuad High0(0.8006,-1.6012,0.8006,-1.561,0.6414); //Make high-pass filter
EvaKrolis 13:abee61d15b5f 83 BiQuad High1(0.8006,-1.6012,0.8006,-1.561,0.6414); //Make high-pass filter
EvaKrolis 13:abee61d15b5f 84 BiQuadChain filter0; //Make chain of filters for the first EMG signal
EvaKrolis 13:abee61d15b5f 85 BiQuadChain filter1; //Make chain of filters for the second EMG signal
EvaKrolis 13:abee61d15b5f 86
EvaKrolis 13:abee61d15b5f 87 //Bool for movement
EvaKrolis 13:abee61d15b5f 88 bool xMove = false; //Bool for the x-movement
EvaKrolis 13:abee61d15b5f 89 bool yMove = false; //Bool for the y-movement
tverouden 3:9c63fc5f157e 90
tverouden 4:5ce2c8864908 91 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ FUNCTIONS ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 6:f32352bc5078 92 // ============================ GENERAL FUNCTIONS =============================
tverouden 6:f32352bc5078 93 void stopProgram(void)
tverouden 6:f32352bc5078 94 {
tverouden 6:f32352bc5078 95 // Error message
tverouden 6:f32352bc5078 96 pc.printf("[ERROR] emergency button pressed\r\n");
tverouden 6:f32352bc5078 97 currentState = failing; // change to state
tverouden 6:f32352bc5078 98 changeState = true; // next loop, switch states
tverouden 6:f32352bc5078 99 }
tverouden 8:8cef1050ebd9 100
tverouden 8:8cef1050ebd9 101 void blinkLED(DigitalOut led) // blinkTimer.attach(&blinkLED,0.5) aanroepen bij initialisation, bij verlaten state:
tverouden 8:8cef1050ebd9 102 { // blinkTimer.detach
tverouden 8:8cef1050ebd9 103 led =! led; // toggle LED
tverouden 10:584b7d2c2d63 104 }
tverouden 4:5ce2c8864908 105
tverouden 4:5ce2c8864908 106 // ============================= EMG FUNCTIONS ===============================
tverouden 4:5ce2c8864908 107
EvaKrolis 13:abee61d15b5f 108 //Function to read and filter the EMG
EvaKrolis 13:abee61d15b5f 109 void ReadUseEMG0(){
EvaKrolis 13:abee61d15b5f 110 for(int i = Parts ; i > 0 ; i--){ //Make a first in, first out array
EvaKrolis 13:abee61d15b5f 111 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
EvaKrolis 13:abee61d15b5f 112 }
EvaKrolis 13:abee61d15b5f 113
EvaKrolis 13:abee61d15b5f 114 Sum0 = 0;
EvaKrolis 13:abee61d15b5f 115 EMG0 = EMG0In; //Save EMG input in float
EvaKrolis 13:abee61d15b5f 116 EMG0filt = filter0.step(EMG0); //Filter the signal
EvaKrolis 13:abee61d15b5f 117 EMG0filt = abs(EMG0filt); //Take the absolute value
EvaKrolis 13:abee61d15b5f 118 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
EvaKrolis 13:abee61d15b5f 119
EvaKrolis 13:abee61d15b5f 120 for(int i = 0 ; i < Parts ; i++){ //Moving Average filter
EvaKrolis 13:abee61d15b5f 121 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:abee61d15b5f 122 }
EvaKrolis 13:abee61d15b5f 123 EMG0Average = (float)Sum0/Parts; //Divide the sum by 50
EvaKrolis 13:abee61d15b5f 124
EvaKrolis 13:abee61d15b5f 125 if (EMG0Average > Threshold0){ //If the value is higher than the threshold value
EvaKrolis 13:abee61d15b5f 126 xMove = true; //Set movement to true
EvaKrolis 13:abee61d15b5f 127 }
EvaKrolis 13:abee61d15b5f 128 else{
EvaKrolis 13:abee61d15b5f 129 xMove = false; //Otherwise set movement to false
EvaKrolis 13:abee61d15b5f 130 }
EvaKrolis 13:abee61d15b5f 131 }
EvaKrolis 13:abee61d15b5f 132
EvaKrolis 13:abee61d15b5f 133 //Function to read and filter the EMG
EvaKrolis 13:abee61d15b5f 134 void ReadUseEMG1(){
EvaKrolis 13:abee61d15b5f 135 for(int i = Parts ; i > 0 ; i--){ //Make a first in, first out array
EvaKrolis 13:abee61d15b5f 136 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
EvaKrolis 13:abee61d15b5f 137 }
EvaKrolis 13:abee61d15b5f 138
EvaKrolis 13:abee61d15b5f 139 Sum1 = 0;
EvaKrolis 13:abee61d15b5f 140 EMG1 = EMG1In; //Save EMG input in float
EvaKrolis 13:abee61d15b5f 141 EMG1filt = filter1.step(EMG1); //Filter the signal
EvaKrolis 13:abee61d15b5f 142 EMG1filt = abs(EMG1filt); //Take the absolute value
EvaKrolis 13:abee61d15b5f 143 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
EvaKrolis 13:abee61d15b5f 144
EvaKrolis 13:abee61d15b5f 145 for(int i = 0 ; i < Parts ; i++){ //Moving Average filter
EvaKrolis 13:abee61d15b5f 146 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:abee61d15b5f 147 }
EvaKrolis 13:abee61d15b5f 148 EMG1Average = (float)Sum1/Parts; //Divide the sum by 50
EvaKrolis 13:abee61d15b5f 149
EvaKrolis 13:abee61d15b5f 150 if (EMG1Average > Threshold1){ //If the value is higher than the threshold value
EvaKrolis 13:abee61d15b5f 151 yMove = true; //Set y movement to true
EvaKrolis 13:abee61d15b5f 152 }
EvaKrolis 13:abee61d15b5f 153 else{
EvaKrolis 13:abee61d15b5f 154 yMove = false; //Otherwise set y movement to false
EvaKrolis 13:abee61d15b5f 155 }
EvaKrolis 13:abee61d15b5f 156 }
EvaKrolis 13:abee61d15b5f 157
EvaKrolis 13:abee61d15b5f 158 //Function to make an array during the calibration
EvaKrolis 13:abee61d15b5f 159 void CalibrateEMG0(){
EvaKrolis 13:abee61d15b5f 160 for(int i = Parts ; i > 0 ; i--){ //Make a first in, first out array
EvaKrolis 13:abee61d15b5f 161 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
EvaKrolis 13:abee61d15b5f 162 }
EvaKrolis 13:abee61d15b5f 163
EvaKrolis 13:abee61d15b5f 164 Sum0 = 0;
EvaKrolis 13:abee61d15b5f 165 EMG0 = EMG0In; //Save EMG input in float
EvaKrolis 13:abee61d15b5f 166 EMG0filt = filter0.step(EMG0); //Filter the signal
EvaKrolis 13:abee61d15b5f 167 EMG0filt = abs(EMG0filt); //Take the absolute value
EvaKrolis 13:abee61d15b5f 168 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
EvaKrolis 13:abee61d15b5f 169
EvaKrolis 13:abee61d15b5f 170 for(int i = 0 ; i < Parts ; i++){ //Moving Average filter
EvaKrolis 13:abee61d15b5f 171 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:abee61d15b5f 172 }
EvaKrolis 13:abee61d15b5f 173 EMG0Calibrate[ReadCal0] = (float)Sum0/Parts; //Divide the sum by 50
EvaKrolis 13:abee61d15b5f 174
EvaKrolis 13:abee61d15b5f 175 ReadCal0++;
EvaKrolis 13:abee61d15b5f 176 }
EvaKrolis 13:abee61d15b5f 177
EvaKrolis 13:abee61d15b5f 178 //Function to make an array during the calibration
EvaKrolis 13:abee61d15b5f 179 void CalibrateEMG1(){
EvaKrolis 13:abee61d15b5f 180 for(int i = Parts ; i > 0 ; i--){ //Make a first in, first out array
EvaKrolis 13:abee61d15b5f 181 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
EvaKrolis 13:abee61d15b5f 182 }
EvaKrolis 13:abee61d15b5f 183
EvaKrolis 13:abee61d15b5f 184 Sum1 = 0;
EvaKrolis 13:abee61d15b5f 185 EMG1 = EMG1In; //Save EMG input in float
EvaKrolis 13:abee61d15b5f 186 EMG1filt = filter1.step(EMG1); //Filter the signal
EvaKrolis 13:abee61d15b5f 187 EMG1filt = abs(EMG1filt); //Take the absolute value
EvaKrolis 13:abee61d15b5f 188 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
EvaKrolis 13:abee61d15b5f 189
EvaKrolis 13:abee61d15b5f 190 for(int i = 0 ; i < Parts ; i++){ //Moving Average filter
EvaKrolis 13:abee61d15b5f 191 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
EvaKrolis 13:abee61d15b5f 192 }
EvaKrolis 13:abee61d15b5f 193 EMG1Calibrate[ReadCal1] = (float)Sum1/Parts; //Divide the sum by 50
EvaKrolis 13:abee61d15b5f 194
EvaKrolis 13:abee61d15b5f 195 ReadCal1++;
EvaKrolis 13:abee61d15b5f 196 }
EvaKrolis 13:abee61d15b5f 197
EvaKrolis 13:abee61d15b5f 198 //Function to find the max value during the calibration
EvaKrolis 13:abee61d15b5f 199 void FindMax0(){
EvaKrolis 13:abee61d15b5f 200 MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length); //Find max value, but discard the first 100 values
EvaKrolis 13:abee61d15b5f 201 Threshold0 = 0.30f*MaxValue0; //The threshold is a percentage of the max value
EvaKrolis 13:abee61d15b5f 202 pc.printf("The calibration value of the first EMG is %f.\n\r The threshold is %f. \n\r",MaxValue0,Threshold0); //Print the max value and the threshold
EvaKrolis 13:abee61d15b5f 203 FindMax0_timer.detach(); //Detach the timer, so you only use this once
EvaKrolis 13:abee61d15b5f 204 }
EvaKrolis 13:abee61d15b5f 205
EvaKrolis 13:abee61d15b5f 206 //Function to find the max value during the calibration
EvaKrolis 13:abee61d15b5f 207 void FindMax1(){
EvaKrolis 13:abee61d15b5f 208 MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length); //Find max value, but discard the first 100 values
EvaKrolis 13:abee61d15b5f 209 Threshold1 = 0.30f*MaxValue1; //The threshold is a percentage of the max value
EvaKrolis 13:abee61d15b5f 210 pc.printf("The calibration value of the second EMG is %f.\n\r The threshold is %f. \n\r",MaxValue1,Threshold1); //Print the Max value and the threshold
EvaKrolis 13:abee61d15b5f 211 FindMax1_timer.detach(); //Detach the timer, so you only use this once
EvaKrolis 13:abee61d15b5f 212 }
tverouden 4:5ce2c8864908 213
tverouden 12:323ac4e27a0d 214 // ========================= KINEMATICS FUNCTIONS ============================
tverouden 12:323ac4e27a0d 215
tverouden 12:323ac4e27a0d 216
EvaKrolis 13:abee61d15b5f 217 // ============================ MOTOR FUNCTIONS ==============================
EvaKrolis 13:abee61d15b5f 218
EvaKrolis 13:abee61d15b5f 219
tverouden 2:d70795e4e0bf 220 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ STATE MACHINE ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 7:ef5966469621 221 void stateMachine(void)
tverouden 3:9c63fc5f157e 222 {
tverouden 5:04b26b2f536a 223 switch (currentState) { // determine which state Odin is in // Van een aantal if statements moeten de conditions nog bepaald worden, niet vergeten
tverouden 3:9c63fc5f157e 224
tverouden 4:5ce2c8864908 225 // ========================= MOTOR CALIBRATION MODE ==========================
tverouden 4:5ce2c8864908 226 case calibratingMotors:
tverouden 4:5ce2c8864908 227 // ------------------------- initialisation --------------------------
tverouden 4:5ce2c8864908 228 if (changeState) { // when entering the state
tverouden 4:5ce2c8864908 229 pc.printf("[MODE] calibrating motors...r\n");
tverouden 5:04b26b2f536a 230 // print current state
tverouden 4:5ce2c8864908 231 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 232
tverouden 4:5ce2c8864908 233 // Actions when entering state
tverouden 4:5ce2c8864908 234 /* */
tverouden 4:5ce2c8864908 235
tverouden 4:5ce2c8864908 236 }
tverouden 4:5ce2c8864908 237 // ----------------------------- action ------------------------------
tverouden 5:04b26b2f536a 238 // Actions for each loop iteration
tverouden 5:04b26b2f536a 239 /* */
tverouden 4:5ce2c8864908 240
tverouden 4:5ce2c8864908 241 // --------------------------- transition ----------------------------
tverouden 4:5ce2c8864908 242 // Transition condition #1: after 3 sec in state && all motor // Als vóór het einde van de 3 seconden alle motoren al snelheid 0 hebben, zitten ze zo dus door te draaien terwijl dat niet kan
tverouden 4:5ce2c8864908 243 // velocities zero, start calibrating EMG-x // dat lijkt me niet de bedoeling
tverouden 7:ef5966469621 244 if (1) { // CONDITION
tverouden 5:04b26b2f536a 245 // Actions when leaving state
tverouden 4:5ce2c8864908 246 /* */
tverouden 5:04b26b2f536a 247
tverouden 10:584b7d2c2d63 248 currentState = calibratingEMG; // change to state
tverouden 4:5ce2c8864908 249 changeState = true; // next loop, switch states
tverouden 4:5ce2c8864908 250 }
tverouden 4:5ce2c8864908 251 break; // end case
tverouden 4:5ce2c8864908 252
tverouden 7:ef5966469621 253 // =========================== EMG CALIBRATION MODE ===========================
tverouden 7:ef5966469621 254 case calibratingEMG:
tverouden 4:5ce2c8864908 255 // ------------------------- initialisation --------------------------
tverouden 3:9c63fc5f157e 256 if (changeState) { // when entering the state
tverouden 4:5ce2c8864908 257 pc.printf("[MODE] calibrating EMG 1 (x-direction)...\r\n");
tverouden 5:04b26b2f536a 258 // print current state
tverouden 4:5ce2c8864908 259 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 260
tverouden 4:5ce2c8864908 261 // Actions when entering state
tverouden 4:5ce2c8864908 262 /* */
tverouden 4:5ce2c8864908 263
tverouden 4:5ce2c8864908 264 }
tverouden 4:5ce2c8864908 265 // ----------------------------- action ------------------------------
tverouden 5:04b26b2f536a 266 // Actions for each loop iteration
tverouden 5:04b26b2f536a 267 /* */
tverouden 4:5ce2c8864908 268
tverouden 4:5ce2c8864908 269 // --------------------------- transition ----------------------------
tverouden 7:ef5966469621 270 // Transition condition #1: after 20 sec in state
tverouden 7:ef5966469621 271 if (1) { // CONDITION
tverouden 4:5ce2c8864908 272 // Actions when leaving state
tverouden 4:5ce2c8864908 273 /* */
tverouden 5:04b26b2f536a 274
tverouden 4:5ce2c8864908 275 currentState = homing; // change to state
tverouden 4:5ce2c8864908 276 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 277 }
tverouden 5:04b26b2f536a 278 break; // end case
tverouden 4:5ce2c8864908 279
tverouden 4:5ce2c8864908 280 // ============================== HOMING MODE ================================
tverouden 4:5ce2c8864908 281 case homing:
tverouden 7:ef5966469621 282 // ------------------------- initialisation -------------------------- // Evt. ook checken op snelheid als mensen zo dom zijn om tijdens homing op random knopjes te drukken
tverouden 4:5ce2c8864908 283 if (changeState) { // when entering the state
tverouden 4:5ce2c8864908 284 pc.printf("[MODE] homing...\r\n");
tverouden 5:04b26b2f536a 285 // print current state
tverouden 4:5ce2c8864908 286 changeState = false; // stay in this state
tverouden 4:5ce2c8864908 287
tverouden 4:5ce2c8864908 288
tverouden 4:5ce2c8864908 289 // Actions when entering state
tverouden 4:5ce2c8864908 290 /* */
tverouden 4:5ce2c8864908 291
tverouden 4:5ce2c8864908 292 }
tverouden 4:5ce2c8864908 293 // ----------------------------- action ------------------------------
tverouden 5:04b26b2f536a 294 // Actions for each loop iteration
tverouden 5:04b26b2f536a 295 /* */
tverouden 4:5ce2c8864908 296
tverouden 7:ef5966469621 297 // --------------------------- transition ---------------------------- // Volgorde veranderen voor logica?
tverouden 4:5ce2c8864908 298 // Transition condition #1: with button press, enter demo mode
tverouden 4:5ce2c8864908 299 if (buttonBio1 == true) { // Het is niet nodig om hier ook nog "&& currentState == homing" toe te voegen, want hij bereikt dit stuk code alleen in homing mode
tverouden 4:5ce2c8864908 300 // Actions when leaving state
tverouden 4:5ce2c8864908 301 /* */
tverouden 5:04b26b2f536a 302
tverouden 4:5ce2c8864908 303 currentState = demoing; // change to state
tverouden 4:5ce2c8864908 304 changeState = true; // next loop, switch states
tverouden 4:5ce2c8864908 305 }
tverouden 4:5ce2c8864908 306 // Transition condition #2: with button press, enter operation mode
tverouden 4:5ce2c8864908 307 if (buttonBio2 == true) {
tverouden 4:5ce2c8864908 308 // Actions when leaving state
tverouden 4:5ce2c8864908 309 /* */
tverouden 5:04b26b2f536a 310
tverouden 4:5ce2c8864908 311 currentState = operating; // change to state
tverouden 4:5ce2c8864908 312 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 313 }
tverouden 4:5ce2c8864908 314 break; // end case
tverouden 4:5ce2c8864908 315
tverouden 4:5ce2c8864908 316 // ============================= OPERATING MODE ==============================
tverouden 4:5ce2c8864908 317 case operating:
tverouden 4:5ce2c8864908 318 // ------------------------- initialisation --------------------------
tverouden 4:5ce2c8864908 319 if (changeState) { // when entering the state
tverouden 4:5ce2c8864908 320 pc.printf("[MODE] operating...\r\n");
tverouden 5:04b26b2f536a 321 // print current state
tverouden 5:04b26b2f536a 322 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 323
tverouden 5:04b26b2f536a 324 // Actions when entering state
tverouden 5:04b26b2f536a 325 /* */
tverouden 5:04b26b2f536a 326
tverouden 5:04b26b2f536a 327 }
tverouden 5:04b26b2f536a 328 // ----------------------------- action ------------------------------
tverouden 5:04b26b2f536a 329 // Actions for each loop iteration
tverouden 5:04b26b2f536a 330 /* */
tverouden 5:04b26b2f536a 331
tverouden 5:04b26b2f536a 332 // --------------------------- transition ----------------------------
tverouden 7:ef5966469621 333 // Transition condition #1: with button press, back to homing mode // Is het nodig om vanuit operating mode naar homing mode terug te kunnen gaan? -> ja, voor als je een demo wilt starten
tverouden 5:04b26b2f536a 334 if (buttonBio2 == true) {
tverouden 5:04b26b2f536a 335 // Actions when leaving state
tverouden 5:04b26b2f536a 336 /* */
tverouden 5:04b26b2f536a 337
tverouden 5:04b26b2f536a 338 currentState = homing; // change to state
tverouden 5:04b26b2f536a 339 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 340 }
tverouden 7:ef5966469621 341 // Transition condition #2: motor angle error < certain value,
tverouden 7:ef5966469621 342 // start reading
tverouden 7:ef5966469621 343 if (1) { // CONDITION
tverouden 5:04b26b2f536a 344 // Actions when leaving state
tverouden 5:04b26b2f536a 345 /* */
tverouden 5:04b26b2f536a 346
tverouden 5:04b26b2f536a 347 currentState = homing; // change to state
tverouden 5:04b26b2f536a 348 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 349 }
tverouden 5:04b26b2f536a 350 break; // end case
tverouden 5:04b26b2f536a 351
tverouden 7:ef5966469621 352 // ============================== READING MODE =============================== // Beweegt deze modus zowel heen als weer en zo ja, hoe bepaalt hij wat moet gebeuren?
tverouden 7:ef5966469621 353 case reading:
tverouden 5:04b26b2f536a 354 // ------------------------- initialisation --------------------------
tverouden 5:04b26b2f536a 355 if (changeState) { // when entering the state
tverouden 7:ef5966469621 356 pc.printf("[MODE] reading...\r\n");
tverouden 5:04b26b2f536a 357 // print current state
tverouden 5:04b26b2f536a 358 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 359
tverouden 5:04b26b2f536a 360 // Actions when entering state
tverouden 5:04b26b2f536a 361 /* */
tverouden 5:04b26b2f536a 362
tverouden 5:04b26b2f536a 363 }
tverouden 5:04b26b2f536a 364 // ----------------------------- action ------------------------------
tverouden 5:04b26b2f536a 365 // Actions for each loop iteration
tverouden 5:04b26b2f536a 366 /* */
tverouden 5:04b26b2f536a 367
tverouden 5:04b26b2f536a 368 // --------------------------- transition ----------------------------
tverouden 7:ef5966469621 369 // Transition condition #1: with button press, back to homing mode // Hier automatisch terug naar operating mode!
tverouden 5:04b26b2f536a 370 if (1) { // En hij gaat nu terug naar homing mode, maar dan moet je dus elke keer weer kiezen voor demo of operation mode?
tverouden 7:ef5966469621 371 // Actions when leaving state // CONDITION
tverouden 5:04b26b2f536a 372 /* */
tverouden 5:04b26b2f536a 373
tverouden 5:04b26b2f536a 374 currentState = homing; // change to state
tverouden 5:04b26b2f536a 375 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 376 }
tverouden 5:04b26b2f536a 377 break; // end case
tverouden 5:04b26b2f536a 378
tverouden 5:04b26b2f536a 379 // ============================== DEMOING MODE ===============================
tverouden 5:04b26b2f536a 380 case demoing:
tverouden 5:04b26b2f536a 381 // ------------------------- initialisation --------------------------
tverouden 5:04b26b2f536a 382 if (changeState) { // when entering the state
tverouden 5:04b26b2f536a 383 pc.printf("[MODE] demoing...\r\n");
tverouden 5:04b26b2f536a 384 // print current state
tverouden 5:04b26b2f536a 385 changeState = false; // stay in this state
tverouden 5:04b26b2f536a 386
tverouden 5:04b26b2f536a 387 // Actions when entering state
tverouden 5:04b26b2f536a 388 /* */
tverouden 5:04b26b2f536a 389
tverouden 5:04b26b2f536a 390 }
tverouden 5:04b26b2f536a 391 // ----------------------------- action ------------------------------
tverouden 5:04b26b2f536a 392 // Actions for each loop iteration
tverouden 5:04b26b2f536a 393 /* */
tverouden 5:04b26b2f536a 394
tverouden 5:04b26b2f536a 395 // --------------------------- transition ----------------------------
tverouden 5:04b26b2f536a 396 // Transition condition #1: with button press, back to homing mode
tverouden 5:04b26b2f536a 397 if (1) {
tverouden 5:04b26b2f536a 398 // Actions when leaving state
tverouden 5:04b26b2f536a 399 /* */
tverouden 5:04b26b2f536a 400
tverouden 5:04b26b2f536a 401 currentState = homing; // change to state
tverouden 5:04b26b2f536a 402 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 403 }
tverouden 7:ef5966469621 404 // Transition condition #2: after 3 sec relaxation, start reading // In 3 seconden zijn de bladzijden uit zichzelf alweer helemaal teruggegaan. Misschien dit gewoon doen na het voorgedefinieerde pad
tverouden 5:04b26b2f536a 405 if (1) {
tverouden 5:04b26b2f536a 406 // Actions when leaving state
tverouden 5:04b26b2f536a 407 /* */
tverouden 5:04b26b2f536a 408
tverouden 7:ef5966469621 409 currentState = reading; // change to state
tverouden 5:04b26b2f536a 410 changeState = true; // next loop, switch states
tverouden 5:04b26b2f536a 411 }
tverouden 5:04b26b2f536a 412 break; // end case
tverouden 5:04b26b2f536a 413
tverouden 5:04b26b2f536a 414 // =============================== FAILING MODE ================================
tverouden 5:04b26b2f536a 415 case failing:
tverouden 5:04b26b2f536a 416 // ------------------------- initialisation --------------------------
tverouden 5:04b26b2f536a 417 if (changeState) { // when entering the state
tverouden 5:04b26b2f536a 418 pc.printf("[ERROR] entering failure mode\r\n");
tverouden 7:ef5966469621 419 // print current state
tverouden 3:9c63fc5f157e 420 changeState = false; // stay in this state
tverouden 3:9c63fc5f157e 421
tverouden 3:9c63fc5f157e 422 // Actions when entering state
tverouden 7:ef5966469621 423 ledGreen = 1; // red LED on // Blijft dit aan?
tverouden 6:f32352bc5078 424 ledBlue = 1;
tverouden 6:f32352bc5078 425 ledRed = 0;
tverouden 4:5ce2c8864908 426
tverouden 6:f32352bc5078 427 // pin3 = 0; // all motor forces to zero // Pins nog niet gedefiniëerd
tverouden 6:f32352bc5078 428 // pin5 = 0;
tverouden 6:f32352bc5078 429 // pin6 = 0;
tverouden 6:f32352bc5078 430 exit (0); // abort mission
tverouden 4:5ce2c8864908 431 }
tverouden 4:5ce2c8864908 432 break; // end case
tverouden 4:5ce2c8864908 433
tverouden 4:5ce2c8864908 434 // ============================== DEFAULT MODE =================================
tverouden 3:9c63fc5f157e 435 default:
tverouden 4:5ce2c8864908 436 // ---------------------------- enter failing mode -----------------------------
tverouden 4:5ce2c8864908 437 currentState = failing; // change to state
tverouden 4:5ce2c8864908 438 changeState = true; // next loop, switch states
tverouden 4:5ce2c8864908 439 // print current state
tverouden 4:5ce2c8864908 440 pc.printf("[ERROR] unknown or unimplemented state reached\r\n");
tverouden 3:9c63fc5f157e 441
tverouden 4:5ce2c8864908 442 } // end switch
tverouden 4:5ce2c8864908 443 } // end stateMachine
tverouden 3:9c63fc5f157e 444
tverouden 3:9c63fc5f157e 445
tverouden 2:d70795e4e0bf 446
tverouden 2:d70795e4e0bf 447 // ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡ MAIN LOOP ≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡
tverouden 2:d70795e4e0bf 448
tverouden 3:9c63fc5f157e 449 int main()
tverouden 3:9c63fc5f157e 450 {
tverouden 8:8cef1050ebd9 451 // ================================ EMERGENCY ================================
tverouden 7:ef5966469621 452 //If the emergency button is pressed, stop program via failing state
tverouden 8:8cef1050ebd9 453 buttonEmergency.rise(stopProgram); // Automatische triggers voor failure mode?
tverouden 8:8cef1050ebd9 454
tverouden 8:8cef1050ebd9 455 // ================================ EMERGENCY ================================
tverouden 8:8cef1050ebd9 456 pc.baud(115200); // communication with terminal // Baud rate
tverouden 6:f32352bc5078 457
tverouden 2:d70795e4e0bf 458 // ==================================== LOOP ===================================
tverouden 2:d70795e4e0bf 459 while (true) { // loop forever
tverouden 2:d70795e4e0bf 460
tverouden 2:d70795e4e0bf 461 }
tverouden 2:d70795e4e0bf 462 }