Casper Kroon / Mbed 2 deprecated State_machine2

State machine

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of State_machine by Kilian Buitenhuis

main.cpp@3:ed4676f76a5c, 2018-10-31 (annotated)

Committer:
CasperK
Date:
Wed Oct 31 10:35:27 2018 +0000
Revision:
3:ed4676f76a5c
Parent:
2:3f67b4833256
Child:
4:dfe39188f2b2
HIDSCOPE DOET KUT

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 1:afb820c6fc0d 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 3:ed4676f76a5c 26 BiQuadChain emg0bqc1; // merged chain of three filters
CasperK 3:ed4676f76a5c 27 BiQuadChain emg0bqc2;
CasperK 3:ed4676f76a5c 28 BiQuadChain emg0bqc3;
CasperK 2:3f67b4833256 29
CasperK 2:3f67b4833256 30 AnalogIn emg1( A1 ); // EMG at A1
CasperK 2:3f67b4833256 31 BiQuad emg1bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1
CasperK 2:3f67b4833256 32 BiQuad emg1bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
CasperK 2:3f67b4833256 33 BiQuad emg1bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
CasperK 2:3f67b4833256 34 BiQuadChain emg1bqc; // merged chain of three filters
CasperK 2:3f67b4833256 35
CasperK 2:3f67b4833256 36
CasperK 2:3f67b4833256 37 AnalogIn emg2( A2 ); // EMG at A2
CasperK 2:3f67b4833256 38 BiQuad emg2bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1
CasperK 2:3f67b4833256 39 BiQuad emg2bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
CasperK 2:3f67b4833256 40 BiQuad emg2bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
CasperK 2:3f67b4833256 41 BiQuadChain emg2bqc; // merged chain of three filters
CasperK 2:3f67b4833256 42
CasperK 1:afb820c6fc0d 43 DigitalIn kill_switch(SW2); //position has to be changed
CasperK 1:afb820c6fc0d 44 DigitalIn next_switch(SW3); //name and position should be replaced
CasperK 1:afb820c6fc0d 45
CasperK 2:3f67b4833256 46 enum states{PositionCalibration, EmgCalibration, Movement, KILL};
CasperK 2:3f67b4833256 47 states CurrentState;
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 2:3f67b4833256 56 MODSERIAL pc(USBTX, USBRX);
CasperK 3:ed4676f76a5c 57 HIDScope scope(5);
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 1:afb820c6fc0d 65
CasperK 3:ed4676f76a5c 66 double input = 0; // raw input
CasperK 3:ed4676f76a5c 67 double filtHigh = 0; // filtered after highpass
CasperK 3:ed4676f76a5c 68 double filtlow = 0; // filtered after lowpass
CasperK 3:ed4676f76a5c 69 double filtNotch = 0; // filtered after notch
CasperK 3:ed4676f76a5c 70 double emg0filteredAbs;
CasperK 3:ed4676f76a5c 71
CasperK 2:3f67b4833256 72 float threshold0;
CasperK 2:3f67b4833256 73 float threshold1;
CasperK 2:3f67b4833256 74 float threshold2;
CasperK 1:afb820c6fc0d 75
CasperK 1:afb820c6fc0d 76 volatile float pwm_value1 = 0.0;
CasperK 1:afb820c6fc0d 77 volatile float pwm_value2 = 0.0;
CasperK 0:1b2c842eca42 78
CasperK 2:3f67b4833256 79 /** Sample functions
CasperK 2:3f67b4833256 80 * these functions sample the emg and send it to HIDScope
CasperK 2:3f67b4833256 81 **/
CasperK 2:3f67b4833256 82 bool emg0Filter(void){
CasperK 3:ed4676f76a5c 83
CasperK 3:ed4676f76a5c 84 input = emg0.read();
CasperK 3:ed4676f76a5c 85 scope.set( 0, input);
CasperK 3:ed4676f76a5c 86 filtHigh = emg0bqc1.step(emg0.read());
CasperK 3:ed4676f76a5c 87 scope.set( 1, filtHigh);
CasperK 3:ed4676f76a5c 88 filtlow = emg0bqc2.step(emg0.read());
CasperK 3:ed4676f76a5c 89 scope.set( 2, filtlow);
CasperK 3:ed4676f76a5c 90 filtNotch = emg0bqc3.step(emg0.read());
CasperK 3:ed4676f76a5c 91 scope.set( 3, filtNotch);
CasperK 3:ed4676f76a5c 92
CasperK 2:3f67b4833256 93 /* this is the threshhold */
CasperK 3:ed4676f76a5c 94 emg0filteredAbs = fabs(filtNotch);
CasperK 2:3f67b4833256 95 if (emg0filteredAbs > threshold0) { // when above threshold set bool to 1, here can the parameters be changed using global variables
CasperK 3:ed4676f76a5c 96 emg0Bool = 1;
CasperK 2:3f67b4833256 97 emg0Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
CasperK 2:3f67b4833256 98 }
CasperK 2:3f67b4833256 99 else if (emg0Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
CasperK 3:ed4676f76a5c 100 emg0Bool = 0;
CasperK 2:3f67b4833256 101 }
CasperK 2:3f67b4833256 102 else {
CasperK 2:3f67b4833256 103 emg0Ignore--; // else decrease counter by one each time has passed without threshold being met
CasperK 2:3f67b4833256 104 }
CasperK 3:ed4676f76a5c 105
CasperK 3:ed4676f76a5c 106 scope.set( 4, emg0Bool);
CasperK 3:ed4676f76a5c 107 scope.send();
CasperK 2:3f67b4833256 108 return emg0Bool;
CasperK 2:3f67b4833256 109 }
CasperK 2:3f67b4833256 110
CasperK 2:3f67b4833256 111 bool emg1Filter(void){
CasperK 2:3f67b4833256 112 double emg1filteredAbs = fabs( emg1bqc.step(emg1.read())); // Filter and make absolute
CasperK 2:3f67b4833256 113 /* this is the threshhold */
CasperK 2:3f67b4833256 114 if (emg1filteredAbs > threshold1) { // when above threshold set bool to 1 here can the parameters be changed using global variables
CasperK 2:3f67b4833256 115 emg1Bool = true;
CasperK 2:3f67b4833256 116 emg1Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
CasperK 2:3f67b4833256 117 }
CasperK 2:3f67b4833256 118 else if (emg1Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
CasperK 2:3f67b4833256 119 emg1Bool = false;
CasperK 2:3f67b4833256 120 }
CasperK 2:3f67b4833256 121 else {
CasperK 2:3f67b4833256 122 emg1Ignore--; // else decrease counter by one each time has passed without threshold being met
CasperK 2:3f67b4833256 123 }
CasperK 2:3f67b4833256 124 return emg1Bool;
CasperK 2:3f67b4833256 125 }
CasperK 2:3f67b4833256 126
CasperK 2:3f67b4833256 127 bool emg2Filter(void){
CasperK 2:3f67b4833256 128 double emg2filteredAbs = fabs( emg2bqc.step(emg2.read())); // Filter and make absolute
CasperK 2:3f67b4833256 129 /* this is the threshhold */
CasperK 2:3f67b4833256 130 if (emg2filteredAbs > threshold2) { // when above threshold set bool to 1 here can the parameters be changed using global variables
CasperK 2:3f67b4833256 131 emg2Bool = true;
CasperK 2:3f67b4833256 132 emg2Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
CasperK 2:3f67b4833256 133 }
CasperK 2:3f67b4833256 134 else if (emg2Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
CasperK 2:3f67b4833256 135 emg2Bool = false;
CasperK 2:3f67b4833256 136 }
CasperK 2:3f67b4833256 137 else {
CasperK 2:3f67b4833256 138 emg2Ignore--; // else decrease counter by one each time has passed without threshold being met
CasperK 2:3f67b4833256 139 }
CasperK 2:3f67b4833256 140 return emg2Bool;
CasperK 2:3f67b4833256 141 }
CasperK 2:3f67b4833256 142 void sample() {
CasperK 2:3f67b4833256 143 bool Bool1 = emg0Filter(); // whatever name casper uses for the bool
CasperK 3:ed4676f76a5c 144 // bool Bool2 = emg1Filter();
CasperK 3:ed4676f76a5c 145 // bool Bool3 = emg2Filter();
CasperK 2:3f67b4833256 146 }
CasperK 2:3f67b4833256 147
CasperK 1:afb820c6fc0d 148 void positionCalibration() {
CasperK 1:afb820c6fc0d 149 while(!button1){
CasperK 2:3f67b4833256 150 directionpin1 = true;
CasperK 2:3f67b4833256 151 pwm_value1 = 0.7f;
CasperK 1:afb820c6fc0d 152 }
CasperK 2:3f67b4833256 153 pwm_value1 = 0.0f;
CasperK 1:afb820c6fc0d 154 while(!button2){
CasperK 1:afb820c6fc0d 155 directionpin2 = true;
CasperK 1:afb820c6fc0d 156 pwm_value2 = 0.7f;
CasperK 1:afb820c6fc0d 157 }
CasperK 1:afb820c6fc0d 158 pwm_value2 = 0.0f;
CasperK 1:afb820c6fc0d 159
CasperK 1:afb820c6fc0d 160 // pwm_value1 = potmeter1;
CasperK 1:afb820c6fc0d 161 // pwm_value2 = potmeter2;
CasperK 1:afb820c6fc0d 162
CasperK 1:afb820c6fc0d 163 if (!next_switch) {
CasperK 1:afb820c6fc0d 164 CurrentState = EmgCalibration;
CasperK 1:afb820c6fc0d 165 pc.printf("current state = EmgCalibration\n\r");
CasperK 1:afb820c6fc0d 166 }
CasperK 1:afb820c6fc0d 167 }
CasperK 1:afb820c6fc0d 168
CasperK 2:3f67b4833256 169 void emg0Calibration() {
CasperK 2:3f67b4833256 170 int C = 500; // half a second at 1000Hz
CasperK 2:3f67b4833256 171 double A0=0, A1=0, A2=0, A3=0, A4=0;
CasperK 2:3f67b4833256 172 double emg0FiAbs;
CasperK 2:3f67b4833256 173 while (C > 0){
CasperK 2:3f67b4833256 174 emg0FiAbs = fabs( emg1bqc.step(emg0.read()));
CasperK 2:3f67b4833256 175 if (C==500){ //first instance make all values the first in case this is the highest
CasperK 2:3f67b4833256 176 A0=A1=A2=A3=A4=emg0FiAbs;
CasperK 2:3f67b4833256 177 }
CasperK 2:3f67b4833256 178 else if(emg0FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5
CasperK 2:3f67b4833256 179 A4=A3;
CasperK 2:3f67b4833256 180 A3=A2;
CasperK 2:3f67b4833256 181 A2=A1;
CasperK 2:3f67b4833256 182 A1=A0;
CasperK 2:3f67b4833256 183 A0=emg0FiAbs;
CasperK 2:3f67b4833256 184 }
CasperK 2:3f67b4833256 185 C--;
CasperK 2:3f67b4833256 186 wait(0.001f);
CasperK 2:3f67b4833256 187 threshold0 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold
CasperK 2:3f67b4833256 188 }
CasperK 2:3f67b4833256 189
CasperK 2:3f67b4833256 190 if (!next_switch) {
CasperK 1:afb820c6fc0d 191 CurrentState = Movement;
CasperK 1:afb820c6fc0d 192 pc.printf("current state = Movement\n\r");
CasperK 1:afb820c6fc0d 193 }
CasperK 1:afb820c6fc0d 194 }
CasperK 1:afb820c6fc0d 195
CasperK 2:3f67b4833256 196 void emg1Calibration() {
CasperK 2:3f67b4833256 197
CasperK 2:3f67b4833256 198 }
CasperK 2:3f67b4833256 199
CasperK 2:3f67b4833256 200 void emg2Calibration() {
CasperK 2:3f67b4833256 201
CasperK 2:3f67b4833256 202 }
CasperK 2:3f67b4833256 203
CasperK 1:afb820c6fc0d 204 void movement() {
CasperK 1:afb820c6fc0d 205
CasperK 1:afb820c6fc0d 206 }
CasperK 1:afb820c6fc0d 207
CasperK 1:afb820c6fc0d 208 void move_motors() {
CasperK 1:afb820c6fc0d 209 pwmpin1 = pwm_value1;
CasperK 1:afb820c6fc0d 210 pwmpin2 = pwm_value2;
CasperK 1:afb820c6fc0d 211 }
CasperK 0:1b2c842eca42 212
CasperK 0:1b2c842eca42 213 int main()
CasperK 0:1b2c842eca42 214 {
CasperK 1:afb820c6fc0d 215 pc.baud(115200);
CasperK 1:afb820c6fc0d 216 pc.printf(" ** program reset **\n\r");
CasperK 1:afb820c6fc0d 217 pwmpin1.period_us(60);
CasperK 1:afb820c6fc0d 218 pwmpin2.period_us(60);
CasperK 1:afb820c6fc0d 219 directionpin1 = true;
CasperK 1:afb820c6fc0d 220 directionpin2 = true;
CasperK 1:afb820c6fc0d 221
CasperK 3:ed4676f76a5c 222 ledred = true;
CasperK 3:ed4676f76a5c 223 ledgreen = true;
CasperK 3:ed4676f76a5c 224 ledblue = true;
CasperK 3:ed4676f76a5c 225
CasperK 2:3f67b4833256 226 // emg filters
CasperK 2:3f67b4833256 227 // combining biquad chains is done in main, before the ticker, so only once.
CasperK 3:ed4676f76a5c 228 emg0bqc1.add( &emg0bq1 );
CasperK 3:ed4676f76a5c 229 emg0bqc2.add( &emg0bq1 ).add( &emg0bq2 );
CasperK 3:ed4676f76a5c 230 emg0bqc3.add( &emg0bq1 ).add( &emg0bq2 ).add ( &emg0bq3 ); // combining biquad chains is done in main, before the ticker, so only once.
CasperK 2:3f67b4833256 231
CasperK 2:3f67b4833256 232 MotorsTicker.attach(&move_motors, 0.02f); //ticker at 50Hz
CasperK 2:3f67b4833256 233 sample_timer.attach(&sample, 0.001); //ticker at 1000Hz
CasperK 2:3f67b4833256 234
CasperK 1:afb820c6fc0d 235 CurrentState = PositionCalibration;
CasperK 1:afb820c6fc0d 236 pc.printf("current state = PositionCalibration\n\r");
CasperK 0:1b2c842eca42 237
CasperK 0:1b2c842eca42 238 while (true) {
CasperK 1:afb820c6fc0d 239 switch(CurrentState) {
CasperK 0:1b2c842eca42 240 case PositionCalibration:
CasperK 1:afb820c6fc0d 241 positionCalibration();
CasperK 1:afb820c6fc0d 242 if (!kill_switch) {
CasperK 1:afb820c6fc0d 243 CurrentState = KILL;
CasperK 1:afb820c6fc0d 244 pc.printf("current state = KILL\n\r");
CasperK 1:afb820c6fc0d 245 }
CasperK 1:afb820c6fc0d 246 break;
CasperK 1:afb820c6fc0d 247
CasperK 0:1b2c842eca42 248 case EmgCalibration:
CasperK 2:3f67b4833256 249 emg0Calibration();
CasperK 3:ed4676f76a5c 250 //emg1Calibration();
CasperK 3:ed4676f76a5c 251 //emg2Calibration();
CasperK 1:afb820c6fc0d 252
CasperK 1:afb820c6fc0d 253 if (!kill_switch) {
CasperK 1:afb820c6fc0d 254 CurrentState = KILL;
CasperK 1:afb820c6fc0d 255 pc.printf("current state = KILL\n\r");
CasperK 0:1b2c842eca42 256 }
CasperK 1:afb820c6fc0d 257 break;
CasperK 0:1b2c842eca42 258
CasperK 0:1b2c842eca42 259 case Movement:
CasperK 1:afb820c6fc0d 260 movement();
CasperK 1:afb820c6fc0d 261
CasperK 1:afb820c6fc0d 262 if (!kill_switch) {
CasperK 1:afb820c6fc0d 263 CurrentState = KILL;
CasperK 1:afb820c6fc0d 264 pc.printf("current state = KILL\n\r");
CasperK 0:1b2c842eca42 265 }
CasperK 1:afb820c6fc0d 266 break;
CasperK 0:1b2c842eca42 267
CasperK 0:1b2c842eca42 268 case KILL:
CasperK 1:afb820c6fc0d 269 bool u = true;
CasperK 1:afb820c6fc0d 270 ledgreen = true;
CasperK 1:afb820c6fc0d 271 ledblue = true;
CasperK 1:afb820c6fc0d 272 ledred = false;
CasperK 2:3f67b4833256 273
CasperK 2:3f67b4833256 274 pwm_value1 = 0;
CasperK 2:3f67b4833256 275 pwm_value2 = 0;
CasperK 2:3f67b4833256 276
CasperK 2:3f67b4833256 277 timer.start();
CasperK 2:3f67b4833256 278 if (timer.read_ms()> 2000) {
CasperK 2:3f67b4833256 279 timer.stop();
CasperK 2:3f67b4833256 280 timer.reset();
CasperK 2:3f67b4833256 281 while(u) {
CasperK 2:3f67b4833256 282 if (!kill_switch) {
CasperK 2:3f67b4833256 283 timer.start();
CasperK 2:3f67b4833256 284 u = false;
CasperK 2:3f67b4833256 285 ledred = true;
CasperK 2:3f67b4833256 286 CurrentState = PositionCalibration;
CasperK 2:3f67b4833256 287 pc.printf("current state = PositionCalibration\n\r");
CasperK 2:3f67b4833256 288 wait(1.0f);
CasperK 2:3f67b4833256 289 }
CasperK 1:afb820c6fc0d 290 }
CasperK 0:1b2c842eca42 291 }
CasperK 1:afb820c6fc0d 292 break;
CasperK 1:afb820c6fc0d 293 }
CasperK 1:afb820c6fc0d 294 wait(0.2f);
CasperK 0:1b2c842eca42 295 }
CasperK 0:1b2c842eca42 296 }