Casper Kroon / Mbed 2 deprecated State_machine2

State machine

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of State_machine by Kilian Buitenhuis

main.cpp@4:dfe39188f2b2, 2018-11-01 (annotated)

Committer:
CasperK
Date:
Thu Nov 01 14:11:05 2018 +0000
Revision:
4:dfe39188f2b2
Parent:
3:ed4676f76a5c
Child:
5:e96d03bd557c
Implemented emg and emgcalibration

Who changed what in which revision?

UserRevisionLine numberNew contents of line
CasperK 0:1b2c842eca42 1 #include "mbed.h"
CasperK 1:afb820c6fc0d 2 #include "QEI.h"
CasperK 1:afb820c6fc0d 3 #include "HIDScope.h"
CasperK 4:dfe39188f2b2 4 //#include "MODSERIAL.h"
CasperK 2:3f67b4833256 5 #include "BiQuad.h"
CasperK 2:3f67b4833256 6 #include "math.h"
CasperK 2:3f67b4833256 7
CasperK 2:3f67b4833256 8 #define IGNORECOUNT 100
CasperK 1:afb820c6fc0d 9
CasperK 1:afb820c6fc0d 10 PwmOut pwmpin1(D6);
CasperK 1:afb820c6fc0d 11 PwmOut pwmpin2(D5);
CasperK 1:afb820c6fc0d 12 AnalogIn potmeter1(A5);
CasperK 1:afb820c6fc0d 13 AnalogIn potmeter2(A4);
CasperK 1:afb820c6fc0d 14 DigitalIn button1(D2);
CasperK 1:afb820c6fc0d 15 DigitalIn button2(D3);
CasperK 1:afb820c6fc0d 16 DigitalOut directionpin1(D4);
CasperK 1:afb820c6fc0d 17 DigitalOut directionpin2(D7);
CasperK 1:afb820c6fc0d 18 QEI motor1(D13,D12,NC, 32);
CasperK 1:afb820c6fc0d 19 QEI motor2(D11,D10,NC, 32);
CasperK 1:afb820c6fc0d 20
CasperK 2:3f67b4833256 21 //Define objects
CasperK 2:3f67b4833256 22 AnalogIn emg0( A0 ); // EMG at A0
CasperK 2:3f67b4833256 23 BiQuad emg0bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1
CasperK 2:3f67b4833256 24 BiQuad emg0bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
CasperK 2:3f67b4833256 25 BiQuad emg0bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
CasperK 4:dfe39188f2b2 26 BiQuadChain emg0bqc; // merged chain of three filters
CasperK 2:3f67b4833256 27
CasperK 2:3f67b4833256 28 AnalogIn emg1( A1 ); // EMG at A1
CasperK 2:3f67b4833256 29 BiQuad emg1bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1
CasperK 2:3f67b4833256 30 BiQuad emg1bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
CasperK 2:3f67b4833256 31 BiQuad emg1bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
CasperK 2:3f67b4833256 32 BiQuadChain emg1bqc; // merged chain of three filters
CasperK 2:3f67b4833256 33
CasperK 2:3f67b4833256 34 AnalogIn emg2( A2 ); // EMG at A2
CasperK 2:3f67b4833256 35 BiQuad emg2bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1
CasperK 2:3f67b4833256 36 BiQuad emg2bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
CasperK 2:3f67b4833256 37 BiQuad emg2bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
CasperK 2:3f67b4833256 38 BiQuadChain emg2bqc; // merged chain of three filters
CasperK 2:3f67b4833256 39
CasperK 1:afb820c6fc0d 40 DigitalIn kill_switch(SW2); //position has to be changed
CasperK 1:afb820c6fc0d 41 DigitalIn next_switch(SW3); //name and position should be replaced
CasperK 1:afb820c6fc0d 42
CasperK 4:dfe39188f2b2 43 enum position_states{PositionCalibration, EmgCalibration, Movement, KILL};
CasperK 4:dfe39188f2b2 44 enum emg_states{EMG0, EMG1, EMG2};
CasperK 4:dfe39188f2b2 45 position_states CurrentState;
CasperK 4:dfe39188f2b2 46 emg_states CalibrationState;
CasperK 4:dfe39188f2b2 47
CasperK 2:3f67b4833256 48 Ticker sample_timer;
CasperK 2:3f67b4833256 49 Ticker MotorsTicker;
CasperK 2:3f67b4833256 50 Timer timer;
CasperK 2:3f67b4833256 51
CasperK 1:afb820c6fc0d 52 //for testing purposes
CasperK 1:afb820c6fc0d 53 DigitalOut ledred(LED_RED);
CasperK 1:afb820c6fc0d 54 DigitalOut ledgreen(LED_GREEN);
CasperK 1:afb820c6fc0d 55 DigitalOut ledblue(LED_BLUE);
CasperK 4:dfe39188f2b2 56 //MODSERIAL pc(USBTX, USBRX);
CasperK 4:dfe39188f2b2 57 HIDScope scope(6);
CasperK 1:afb820c6fc0d 58
CasperK 2:3f67b4833256 59 bool emg0Bool = 0; // I don't know if these NEED to be global, but when I tried to put them in they wouldn't work...
CasperK 2:3f67b4833256 60 int emg0Ignore = 0;
CasperK 2:3f67b4833256 61 bool emg1Bool = 0;
CasperK 2:3f67b4833256 62 int emg1Ignore = 0;
CasperK 2:3f67b4833256 63 bool emg2Bool = 0;
CasperK 2:3f67b4833256 64 int emg2Ignore = 0;
CasperK 4:dfe39188f2b2 65 float Calibration0;
CasperK 4:dfe39188f2b2 66 float Calibration1;
CasperK 4:dfe39188f2b2 67 float Calibration2;
CasperK 1:afb820c6fc0d 68
CasperK 3:ed4676f76a5c 69 double input = 0; // raw input
CasperK 3:ed4676f76a5c 70 double filtHigh = 0; // filtered after highpass
CasperK 3:ed4676f76a5c 71 double filtlow = 0; // filtered after lowpass
CasperK 3:ed4676f76a5c 72 double filtNotch = 0; // filtered after notch
CasperK 3:ed4676f76a5c 73 double emg0filteredAbs;
CasperK 3:ed4676f76a5c 74
CasperK 2:3f67b4833256 75 float threshold0;
CasperK 2:3f67b4833256 76 float threshold1;
CasperK 2:3f67b4833256 77 float threshold2;
CasperK 1:afb820c6fc0d 78
CasperK 1:afb820c6fc0d 79 volatile float pwm_value1 = 0.0;
CasperK 1:afb820c6fc0d 80 volatile float pwm_value2 = 0.0;
CasperK 0:1b2c842eca42 81
CasperK 4:dfe39188f2b2 82 void positionCalibration() {
CasperK 4:dfe39188f2b2 83 while(!button1) {
CasperK 4:dfe39188f2b2 84 directionpin1 = true;
CasperK 4:dfe39188f2b2 85 pwm_value1 = 0.7f;
CasperK 4:dfe39188f2b2 86 }
CasperK 4:dfe39188f2b2 87 pwm_value1 = 0.0f;
CasperK 4:dfe39188f2b2 88 while(!button2) {
CasperK 4:dfe39188f2b2 89 directionpin2 = true;
CasperK 4:dfe39188f2b2 90 pwm_value2 = 0.6f;
CasperK 4:dfe39188f2b2 91 }
CasperK 4:dfe39188f2b2 92 pwm_value2 = 0.0f;
CasperK 4:dfe39188f2b2 93
CasperK 4:dfe39188f2b2 94 if(!next_switch) {
CasperK 4:dfe39188f2b2 95 CurrentState = EmgCalibration;
CasperK 4:dfe39188f2b2 96 }
CasperK 4:dfe39188f2b2 97 }
CasperK 4:dfe39188f2b2 98
CasperK 4:dfe39188f2b2 99 void CalibrateEMG0(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
CasperK 4:dfe39188f2b2 100 ledred = !ledred;
CasperK 4:dfe39188f2b2 101 int C = 500; // half a second at 1000Hz
CasperK 4:dfe39188f2b2 102 double A0=0, A1=0, A2=0, A3=0, A4=0;
CasperK 4:dfe39188f2b2 103 double emg0FiAbs;
CasperK 4:dfe39188f2b2 104 while (C > 0){
CasperK 4:dfe39188f2b2 105 emg0FiAbs = fabs( emg1bqc.step(emg1.read()));
CasperK 4:dfe39188f2b2 106 if (C==500){ //first instance make all values the first in case this is the highest
CasperK 4:dfe39188f2b2 107 A0=A1=A2=A3=A4=emg0FiAbs;
CasperK 4:dfe39188f2b2 108 }
CasperK 4:dfe39188f2b2 109 else if(emg0FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5
CasperK 4:dfe39188f2b2 110 A4=A3;
CasperK 4:dfe39188f2b2 111 A3=A2;
CasperK 4:dfe39188f2b2 112 A2=A1;
CasperK 4:dfe39188f2b2 113 A1=A0;
CasperK 4:dfe39188f2b2 114 A0=emg0FiAbs;
CasperK 4:dfe39188f2b2 115 }
CasperK 4:dfe39188f2b2 116 C--;
CasperK 4:dfe39188f2b2 117 wait(0.001f);
CasperK 4:dfe39188f2b2 118 }
CasperK 4:dfe39188f2b2 119 Calibration0 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold
CasperK 4:dfe39188f2b2 120 ledred = !ledred;
CasperK 4:dfe39188f2b2 121 }
CasperK 3:ed4676f76a5c 122
CasperK 4:dfe39188f2b2 123 void CalibrateEMG1(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
CasperK 4:dfe39188f2b2 124 ledgreen = !ledgreen;
CasperK 4:dfe39188f2b2 125 int C = 500; // half a second at 1000Hz
CasperK 4:dfe39188f2b2 126 double A0=0, A1=0, A2=0, A3=0, A4=0;
CasperK 4:dfe39188f2b2 127 double emg1FiAbs;
CasperK 4:dfe39188f2b2 128 while (C > 0){
CasperK 4:dfe39188f2b2 129 emg1FiAbs = fabs( emg1bqc.step(emg1.read()));
CasperK 4:dfe39188f2b2 130 if (C==500){ //first instance make all values the first in case this is the highest
CasperK 4:dfe39188f2b2 131 A0=A1=A2=A3=A4=emg1FiAbs;
CasperK 4:dfe39188f2b2 132 }
CasperK 4:dfe39188f2b2 133 else if(emg1FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5
CasperK 4:dfe39188f2b2 134 A4=A3;
CasperK 4:dfe39188f2b2 135 A3=A2;
CasperK 4:dfe39188f2b2 136 A2=A1;
CasperK 4:dfe39188f2b2 137 A1=A0;
CasperK 4:dfe39188f2b2 138 A0=emg1FiAbs;
CasperK 4:dfe39188f2b2 139 }
CasperK 4:dfe39188f2b2 140 C--;
CasperK 4:dfe39188f2b2 141 wait(0.001f);
CasperK 4:dfe39188f2b2 142 }
CasperK 4:dfe39188f2b2 143 Calibration1 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold
CasperK 4:dfe39188f2b2 144 ledgreen = !ledgreen;
CasperK 4:dfe39188f2b2 145 }
CasperK 4:dfe39188f2b2 146
CasperK 4:dfe39188f2b2 147 void CalibrateEMG2(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
CasperK 4:dfe39188f2b2 148 ledblue = !ledblue;
CasperK 4:dfe39188f2b2 149 int C = 500; // half a second at 1000Hz
CasperK 4:dfe39188f2b2 150 double A0=0, A1=0, A2=0, A3=0, A4=0;
CasperK 4:dfe39188f2b2 151 double emg2FiAbs;
CasperK 4:dfe39188f2b2 152 while (C > 0){
CasperK 4:dfe39188f2b2 153 emg2FiAbs = fabs( emg2bqc.step(emg2.read()));
CasperK 4:dfe39188f2b2 154 if (C==500){ //first instance make all values the first in case this is the highest
CasperK 4:dfe39188f2b2 155 A0=A1=A2=A3=A4=emg2FiAbs;
CasperK 4:dfe39188f2b2 156 }
CasperK 4:dfe39188f2b2 157 else if(emg2FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5
CasperK 4:dfe39188f2b2 158 A4=A3;
CasperK 4:dfe39188f2b2 159 A3=A2;
CasperK 4:dfe39188f2b2 160 A2=A1;
CasperK 4:dfe39188f2b2 161 A1=A0;
CasperK 4:dfe39188f2b2 162 A0=emg2FiAbs;
CasperK 4:dfe39188f2b2 163 }
CasperK 4:dfe39188f2b2 164 C--;
CasperK 4:dfe39188f2b2 165 wait(0.001f);
CasperK 4:dfe39188f2b2 166 }
CasperK 4:dfe39188f2b2 167 Calibration2 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold
CasperK 4:dfe39188f2b2 168 ledblue = !ledblue;
CasperK 4:dfe39188f2b2 169 }
CasperK 4:dfe39188f2b2 170
CasperK 4:dfe39188f2b2 171 bool emg0Filter(void){
CasperK 4:dfe39188f2b2 172 double emg0filteredAbs = fabs( emg0bqc.step(emg0.read())); // Filter and make absolute,
CasperK 2:3f67b4833256 173 /* this is the threshhold */
CasperK 4:dfe39188f2b2 174 if (emg0filteredAbs > Calibration0) { // when above threshold set bool to 1, here can the parameters be changed using global variables
CasperK 3:ed4676f76a5c 175 emg0Bool = 1;
CasperK 4:dfe39188f2b2 176 emg0Ignore = IGNORECOUNT; // here is the counter reset to catch sudden jolts
CasperK 2:3f67b4833256 177 }
CasperK 2:3f67b4833256 178 else if (emg0Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
CasperK 3:ed4676f76a5c 179 emg0Bool = 0;
CasperK 2:3f67b4833256 180 }
CasperK 2:3f67b4833256 181 else {
CasperK 2:3f67b4833256 182 emg0Ignore--; // else decrease counter by one each time has passed without threshold being met
CasperK 4:dfe39188f2b2 183 }
CasperK 2:3f67b4833256 184 return emg0Bool;
CasperK 2:3f67b4833256 185 }
CasperK 2:3f67b4833256 186
CasperK 2:3f67b4833256 187 bool emg1Filter(void){
CasperK 2:3f67b4833256 188 double emg1filteredAbs = fabs( emg1bqc.step(emg1.read())); // Filter and make absolute
CasperK 2:3f67b4833256 189 /* this is the threshhold */
CasperK 4:dfe39188f2b2 190 if (emg1filteredAbs > Calibration1) { // when above threshold set bool to 1 here can the parameters be changed using global variables
CasperK 2:3f67b4833256 191 emg1Bool = true;
CasperK 2:3f67b4833256 192 emg1Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
CasperK 2:3f67b4833256 193 }
CasperK 2:3f67b4833256 194 else if (emg1Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
CasperK 2:3f67b4833256 195 emg1Bool = false;
CasperK 2:3f67b4833256 196 }
CasperK 2:3f67b4833256 197 else {
CasperK 2:3f67b4833256 198 emg1Ignore--; // else decrease counter by one each time has passed without threshold being met
CasperK 2:3f67b4833256 199 }
CasperK 2:3f67b4833256 200 return emg1Bool;
CasperK 2:3f67b4833256 201 }
CasperK 2:3f67b4833256 202
CasperK 2:3f67b4833256 203 bool emg2Filter(void){
CasperK 2:3f67b4833256 204 double emg2filteredAbs = fabs( emg2bqc.step(emg2.read())); // Filter and make absolute
CasperK 2:3f67b4833256 205 /* this is the threshhold */
CasperK 4:dfe39188f2b2 206 if (emg2filteredAbs > Calibration2) { // when above threshold set bool to 1 here can the parameters be changed using global variables
CasperK 2:3f67b4833256 207 emg2Bool = true;
CasperK 2:3f67b4833256 208 emg2Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
CasperK 2:3f67b4833256 209 }
CasperK 2:3f67b4833256 210 else if (emg2Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
CasperK 4:dfe39188f2b2 211 emg2Bool = false;
CasperK 2:3f67b4833256 212 }
CasperK 2:3f67b4833256 213 else {
CasperK 2:3f67b4833256 214 emg2Ignore--; // else decrease counter by one each time has passed without threshold being met
CasperK 2:3f67b4833256 215 }
CasperK 4:dfe39188f2b2 216
CasperK 2:3f67b4833256 217 return emg2Bool;
CasperK 2:3f67b4833256 218 }
CasperK 2:3f67b4833256 219
CasperK 4:dfe39188f2b2 220 void sample() {
CasperK 4:dfe39188f2b2 221 emg0Filter();
CasperK 4:dfe39188f2b2 222 emg1Filter();
CasperK 4:dfe39188f2b2 223 emg2Filter();
CasperK 1:afb820c6fc0d 224
CasperK 4:dfe39188f2b2 225 scope.set(0, Calibration0);
CasperK 4:dfe39188f2b2 226 scope.set(1, Calibration1);
CasperK 4:dfe39188f2b2 227 scope.set(2, Calibration2);
CasperK 4:dfe39188f2b2 228 scope.set(3, emg0Bool);
CasperK 4:dfe39188f2b2 229 scope.set(4, emg1Bool);
CasperK 4:dfe39188f2b2 230 scope.set(5, emg2Bool);
CasperK 4:dfe39188f2b2 231 scope.send();
CasperK 1:afb820c6fc0d 232 }
CasperK 1:afb820c6fc0d 233
CasperK 4:dfe39188f2b2 234 void emgCalibration() {
CasperK 4:dfe39188f2b2 235 emg0bqc.add( &emg0bq1 ).add( &emg0bq2 ).add ( &emg0bq3 ); // combining biquad chains is done in main, before the ticker, so it happens only once.
CasperK 4:dfe39188f2b2 236 emg1bqc.add( &emg1bq1 ).add( &emg1bq2 ).add ( &emg1bq3 );
CasperK 4:dfe39188f2b2 237 emg2bqc.add( &emg2bq1 ).add( &emg2bq2 ).add ( &emg2bq3 );
CasperK 4:dfe39188f2b2 238
CasperK 4:dfe39188f2b2 239 bool u = true;
CasperK 4:dfe39188f2b2 240 CalibrationState = EMG0;
CasperK 4:dfe39188f2b2 241 while (u){ // !!! this is a place holder for the calibration!!!
CasperK 4:dfe39188f2b2 242 switch(CalibrationState) {
CasperK 4:dfe39188f2b2 243 case EMG0:
CasperK 4:dfe39188f2b2 244 if(!next_switch) {
CasperK 4:dfe39188f2b2 245 CalibrateEMG0(); // calibration function
CasperK 4:dfe39188f2b2 246 CalibrationState = EMG1;
CasperK 4:dfe39188f2b2 247 }
CasperK 4:dfe39188f2b2 248 break;
CasperK 4:dfe39188f2b2 249 case EMG1:
CasperK 4:dfe39188f2b2 250 if(!next_switch) {
CasperK 4:dfe39188f2b2 251 CalibrateEMG1(); // calibration function
CasperK 4:dfe39188f2b2 252 CalibrationState = EMG2;
CasperK 4:dfe39188f2b2 253 }
CasperK 4:dfe39188f2b2 254 break;
CasperK 4:dfe39188f2b2 255 case EMG2:
CasperK 4:dfe39188f2b2 256 if(!next_switch) {
CasperK 4:dfe39188f2b2 257 CalibrateEMG2(); // calibration function
CasperK 4:dfe39188f2b2 258 CurrentState = Movement;
CasperK 4:dfe39188f2b2 259 sample_timer.attach(&sample, 0.001); //ticker at 1000Hz
CasperK 4:dfe39188f2b2 260 ledred = false; // to indicate filter is working
CasperK 4:dfe39188f2b2 261 ledgreen = false;
CasperK 4:dfe39188f2b2 262 ledblue = false;
CasperK 4:dfe39188f2b2 263 u = false;
CasperK 4:dfe39188f2b2 264 }
CasperK 4:dfe39188f2b2 265 break;
CasperK 2:3f67b4833256 266 }
CasperK 4:dfe39188f2b2 267 }
CasperK 1:afb820c6fc0d 268 }
CasperK 1:afb820c6fc0d 269
CasperK 2:3f67b4833256 270
CasperK 2:3f67b4833256 271
CasperK 1:afb820c6fc0d 272 void movement() {
CasperK 1:afb820c6fc0d 273
CasperK 1:afb820c6fc0d 274 }
CasperK 1:afb820c6fc0d 275
CasperK 1:afb820c6fc0d 276 void move_motors() {
CasperK 1:afb820c6fc0d 277 pwmpin1 = pwm_value1;
CasperK 1:afb820c6fc0d 278 pwmpin2 = pwm_value2;
CasperK 1:afb820c6fc0d 279 }
CasperK 0:1b2c842eca42 280
CasperK 0:1b2c842eca42 281 int main()
CasperK 0:1b2c842eca42 282 {
CasperK 4:dfe39188f2b2 283 // pc.baud(115200);
CasperK 4:dfe39188f2b2 284 // pc.printf(" ** program reset **\n\r");
CasperK 1:afb820c6fc0d 285 pwmpin1.period_us(60);
CasperK 1:afb820c6fc0d 286 pwmpin2.period_us(60);
CasperK 1:afb820c6fc0d 287 directionpin1 = true;
CasperK 1:afb820c6fc0d 288 directionpin2 = true;
CasperK 1:afb820c6fc0d 289
CasperK 3:ed4676f76a5c 290 ledred = true;
CasperK 3:ed4676f76a5c 291 ledgreen = true;
CasperK 3:ed4676f76a5c 292 ledblue = true;
CasperK 4:dfe39188f2b2 293
CasperK 2:3f67b4833256 294 MotorsTicker.attach(&move_motors, 0.02f); //ticker at 50Hz
CasperK 2:3f67b4833256 295
CasperK 1:afb820c6fc0d 296 CurrentState = PositionCalibration;
CasperK 4:dfe39188f2b2 297 // pc.printf("current state = PositionCalibration\n\r");
CasperK 0:1b2c842eca42 298
CasperK 0:1b2c842eca42 299 while (true) {
CasperK 1:afb820c6fc0d 300 switch(CurrentState) {
CasperK 0:1b2c842eca42 301 case PositionCalibration:
CasperK 4:dfe39188f2b2 302 ledgreen = false;
CasperK 4:dfe39188f2b2 303 positionCalibration();
CasperK 4:dfe39188f2b2 304 if (!kill_switch) {
CasperK 4:dfe39188f2b2 305 CurrentState = KILL;
CasperK 4:dfe39188f2b2 306 // pc.printf("current state = KILL\n\r");
CasperK 4:dfe39188f2b2 307 }
CasperK 1:afb820c6fc0d 308 break;
CasperK 1:afb820c6fc0d 309
CasperK 0:1b2c842eca42 310 case EmgCalibration:
CasperK 4:dfe39188f2b2 311 ledgreen = true;
CasperK 4:dfe39188f2b2 312 ledblue = false;
CasperK 4:dfe39188f2b2 313 emgCalibration();
CasperK 4:dfe39188f2b2 314 //emg1Calibration();
CasperK 4:dfe39188f2b2 315 //emg2Calibration();
CasperK 4:dfe39188f2b2 316
CasperK 4:dfe39188f2b2 317 if (!kill_switch) {
CasperK 4:dfe39188f2b2 318 CurrentState = KILL;
CasperK 4:dfe39188f2b2 319 // pc.printf("current state = KILL\n\r");
CasperK 4:dfe39188f2b2 320 }
CasperK 1:afb820c6fc0d 321 break;
CasperK 0:1b2c842eca42 322
CasperK 0:1b2c842eca42 323 case Movement:
CasperK 4:dfe39188f2b2 324 ledred = true;
CasperK 4:dfe39188f2b2 325 ledgreen = false;
CasperK 4:dfe39188f2b2 326 ledblue = false;
CasperK 4:dfe39188f2b2 327 movement();
CasperK 4:dfe39188f2b2 328
CasperK 4:dfe39188f2b2 329 if (!kill_switch) {
CasperK 4:dfe39188f2b2 330 CurrentState = KILL;
CasperK 4:dfe39188f2b2 331 // pc.printf("current state = KILL\n\r");
CasperK 4:dfe39188f2b2 332 }
CasperK 1:afb820c6fc0d 333 break;
CasperK 0:1b2c842eca42 334
CasperK 0:1b2c842eca42 335 case KILL:
CasperK 4:dfe39188f2b2 336 bool u = true;
CasperK 4:dfe39188f2b2 337 ledgreen = true;
CasperK 4:dfe39188f2b2 338 ledblue = true;
CasperK 4:dfe39188f2b2 339 ledred = false;
CasperK 4:dfe39188f2b2 340
CasperK 4:dfe39188f2b2 341 pwm_value1 = 0;
CasperK 4:dfe39188f2b2 342 pwm_value2 = 0;
CasperK 4:dfe39188f2b2 343
CasperK 4:dfe39188f2b2 344 timer.start();
CasperK 4:dfe39188f2b2 345 if (timer.read_ms()> 2000) {
CasperK 4:dfe39188f2b2 346 timer.stop();
CasperK 4:dfe39188f2b2 347 timer.reset();
CasperK 4:dfe39188f2b2 348 while(u) {
CasperK 4:dfe39188f2b2 349 if (!kill_switch) {
CasperK 4:dfe39188f2b2 350 timer.start();
CasperK 4:dfe39188f2b2 351 u = false;
CasperK 4:dfe39188f2b2 352 ledred = true;
CasperK 4:dfe39188f2b2 353 CurrentState = PositionCalibration;
CasperK 4:dfe39188f2b2 354 // pc.printf("current state = PositionCalibration\n\r");
CasperK 4:dfe39188f2b2 355 wait(1.0f);
CasperK 1:afb820c6fc0d 356 }
CasperK 0:1b2c842eca42 357 }
CasperK 4:dfe39188f2b2 358 }
CasperK 1:afb820c6fc0d 359 break;
CasperK 1:afb820c6fc0d 360 }
CasperK 1:afb820c6fc0d 361 wait(0.2f);
CasperK 0:1b2c842eca42 362 }
CasperK 0:1b2c842eca42 363 }