Casper Kroon / Mbed 2 deprecated State_machine2

State machine

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of State_machine by Kilian Buitenhuis

main.cpp@5:e96d03bd557c, 2018-11-01 (annotated)

Committer:
Wabbitdrawing
Date:
Thu Nov 01 14:18:00 2018 +0000
Revision:
5:e96d03bd557c
Parent:
4:dfe39188f2b2
Child:
6:344e075c1221
All constants in a seperate H library;

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"
Wabbitdrawing 5:e96d03bd557c 7 #include "Constants.h
CasperK 2:3f67b4833256 8 #define IGNORECOUNT 100
CasperK 1:afb820c6fc0d 9
CasperK 0:1b2c842eca42 10
CasperK 4:dfe39188f2b2 11 void positionCalibration() {
CasperK 4:dfe39188f2b2 12 while(!button1) {
CasperK 4:dfe39188f2b2 13 directionpin1 = true;
CasperK 4:dfe39188f2b2 14 pwm_value1 = 0.7f;
CasperK 4:dfe39188f2b2 15 }
CasperK 4:dfe39188f2b2 16 pwm_value1 = 0.0f;
CasperK 4:dfe39188f2b2 17 while(!button2) {
CasperK 4:dfe39188f2b2 18 directionpin2 = true;
CasperK 4:dfe39188f2b2 19 pwm_value2 = 0.6f;
CasperK 4:dfe39188f2b2 20 }
CasperK 4:dfe39188f2b2 21 pwm_value2 = 0.0f;
CasperK 4:dfe39188f2b2 22
CasperK 4:dfe39188f2b2 23 if(!next_switch) {
CasperK 4:dfe39188f2b2 24 CurrentState = EmgCalibration;
CasperK 4:dfe39188f2b2 25 }
CasperK 4:dfe39188f2b2 26 }
CasperK 4:dfe39188f2b2 27
CasperK 4:dfe39188f2b2 28 void CalibrateEMG0(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
CasperK 4:dfe39188f2b2 29 ledred = !ledred;
CasperK 4:dfe39188f2b2 30 int C = 500; // half a second at 1000Hz
CasperK 4:dfe39188f2b2 31 double A0=0, A1=0, A2=0, A3=0, A4=0;
CasperK 4:dfe39188f2b2 32 double emg0FiAbs;
CasperK 4:dfe39188f2b2 33 while (C > 0){
CasperK 4:dfe39188f2b2 34 emg0FiAbs = fabs( emg1bqc.step(emg1.read()));
CasperK 4:dfe39188f2b2 35 if (C==500){ //first instance make all values the first in case this is the highest
CasperK 4:dfe39188f2b2 36 A0=A1=A2=A3=A4=emg0FiAbs;
CasperK 4:dfe39188f2b2 37 }
CasperK 4:dfe39188f2b2 38 else if(emg0FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5
CasperK 4:dfe39188f2b2 39 A4=A3;
CasperK 4:dfe39188f2b2 40 A3=A2;
CasperK 4:dfe39188f2b2 41 A2=A1;
CasperK 4:dfe39188f2b2 42 A1=A0;
CasperK 4:dfe39188f2b2 43 A0=emg0FiAbs;
CasperK 4:dfe39188f2b2 44 }
CasperK 4:dfe39188f2b2 45 C--;
CasperK 4:dfe39188f2b2 46 wait(0.001f);
CasperK 4:dfe39188f2b2 47 }
CasperK 4:dfe39188f2b2 48 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 49 ledred = !ledred;
CasperK 4:dfe39188f2b2 50 }
CasperK 3:ed4676f76a5c 51
CasperK 4:dfe39188f2b2 52 void CalibrateEMG1(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
CasperK 4:dfe39188f2b2 53 ledgreen = !ledgreen;
CasperK 4:dfe39188f2b2 54 int C = 500; // half a second at 1000Hz
CasperK 4:dfe39188f2b2 55 double A0=0, A1=0, A2=0, A3=0, A4=0;
CasperK 4:dfe39188f2b2 56 double emg1FiAbs;
CasperK 4:dfe39188f2b2 57 while (C > 0){
CasperK 4:dfe39188f2b2 58 emg1FiAbs = fabs( emg1bqc.step(emg1.read()));
CasperK 4:dfe39188f2b2 59 if (C==500){ //first instance make all values the first in case this is the highest
CasperK 4:dfe39188f2b2 60 A0=A1=A2=A3=A4=emg1FiAbs;
CasperK 4:dfe39188f2b2 61 }
CasperK 4:dfe39188f2b2 62 else if(emg1FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5
CasperK 4:dfe39188f2b2 63 A4=A3;
CasperK 4:dfe39188f2b2 64 A3=A2;
CasperK 4:dfe39188f2b2 65 A2=A1;
CasperK 4:dfe39188f2b2 66 A1=A0;
CasperK 4:dfe39188f2b2 67 A0=emg1FiAbs;
CasperK 4:dfe39188f2b2 68 }
CasperK 4:dfe39188f2b2 69 C--;
CasperK 4:dfe39188f2b2 70 wait(0.001f);
CasperK 4:dfe39188f2b2 71 }
CasperK 4:dfe39188f2b2 72 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 73 ledgreen = !ledgreen;
CasperK 4:dfe39188f2b2 74 }
CasperK 4:dfe39188f2b2 75
CasperK 4:dfe39188f2b2 76 void CalibrateEMG2(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
CasperK 4:dfe39188f2b2 77 ledblue = !ledblue;
CasperK 4:dfe39188f2b2 78 int C = 500; // half a second at 1000Hz
CasperK 4:dfe39188f2b2 79 double A0=0, A1=0, A2=0, A3=0, A4=0;
CasperK 4:dfe39188f2b2 80 double emg2FiAbs;
CasperK 4:dfe39188f2b2 81 while (C > 0){
CasperK 4:dfe39188f2b2 82 emg2FiAbs = fabs( emg2bqc.step(emg2.read()));
CasperK 4:dfe39188f2b2 83 if (C==500){ //first instance make all values the first in case this is the highest
CasperK 4:dfe39188f2b2 84 A0=A1=A2=A3=A4=emg2FiAbs;
CasperK 4:dfe39188f2b2 85 }
CasperK 4:dfe39188f2b2 86 else if(emg2FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5
CasperK 4:dfe39188f2b2 87 A4=A3;
CasperK 4:dfe39188f2b2 88 A3=A2;
CasperK 4:dfe39188f2b2 89 A2=A1;
CasperK 4:dfe39188f2b2 90 A1=A0;
CasperK 4:dfe39188f2b2 91 A0=emg2FiAbs;
CasperK 4:dfe39188f2b2 92 }
CasperK 4:dfe39188f2b2 93 C--;
CasperK 4:dfe39188f2b2 94 wait(0.001f);
CasperK 4:dfe39188f2b2 95 }
CasperK 4:dfe39188f2b2 96 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 97 ledblue = !ledblue;
CasperK 4:dfe39188f2b2 98 }
CasperK 4:dfe39188f2b2 99
CasperK 4:dfe39188f2b2 100 bool emg0Filter(void){
CasperK 4:dfe39188f2b2 101 double emg0filteredAbs = fabs( emg0bqc.step(emg0.read())); // Filter and make absolute,
CasperK 2:3f67b4833256 102 /* this is the threshhold */
CasperK 4:dfe39188f2b2 103 if (emg0filteredAbs > Calibration0) { // when above threshold set bool to 1, here can the parameters be changed using global variables
CasperK 3:ed4676f76a5c 104 emg0Bool = 1;
CasperK 4:dfe39188f2b2 105 emg0Ignore = IGNORECOUNT; // here is the counter reset to catch sudden jolts
CasperK 2:3f67b4833256 106 }
CasperK 2:3f67b4833256 107 else if (emg0Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
CasperK 3:ed4676f76a5c 108 emg0Bool = 0;
CasperK 2:3f67b4833256 109 }
CasperK 2:3f67b4833256 110 else {
CasperK 2:3f67b4833256 111 emg0Ignore--; // else decrease counter by one each time has passed without threshold being met
CasperK 4:dfe39188f2b2 112 }
CasperK 2:3f67b4833256 113 return emg0Bool;
CasperK 2:3f67b4833256 114 }
CasperK 2:3f67b4833256 115
CasperK 2:3f67b4833256 116 bool emg1Filter(void){
CasperK 2:3f67b4833256 117 double emg1filteredAbs = fabs( emg1bqc.step(emg1.read())); // Filter and make absolute
CasperK 2:3f67b4833256 118 /* this is the threshhold */
CasperK 4:dfe39188f2b2 119 if (emg1filteredAbs > Calibration1) { // when above threshold set bool to 1 here can the parameters be changed using global variables
CasperK 2:3f67b4833256 120 emg1Bool = true;
CasperK 2:3f67b4833256 121 emg1Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
CasperK 2:3f67b4833256 122 }
CasperK 2:3f67b4833256 123 else if (emg1Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
CasperK 2:3f67b4833256 124 emg1Bool = false;
CasperK 2:3f67b4833256 125 }
CasperK 2:3f67b4833256 126 else {
CasperK 2:3f67b4833256 127 emg1Ignore--; // else decrease counter by one each time has passed without threshold being met
CasperK 2:3f67b4833256 128 }
CasperK 2:3f67b4833256 129 return emg1Bool;
CasperK 2:3f67b4833256 130 }
CasperK 2:3f67b4833256 131
CasperK 2:3f67b4833256 132 bool emg2Filter(void){
CasperK 2:3f67b4833256 133 double emg2filteredAbs = fabs( emg2bqc.step(emg2.read())); // Filter and make absolute
CasperK 2:3f67b4833256 134 /* this is the threshhold */
CasperK 4:dfe39188f2b2 135 if (emg2filteredAbs > Calibration2) { // when above threshold set bool to 1 here can the parameters be changed using global variables
CasperK 2:3f67b4833256 136 emg2Bool = true;
CasperK 2:3f67b4833256 137 emg2Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
CasperK 2:3f67b4833256 138 }
CasperK 2:3f67b4833256 139 else if (emg2Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
CasperK 4:dfe39188f2b2 140 emg2Bool = false;
CasperK 2:3f67b4833256 141 }
CasperK 2:3f67b4833256 142 else {
CasperK 2:3f67b4833256 143 emg2Ignore--; // else decrease counter by one each time has passed without threshold being met
CasperK 2:3f67b4833256 144 }
CasperK 4:dfe39188f2b2 145
CasperK 2:3f67b4833256 146 return emg2Bool;
CasperK 2:3f67b4833256 147 }
CasperK 2:3f67b4833256 148
CasperK 4:dfe39188f2b2 149 void sample() {
CasperK 4:dfe39188f2b2 150 emg0Filter();
CasperK 4:dfe39188f2b2 151 emg1Filter();
CasperK 4:dfe39188f2b2 152 emg2Filter();
CasperK 1:afb820c6fc0d 153
CasperK 4:dfe39188f2b2 154 scope.set(0, Calibration0);
CasperK 4:dfe39188f2b2 155 scope.set(1, Calibration1);
CasperK 4:dfe39188f2b2 156 scope.set(2, Calibration2);
CasperK 4:dfe39188f2b2 157 scope.set(3, emg0Bool);
CasperK 4:dfe39188f2b2 158 scope.set(4, emg1Bool);
CasperK 4:dfe39188f2b2 159 scope.set(5, emg2Bool);
CasperK 4:dfe39188f2b2 160 scope.send();
CasperK 1:afb820c6fc0d 161 }
CasperK 1:afb820c6fc0d 162
CasperK 4:dfe39188f2b2 163 void emgCalibration() {
CasperK 4:dfe39188f2b2 164 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 165 emg1bqc.add( &emg1bq1 ).add( &emg1bq2 ).add ( &emg1bq3 );
CasperK 4:dfe39188f2b2 166 emg2bqc.add( &emg2bq1 ).add( &emg2bq2 ).add ( &emg2bq3 );
CasperK 4:dfe39188f2b2 167
CasperK 4:dfe39188f2b2 168 bool u = true;
CasperK 4:dfe39188f2b2 169 CalibrationState = EMG0;
CasperK 4:dfe39188f2b2 170 while (u){ // !!! this is a place holder for the calibration!!!
CasperK 4:dfe39188f2b2 171 switch(CalibrationState) {
CasperK 4:dfe39188f2b2 172 case EMG0:
CasperK 4:dfe39188f2b2 173 if(!next_switch) {
CasperK 4:dfe39188f2b2 174 CalibrateEMG0(); // calibration function
CasperK 4:dfe39188f2b2 175 CalibrationState = EMG1;
CasperK 4:dfe39188f2b2 176 }
CasperK 4:dfe39188f2b2 177 break;
CasperK 4:dfe39188f2b2 178 case EMG1:
CasperK 4:dfe39188f2b2 179 if(!next_switch) {
CasperK 4:dfe39188f2b2 180 CalibrateEMG1(); // calibration function
CasperK 4:dfe39188f2b2 181 CalibrationState = EMG2;
CasperK 4:dfe39188f2b2 182 }
CasperK 4:dfe39188f2b2 183 break;
CasperK 4:dfe39188f2b2 184 case EMG2:
CasperK 4:dfe39188f2b2 185 if(!next_switch) {
CasperK 4:dfe39188f2b2 186 CalibrateEMG2(); // calibration function
CasperK 4:dfe39188f2b2 187 CurrentState = Movement;
CasperK 4:dfe39188f2b2 188 sample_timer.attach(&sample, 0.001); //ticker at 1000Hz
CasperK 4:dfe39188f2b2 189 ledred = false; // to indicate filter is working
CasperK 4:dfe39188f2b2 190 ledgreen = false;
CasperK 4:dfe39188f2b2 191 ledblue = false;
CasperK 4:dfe39188f2b2 192 u = false;
CasperK 4:dfe39188f2b2 193 }
CasperK 4:dfe39188f2b2 194 break;
CasperK 2:3f67b4833256 195 }
CasperK 4:dfe39188f2b2 196 }
CasperK 1:afb820c6fc0d 197 }
CasperK 1:afb820c6fc0d 198
CasperK 2:3f67b4833256 199
CasperK 2:3f67b4833256 200
CasperK 1:afb820c6fc0d 201 void movement() {
CasperK 1:afb820c6fc0d 202
CasperK 1:afb820c6fc0d 203 }
CasperK 1:afb820c6fc0d 204
CasperK 1:afb820c6fc0d 205 void move_motors() {
CasperK 1:afb820c6fc0d 206 pwmpin1 = pwm_value1;
CasperK 1:afb820c6fc0d 207 pwmpin2 = pwm_value2;
CasperK 1:afb820c6fc0d 208 }
CasperK 0:1b2c842eca42 209
CasperK 0:1b2c842eca42 210 int main()
CasperK 0:1b2c842eca42 211 {
CasperK 4:dfe39188f2b2 212 // pc.baud(115200);
CasperK 4:dfe39188f2b2 213 // pc.printf(" ** program reset **\n\r");
CasperK 1:afb820c6fc0d 214 pwmpin1.period_us(60);
CasperK 1:afb820c6fc0d 215 pwmpin2.period_us(60);
CasperK 1:afb820c6fc0d 216 directionpin1 = true;
CasperK 1:afb820c6fc0d 217 directionpin2 = true;
CasperK 1:afb820c6fc0d 218
CasperK 3:ed4676f76a5c 219 ledred = true;
CasperK 3:ed4676f76a5c 220 ledgreen = true;
CasperK 3:ed4676f76a5c 221 ledblue = true;
CasperK 4:dfe39188f2b2 222
CasperK 2:3f67b4833256 223 MotorsTicker.attach(&move_motors, 0.02f); //ticker at 50Hz
CasperK 2:3f67b4833256 224
CasperK 1:afb820c6fc0d 225 CurrentState = PositionCalibration;
CasperK 4:dfe39188f2b2 226 // pc.printf("current state = PositionCalibration\n\r");
CasperK 0:1b2c842eca42 227
CasperK 0:1b2c842eca42 228 while (true) {
CasperK 1:afb820c6fc0d 229 switch(CurrentState) {
CasperK 0:1b2c842eca42 230 case PositionCalibration:
CasperK 4:dfe39188f2b2 231 ledgreen = false;
CasperK 4:dfe39188f2b2 232 positionCalibration();
CasperK 4:dfe39188f2b2 233 if (!kill_switch) {
CasperK 4:dfe39188f2b2 234 CurrentState = KILL;
CasperK 4:dfe39188f2b2 235 // pc.printf("current state = KILL\n\r");
CasperK 4:dfe39188f2b2 236 }
CasperK 1:afb820c6fc0d 237 break;
CasperK 1:afb820c6fc0d 238
CasperK 0:1b2c842eca42 239 case EmgCalibration:
CasperK 4:dfe39188f2b2 240 ledgreen = true;
CasperK 4:dfe39188f2b2 241 ledblue = false;
CasperK 4:dfe39188f2b2 242 emgCalibration();
CasperK 4:dfe39188f2b2 243 //emg1Calibration();
CasperK 4:dfe39188f2b2 244 //emg2Calibration();
CasperK 4:dfe39188f2b2 245
CasperK 4:dfe39188f2b2 246 if (!kill_switch) {
CasperK 4:dfe39188f2b2 247 CurrentState = KILL;
CasperK 4:dfe39188f2b2 248 // pc.printf("current state = KILL\n\r");
CasperK 4:dfe39188f2b2 249 }
CasperK 1:afb820c6fc0d 250 break;
CasperK 0:1b2c842eca42 251
CasperK 0:1b2c842eca42 252 case Movement:
CasperK 4:dfe39188f2b2 253 ledred = true;
CasperK 4:dfe39188f2b2 254 ledgreen = false;
CasperK 4:dfe39188f2b2 255 ledblue = false;
CasperK 4:dfe39188f2b2 256 movement();
CasperK 4:dfe39188f2b2 257
CasperK 4:dfe39188f2b2 258 if (!kill_switch) {
CasperK 4:dfe39188f2b2 259 CurrentState = KILL;
CasperK 4:dfe39188f2b2 260 // pc.printf("current state = KILL\n\r");
CasperK 4:dfe39188f2b2 261 }
CasperK 1:afb820c6fc0d 262 break;
CasperK 0:1b2c842eca42 263
CasperK 0:1b2c842eca42 264 case KILL:
CasperK 4:dfe39188f2b2 265 bool u = true;
CasperK 4:dfe39188f2b2 266 ledgreen = true;
CasperK 4:dfe39188f2b2 267 ledblue = true;
CasperK 4:dfe39188f2b2 268 ledred = false;
CasperK 4:dfe39188f2b2 269
CasperK 4:dfe39188f2b2 270 pwm_value1 = 0;
CasperK 4:dfe39188f2b2 271 pwm_value2 = 0;
CasperK 4:dfe39188f2b2 272
CasperK 4:dfe39188f2b2 273 timer.start();
CasperK 4:dfe39188f2b2 274 if (timer.read_ms()> 2000) {
CasperK 4:dfe39188f2b2 275 timer.stop();
CasperK 4:dfe39188f2b2 276 timer.reset();
CasperK 4:dfe39188f2b2 277 while(u) {
CasperK 4:dfe39188f2b2 278 if (!kill_switch) {
CasperK 4:dfe39188f2b2 279 timer.start();
CasperK 4:dfe39188f2b2 280 u = false;
CasperK 4:dfe39188f2b2 281 ledred = true;
CasperK 4:dfe39188f2b2 282 CurrentState = PositionCalibration;
CasperK 4:dfe39188f2b2 283 // pc.printf("current state = PositionCalibration\n\r");
CasperK 4:dfe39188f2b2 284 wait(1.0f);
CasperK 1:afb820c6fc0d 285 }
CasperK 0:1b2c842eca42 286 }
CasperK 4:dfe39188f2b2 287 }
CasperK 1:afb820c6fc0d 288 break;
CasperK 1:afb820c6fc0d 289 }
CasperK 1:afb820c6fc0d 290 wait(0.2f);
CasperK 0:1b2c842eca42 291 }
CasperK 0:1b2c842eca42 292 }