Casper Kroon / Mbed 2 deprecated State_machine2

State machine

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of State_machine by Kilian Buitenhuis

main.cpp@6:344e075c1221, 2018-11-02 (annotated)

Committer:
CasperK
Date:
Fri Nov 02 09:16:57 2018 +0000
Revision:
6:344e075c1221
Parent:
5:e96d03bd557c
Child:
7:af586102d80c
All included (excep for kinematics and pid) ; Variables in different folder

Who changed what in which revision?

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