Astrid Schut / Mbed 2 deprecated StateMachineMyoArm

Volledig besturingssysteem voor de myoelectrische prothese arm van mijn bachelor opdracht

Dependencies:   mbed QEI MODSERIAL FastPWM biquadFilter

main.cpp@12:20ae1d20148d, 2019-04-19 (annotated)

Committer:
aschut
Date:
Fri Apr 19 11:51:14 2019 +0000
Revision:
12:20ae1d20148d
Parent:
11:08ba9cfc9c8f
Child:
13:5295c4d4be9d
Child:
14:f8e84d287a22
Elleboog en hand werken op EMG, rotatie gewricht wss ook, maar dat kan niet getest worden

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aschut 0:90750f158475 1 //Voor het toevoegen van een button:
aschut 0:90750f158475 2 #include "mbed.h"
aschut 2:7c9974f0947a 3 #include "MODSERIAL.h"
aschut 2:7c9974f0947a 4 #include "QEI.h"
aschut 2:7c9974f0947a 5 #include "BiQuad.h"
aschut 3:2aaf54ce090b 6 #include "FastPWM.h"
aschut 4:babe09a69296 7
aschut 2:7c9974f0947a 8 // Algemeen
aschut 6:464d2fdfd7de 9 DigitalIn button3(SW3);
aschut 6:464d2fdfd7de 10 DigitalIn button2(SW2);
aschut 8:7fd9ac522ea9 11 DigitalIn But2(D12);
aschut 8:7fd9ac522ea9 12 DigitalIn But1(D13);
aschut 0:90750f158475 13
aschut 0:90750f158475 14 DigitalOut led1(LED_GREEN);
aschut 0:90750f158475 15 DigitalOut led2(LED_RED);
aschut 0:90750f158475 16 DigitalOut led3(LED_BLUE);
aschut 6:464d2fdfd7de 17 float counts = 0;
aschut 0:90750f158475 18
aschut 2:7c9974f0947a 19 MODSERIAL pc(USBTX, USBRX);
aschut 0:90750f158475 20 Timer t;
aschut 2:7c9974f0947a 21 Timer t2;
aschut 6:464d2fdfd7de 22
aschut 2:7c9974f0947a 23 //Motoren
aschut 2:7c9974f0947a 24 DigitalOut direction1(D4);
aschut 6:464d2fdfd7de 25 FastPWM pwmpin1(D5);
aschut 3:2aaf54ce090b 26 FastPWM pwmpin2(D6);
aschut 2:7c9974f0947a 27 DigitalOut direction2(D7);
aschut 2:7c9974f0947a 28 volatile float pwm1;
aschut 2:7c9974f0947a 29 volatile float pwm2;
aschut 2:7c9974f0947a 30
aschut 2:7c9974f0947a 31 //Encoder
aschut 8:7fd9ac522ea9 32 QEI encoder1 (D1, D0, NC, 1200, QEI::X4_ENCODING);
aschut 8:7fd9ac522ea9 33 QEI encoder2 (D3, D2, NC, 4800, QEI::X4_ENCODING);
aschut 2:7c9974f0947a 34 double Pulses1;
aschut 2:7c9974f0947a 35 double motor_position1;
aschut 2:7c9974f0947a 36 double Pulses2;
aschut 2:7c9974f0947a 37 double motor_position2;
aschut 2:7c9974f0947a 38 double error1;
aschut 3:2aaf54ce090b 39 double u1;
aschut 2:7c9974f0947a 40
aschut 2:7c9974f0947a 41 //Pot meter
aschut 8:7fd9ac522ea9 42 AnalogIn pot(A5);
aschut 2:7c9974f0947a 43 AnalogIn pot0(A0);
aschut 2:7c9974f0947a 44 float Pot2;
aschut 2:7c9974f0947a 45 float Pot1;
aschut 2:7c9974f0947a 46
aschut 2:7c9974f0947a 47 //Ticker
aschut 2:7c9974f0947a 48 Ticker Pwm;
aschut 2:7c9974f0947a 49 Ticker PotRead;
aschut 2:7c9974f0947a 50 Ticker Kdc;
aschut 2:7c9974f0947a 51
aschut 4:babe09a69296 52 //Servo
aschut 4:babe09a69296 53 Ticker ServoTick;
aschut 6:464d2fdfd7de 54 DigitalOut myservo1(D8); //Duim
aschut 8:7fd9ac522ea9 55 DigitalOut myservo2(D11); //Pink tot Middel vinger
aschut 6:464d2fdfd7de 56 DigitalOut myservo3(D10); //wijsvinger
aschut 6:464d2fdfd7de 57
aschut 6:464d2fdfd7de 58 float Periodlength = 0.02; // de MG996R heeft een PWM periode van 20 ms
aschut 9:ad4bd1b16eed 59 double servo_position1; // in percentage van 0 tot 1, 0 is met de klok mee, 1 is tegen de klok in.
aschut 9:ad4bd1b16eed 60 double servo_position2;
aschut 9:ad4bd1b16eed 61 double servo_position3;
aschut 6:464d2fdfd7de 62
aschut 6:464d2fdfd7de 63 // Vinger posities
aschut 6:464d2fdfd7de 64 float Duim_krom = 0.05;
aschut 6:464d2fdfd7de 65 float Duim_recht = 0.85;
aschut 6:464d2fdfd7de 66 float MWP_krom = 0.06;
aschut 6:464d2fdfd7de 67 float MWP_recht = 0.89;
aschut 6:464d2fdfd7de 68 float Wijsvinger_krom = 0.15;
aschut 6:464d2fdfd7de 69 float Wijsvinger_recht = 0.93;
aschut 6:464d2fdfd7de 70
aschut 4:babe09a69296 71
aschut 3:2aaf54ce090b 72 // EMG
aschut 3:2aaf54ce090b 73 float EMG1; // Rotatie
aschut 3:2aaf54ce090b 74 float EMG2; // Elleboog
aschut 3:2aaf54ce090b 75 float EMG3; // Hand
aschut 3:2aaf54ce090b 76 float EMG4; // Reverse
aschut 3:2aaf54ce090b 77 float Input1; // Voor zonder EMG
aschut 3:2aaf54ce090b 78 float Input2;
aschut 3:2aaf54ce090b 79 int count = 0;
aschut 3:2aaf54ce090b 80
aschut 9:ad4bd1b16eed 81 //------------- EMG gloabals -------------//
aschut 9:ad4bd1b16eed 82 // Tickers
aschut 9:ad4bd1b16eed 83 Ticker sample_ticker; //ticker voor filteren met 1000Hz
aschut 11:08ba9cfc9c8f 84 Ticker sample_timer;
aschut 9:ad4bd1b16eed 85 Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 1000Hz
aschut 9:ad4bd1b16eed 86 Timer timer_calibration; //timer voor EMG Kalibratie
aschut 9:ad4bd1b16eed 87 double ts = 0.001; //tijdsstap !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
aschut 9:ad4bd1b16eed 88 double calibration_time = 37; //Kalibratie tijd
aschut 9:ad4bd1b16eed 89
aschut 9:ad4bd1b16eed 90 //Input
aschut 9:ad4bd1b16eed 91 AnalogIn emg1( A1 ); //Duim
aschut 9:ad4bd1b16eed 92 AnalogIn emg2( A2 ); //Bicep
aschut 9:ad4bd1b16eed 93 AnalogIn emg3( A3 ); //Dorsaal
aschut 9:ad4bd1b16eed 94 AnalogIn emg4( A4 ); //Palmair
aschut 9:ad4bd1b16eed 95
aschut 9:ad4bd1b16eed 96 // GLOBALS EMG
aschut 9:ad4bd1b16eed 97 //Gefilterde EMG signalen
aschut 9:ad4bd1b16eed 98 volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
aschut 9:ad4bd1b16eed 99
aschut 9:ad4bd1b16eed 100 bool thresholdreach1 = false;
aschut 9:ad4bd1b16eed 101 bool thresholdreach2 = false;
aschut 9:ad4bd1b16eed 102 bool thresholdreach3 = false;
aschut 9:ad4bd1b16eed 103 bool thresholdreach4 = false;
aschut 9:ad4bd1b16eed 104
aschut 9:ad4bd1b16eed 105 volatile double temp_highest_emg1 = 0; //Hoogste waarde gevonden tijdens kalibratie
aschut 9:ad4bd1b16eed 106 volatile double temp_highest_emg2 = 0;
aschut 9:ad4bd1b16eed 107 volatile double temp_highest_emg3 = 0;
aschut 9:ad4bd1b16eed 108 volatile double temp_highest_emg4 = 0;
aschut 9:ad4bd1b16eed 109
aschut 9:ad4bd1b16eed 110 //Percentage van de hoogste waarde waar de bovenste treshold gezet moet worden
aschut 9:ad4bd1b16eed 111 double Duim_p_t = 0.5;
aschut 9:ad4bd1b16eed 112 double Bicep_p_t = 0.4;
aschut 9:ad4bd1b16eed 113 double Dorsaal_p_t = 0.6;
aschut 12:20ae1d20148d 114 double Palmair_p_t = 0.5;
aschut 9:ad4bd1b16eed 115
aschut 9:ad4bd1b16eed 116 //Percentage van de hoogste waarde waar de onderste treshold gezet moet worden
aschut 9:ad4bd1b16eed 117 double Duim_p_tL = 0.5;
aschut 9:ad4bd1b16eed 118 double Bicep_p_tL = 0.4;
aschut 9:ad4bd1b16eed 119 double Dorsaal_p_tL = 0.5;
aschut 9:ad4bd1b16eed 120 double Palmair_p_tL = 0.5;
aschut 9:ad4bd1b16eed 121
aschut 9:ad4bd1b16eed 122 // Waarde bovenste treshold waar het signaal overheen moet om de arm te activeren
aschut 9:ad4bd1b16eed 123 volatile double threshold1;
aschut 9:ad4bd1b16eed 124 volatile double threshold2;
aschut 9:ad4bd1b16eed 125 volatile double threshold3;
aschut 9:ad4bd1b16eed 126 volatile double threshold4;
aschut 9:ad4bd1b16eed 127
aschut 9:ad4bd1b16eed 128 // Waarde onderste treshold waar het signaal onder moet om de arm te deactiveren
aschut 9:ad4bd1b16eed 129 volatile double threshold1L;
aschut 9:ad4bd1b16eed 130 volatile double threshold2L;
aschut 9:ad4bd1b16eed 131 volatile double threshold3L;
aschut 9:ad4bd1b16eed 132 volatile double threshold4L;
aschut 9:ad4bd1b16eed 133
aschut 9:ad4bd1b16eed 134 // thresholdreads bools
aschut 11:08ba9cfc9c8f 135 bool Duim;
aschut 11:08ba9cfc9c8f 136 bool Bicep;
aschut 11:08ba9cfc9c8f 137 bool Dorsaal;
aschut 11:08ba9cfc9c8f 138 bool Palmair;
aschut 9:ad4bd1b16eed 139
aschut 9:ad4bd1b16eed 140 // filters
aschut 9:ad4bd1b16eed 141 //EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
aschut 9:ad4bd1b16eed 142 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
aschut 9:ad4bd1b16eed 143 BiQuadChain highp1;
aschut 9:ad4bd1b16eed 144 BiQuad highp1_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
aschut 9:ad4bd1b16eed 145 BiQuad highp1_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
aschut 9:ad4bd1b16eed 146 BiQuad notch1_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
aschut 9:ad4bd1b16eed 147
aschut 9:ad4bd1b16eed 148 //Lowpass first order cutoff 0.4Hz
aschut 9:ad4bd1b16eed 149 BiQuad lowp1( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
aschut 9:ad4bd1b16eed 150
aschut 9:ad4bd1b16eed 151 //EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
aschut 9:ad4bd1b16eed 152 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
aschut 9:ad4bd1b16eed 153 BiQuadChain highp2;
aschut 9:ad4bd1b16eed 154 BiQuad highp2_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
aschut 9:ad4bd1b16eed 155 BiQuad highp2_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
aschut 9:ad4bd1b16eed 156 BiQuad notch2_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
aschut 9:ad4bd1b16eed 157
aschut 9:ad4bd1b16eed 158 //Lowpass first order cutoff 0.4Hz
aschut 9:ad4bd1b16eed 159 BiQuad lowp2( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
aschut 9:ad4bd1b16eed 160
aschut 9:ad4bd1b16eed 161 //EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
aschut 9:ad4bd1b16eed 162 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
aschut 9:ad4bd1b16eed 163 BiQuadChain highp3;
aschut 9:ad4bd1b16eed 164 BiQuad highp3_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
aschut 9:ad4bd1b16eed 165 BiQuad highp3_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
aschut 9:ad4bd1b16eed 166 BiQuad notch3_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
aschut 9:ad4bd1b16eed 167
aschut 9:ad4bd1b16eed 168 //Lowpass first order cutoff 0.4Hz
aschut 9:ad4bd1b16eed 169 BiQuad lowp3( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
aschut 9:ad4bd1b16eed 170
aschut 9:ad4bd1b16eed 171 //EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
aschut 9:ad4bd1b16eed 172 //Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
aschut 9:ad4bd1b16eed 173 BiQuadChain highp4;
aschut 9:ad4bd1b16eed 174 BiQuad highp4_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
aschut 9:ad4bd1b16eed 175 BiQuad highp4_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
aschut 9:ad4bd1b16eed 176 BiQuad notch4_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );
aschut 9:ad4bd1b16eed 177
aschut 9:ad4bd1b16eed 178 //Lowpass first order cutoff 0.4Hz
aschut 9:ad4bd1b16eed 179 BiQuad lowp4( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );
aschut 9:ad4bd1b16eed 180
aschut 2:7c9974f0947a 181 //Kinematica
aschut 2:7c9974f0947a 182 double stap1;
aschut 2:7c9974f0947a 183 double stap2;
aschut 2:7c9974f0947a 184 double KPot;
aschut 2:7c9974f0947a 185
aschut 2:7c9974f0947a 186 float ElbowReference;
aschut 2:7c9974f0947a 187 float Ellebooghoek1;
aschut 2:7c9974f0947a 188 float Ellebooghoek2;
aschut 2:7c9974f0947a 189 float Ellebooghoek3;
aschut 2:7c9974f0947a 190 float Ellebooghoek4;
aschut 2:7c9974f0947a 191
aschut 2:7c9974f0947a 192 float PolsReference;
aschut 2:7c9974f0947a 193 float Polshoek1;
aschut 2:7c9974f0947a 194 float Polshoek2;
aschut 2:7c9974f0947a 195 float Polshoek3;
aschut 2:7c9974f0947a 196 float Polshoek4;
aschut 2:7c9974f0947a 197
aschut 2:7c9974f0947a 198 float Hoeknieuw1;
aschut 2:7c9974f0947a 199 float Hoeknieuw2;
aschut 2:7c9974f0947a 200
aschut 2:7c9974f0947a 201 //Limiet in graden
aschut 3:2aaf54ce090b 202 float lowerlim1 = -900;
aschut 3:2aaf54ce090b 203 float upperlim1 = 900;
aschut 6:464d2fdfd7de 204 float lowerlim2 = 0;
aschut 6:464d2fdfd7de 205 float upperlim2 = 1500;
aschut 2:7c9974f0947a 206
aschut 2:7c9974f0947a 207 // VARIABLES PID CONTROLLER
aschut 6:464d2fdfd7de 208 double Kp1 = 12.5;
aschut 6:464d2fdfd7de 209 double Ki1 = 0;
aschut 2:7c9974f0947a 210 double Kd1 = 1;
aschut 8:7fd9ac522ea9 211 double Kp2 = 20; // Zonder arm: 6,0,1, met rotatie: 10, 0, 1
aschut 6:464d2fdfd7de 212 double Ki2 = 0;
aschut 6:464d2fdfd7de 213 double Kd2 = 1;
aschut 2:7c9974f0947a 214 double Ts = 0.0005; // Sample time in seconds
aschut 2:7c9974f0947a 215
aschut 9:ad4bd1b16eed 216
aschut 9:ad4bd1b16eed 217 // ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~
aschut 9:ad4bd1b16eed 218 void emgsample()
aschut 9:ad4bd1b16eed 219 {
aschut 9:ad4bd1b16eed 220
aschut 9:ad4bd1b16eed 221 // EMG signaal lezen
aschut 9:ad4bd1b16eed 222 double emgread1 = emg1.read();
aschut 9:ad4bd1b16eed 223 double emgread2 = emg2.read();
aschut 9:ad4bd1b16eed 224 double emgread3 = emg3.read();
aschut 9:ad4bd1b16eed 225 double emgread4 = emg4.read();
aschut 9:ad4bd1b16eed 226
aschut 9:ad4bd1b16eed 227 // Vierde orde highpass filter + notch filter
aschut 9:ad4bd1b16eed 228 double emg1_highpassed = highp1.step(emgread1);
aschut 9:ad4bd1b16eed 229 double emg2_highpassed = highp2.step(emgread2);
aschut 9:ad4bd1b16eed 230 double emg3_highpassed = highp3.step(emgread3);
aschut 9:ad4bd1b16eed 231 double emg4_highpassed = highp4.step(emgread4);
aschut 9:ad4bd1b16eed 232
aschut 9:ad4bd1b16eed 233 //Rectificatie
aschut 9:ad4bd1b16eed 234 double emg1_abs = abs(emg1_highpassed);
aschut 9:ad4bd1b16eed 235 double emg2_abs = abs(emg2_highpassed);
aschut 9:ad4bd1b16eed 236 double emg3_abs = abs(emg3_highpassed);
aschut 9:ad4bd1b16eed 237 double emg4_abs = abs(emg4_highpassed);
aschut 9:ad4bd1b16eed 238
aschut 9:ad4bd1b16eed 239 //All EMG abs into lowpass
aschut 9:ad4bd1b16eed 240 emg1_filtered = lowp1.step(emg1_abs);
aschut 9:ad4bd1b16eed 241 emg2_filtered = lowp2.step(emg2_abs);
aschut 9:ad4bd1b16eed 242 emg3_filtered = lowp3.step(emg3_abs);
aschut 9:ad4bd1b16eed 243 emg4_filtered = lowp4.step(emg4_abs);
aschut 9:ad4bd1b16eed 244
aschut 9:ad4bd1b16eed 245 }
aschut 9:ad4bd1b16eed 246
aschut 9:ad4bd1b16eed 247 //Check of emg_filtered boven de threshold is
aschut 9:ad4bd1b16eed 248 void threshold_check()
aschut 9:ad4bd1b16eed 249 {
aschut 9:ad4bd1b16eed 250
aschut 9:ad4bd1b16eed 251 // EMG1 Check
aschut 9:ad4bd1b16eed 252 if (thresholdreach1 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest
aschut 9:ad4bd1b16eed 253 //bovenste threshold check
aschut 9:ad4bd1b16eed 254 if(emg1_filtered>threshold1) {
aschut 9:ad4bd1b16eed 255 Duim = 1;
aschut 9:ad4bd1b16eed 256 thresholdreach1 = true;
aschut 9:ad4bd1b16eed 257
aschut 9:ad4bd1b16eed 258 } else {
aschut 9:ad4bd1b16eed 259 Duim= 0;
aschut 9:ad4bd1b16eed 260 }
aschut 9:ad4bd1b16eed 261 }
aschut 9:ad4bd1b16eed 262 else{ //Als emg_filtered boven de bovenste threshold is geweest
aschut 9:ad4bd1b16eed 263 //onderste threshold check
aschut 9:ad4bd1b16eed 264 if(emg1_filtered<threshold1L) {
aschut 9:ad4bd1b16eed 265 Duim = 0;
aschut 9:ad4bd1b16eed 266 thresholdreach1 = false;
aschut 9:ad4bd1b16eed 267
aschut 9:ad4bd1b16eed 268 } else {
aschut 9:ad4bd1b16eed 269 Duim= 1;
aschut 9:ad4bd1b16eed 270 }
aschut 9:ad4bd1b16eed 271 }
aschut 9:ad4bd1b16eed 272
aschut 9:ad4bd1b16eed 273 // EMG2 Check
aschut 9:ad4bd1b16eed 274 if (thresholdreach2 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest
aschut 9:ad4bd1b16eed 275 //bovenste threshold check
aschut 9:ad4bd1b16eed 276 if(emg2_filtered>threshold2) {
aschut 9:ad4bd1b16eed 277 Bicep = 1;
aschut 9:ad4bd1b16eed 278 thresholdreach2 = true;
aschut 9:ad4bd1b16eed 279 } else {
aschut 9:ad4bd1b16eed 280 Bicep= 0;
aschut 9:ad4bd1b16eed 281 }
aschut 9:ad4bd1b16eed 282 }
aschut 9:ad4bd1b16eed 283 else{ //Als emg_filtered boven de bovenste threshold is geweest
aschut 9:ad4bd1b16eed 284 //onderste threshold check
aschut 9:ad4bd1b16eed 285 if(emg2_filtered<threshold2L) {
aschut 9:ad4bd1b16eed 286 Bicep = 0;
aschut 9:ad4bd1b16eed 287 thresholdreach2 = false;
aschut 9:ad4bd1b16eed 288
aschut 9:ad4bd1b16eed 289 } else {
aschut 9:ad4bd1b16eed 290 Bicep= 1;
aschut 9:ad4bd1b16eed 291 }
aschut 9:ad4bd1b16eed 292 }
aschut 9:ad4bd1b16eed 293
aschut 9:ad4bd1b16eed 294 // EMG3 Check
aschut 9:ad4bd1b16eed 295 if (thresholdreach3 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest
aschut 9:ad4bd1b16eed 296 //bovenste threshold check
aschut 9:ad4bd1b16eed 297 if(emg3_filtered>threshold3) {
aschut 9:ad4bd1b16eed 298 Dorsaal = 1;
aschut 9:ad4bd1b16eed 299 thresholdreach3 = true;
aschut 9:ad4bd1b16eed 300 } else {
aschut 9:ad4bd1b16eed 301 Dorsaal= 0;
aschut 9:ad4bd1b16eed 302 }
aschut 9:ad4bd1b16eed 303 }
aschut 9:ad4bd1b16eed 304 else{ //Als emg_filtered boven de bovenste threshold is geweest
aschut 9:ad4bd1b16eed 305 //onderste threshold check
aschut 9:ad4bd1b16eed 306 if(emg3_filtered<threshold3L) {
aschut 9:ad4bd1b16eed 307 Dorsaal = 0;
aschut 9:ad4bd1b16eed 308 thresholdreach3 = false;
aschut 9:ad4bd1b16eed 309
aschut 9:ad4bd1b16eed 310 } else {
aschut 9:ad4bd1b16eed 311 Dorsaal= 1;
aschut 9:ad4bd1b16eed 312 }
aschut 9:ad4bd1b16eed 313 }
aschut 9:ad4bd1b16eed 314
aschut 9:ad4bd1b16eed 315 // EMG4 Check
aschut 9:ad4bd1b16eed 316 if (thresholdreach4 == false){ //Als emg_filtered nog niet boven de bovenste threshold is geweest
aschut 9:ad4bd1b16eed 317 //bovenste threshold check
aschut 9:ad4bd1b16eed 318 if(emg4_filtered>threshold4) {
aschut 9:ad4bd1b16eed 319 Palmair = 1;
aschut 9:ad4bd1b16eed 320 thresholdreach4 = true;
aschut 9:ad4bd1b16eed 321 } else {
aschut 9:ad4bd1b16eed 322 Palmair= 0;
aschut 9:ad4bd1b16eed 323 }
aschut 9:ad4bd1b16eed 324 }
aschut 9:ad4bd1b16eed 325 else{ //Als emg_filtered boven de bovenste threshold is geweest
aschut 9:ad4bd1b16eed 326 //onderste threshold check
aschut 9:ad4bd1b16eed 327 if(emg4_filtered<threshold4L) {
aschut 9:ad4bd1b16eed 328 Palmair = 0;
aschut 9:ad4bd1b16eed 329 thresholdreach4 = false;
aschut 9:ad4bd1b16eed 330
aschut 9:ad4bd1b16eed 331 } else {
aschut 9:ad4bd1b16eed 332 Palmair= 1;
aschut 9:ad4bd1b16eed 333 }
aschut 9:ad4bd1b16eed 334 }
aschut 9:ad4bd1b16eed 335
aschut 9:ad4bd1b16eed 336 }
aschut 9:ad4bd1b16eed 337
aschut 2:7c9974f0947a 338 // Functies Kinematica
aschut 3:2aaf54ce090b 339 float Kinematics1(float EMG1)
aschut 2:7c9974f0947a 340 {
aschut 2:7c9974f0947a 341
aschut 12:20ae1d20148d 342 if (Dorsaal == 1 && Duim == 0) {
aschut 12:20ae1d20148d 343 stap1 = 450*Ts;
aschut 2:7c9974f0947a 344 Hoeknieuw1 = PolsReference + stap1;
aschut 2:7c9974f0947a 345 return Hoeknieuw1;
aschut 2:7c9974f0947a 346 }
aschut 2:7c9974f0947a 347
aschut 12:20ae1d20148d 348 else if (Dorsaal == 1 && Duim == 1) {
aschut 12:20ae1d20148d 349 stap1 = 450*Ts;
aschut 12:20ae1d20148d 350 Hoeknieuw1 = PolsReference - stap1;
aschut 2:7c9974f0947a 351 return Hoeknieuw1;
aschut 2:7c9974f0947a 352 }
aschut 2:7c9974f0947a 353
aschut 2:7c9974f0947a 354 else {
aschut 2:7c9974f0947a 355 return PolsReference;
aschut 2:7c9974f0947a 356 }
aschut 2:7c9974f0947a 357 }
aschut 11:08ba9cfc9c8f 358 float Kinematics2(bool Dorsaal)
aschut 2:7c9974f0947a 359 {
aschut 2:7c9974f0947a 360
aschut 11:08ba9cfc9c8f 361 if (Dorsaal == 1 && Duim == 0) {
aschut 11:08ba9cfc9c8f 362 stap2 = 300*Ts;
aschut 2:7c9974f0947a 363 Hoeknieuw2 = ElbowReference + stap2;
aschut 2:7c9974f0947a 364 return Hoeknieuw2;
aschut 2:7c9974f0947a 365 }
aschut 2:7c9974f0947a 366
aschut 11:08ba9cfc9c8f 367 else if (Dorsaal == 1 && Duim == 1) {
aschut 11:08ba9cfc9c8f 368 stap2 = 300*Ts;
aschut 11:08ba9cfc9c8f 369 Hoeknieuw2 = ElbowReference - stap2;
aschut 2:7c9974f0947a 370 return Hoeknieuw2;
aschut 2:7c9974f0947a 371 }
aschut 2:7c9974f0947a 372
aschut 2:7c9974f0947a 373 else {
aschut 2:7c9974f0947a 374 return ElbowReference;
aschut 2:7c9974f0947a 375 }
aschut 2:7c9974f0947a 376 }
aschut 2:7c9974f0947a 377
aschut 2:7c9974f0947a 378 float Limits1(float Polshoek2)
aschut 2:7c9974f0947a 379 {
aschut 2:7c9974f0947a 380
aschut 2:7c9974f0947a 381 if (Polshoek2 <= upperlim1 && Polshoek2 >= lowerlim1) { //Binnen de limieten
aschut 2:7c9974f0947a 382 Polshoek3 = Polshoek2;
aschut 2:7c9974f0947a 383 }
aschut 2:7c9974f0947a 384
aschut 2:7c9974f0947a 385 else {
aschut 2:7c9974f0947a 386 if (Polshoek2 >= upperlim1) { //Boven de limiet
aschut 2:7c9974f0947a 387 Polshoek3 = upperlim1;
aschut 2:7c9974f0947a 388 } else { //Onder de limiet
aschut 2:7c9974f0947a 389 Polshoek3 = lowerlim1;
aschut 2:7c9974f0947a 390 }
aschut 2:7c9974f0947a 391 }
aschut 2:7c9974f0947a 392
aschut 2:7c9974f0947a 393 return Polshoek3;
aschut 2:7c9974f0947a 394 }
aschut 2:7c9974f0947a 395
aschut 2:7c9974f0947a 396
aschut 2:7c9974f0947a 397 float Limits2(float Ellebooghoek2)
aschut 2:7c9974f0947a 398 {
aschut 0:90750f158475 399
aschut 2:7c9974f0947a 400 if (Ellebooghoek2 <= upperlim2 && Ellebooghoek2 >= lowerlim2) { //Binnen de limieten
aschut 2:7c9974f0947a 401 Ellebooghoek3 = Ellebooghoek2;
aschut 2:7c9974f0947a 402 }
aschut 2:7c9974f0947a 403
aschut 2:7c9974f0947a 404 else {
aschut 2:7c9974f0947a 405 if (Ellebooghoek2 >= upperlim2) { //Boven de limiet
aschut 2:7c9974f0947a 406 Ellebooghoek3 = upperlim2;
aschut 2:7c9974f0947a 407 } else { //Onder de limiet
aschut 2:7c9974f0947a 408 Ellebooghoek3 = lowerlim2;
aschut 2:7c9974f0947a 409 }
aschut 2:7c9974f0947a 410 }
aschut 2:7c9974f0947a 411
aschut 2:7c9974f0947a 412 return Ellebooghoek3;
aschut 2:7c9974f0947a 413 }
aschut 2:7c9974f0947a 414
aschut 2:7c9974f0947a 415
aschut 2:7c9974f0947a 416 // PID Controller
aschut 2:7c9974f0947a 417 double PID_controller1(double error1)
aschut 2:7c9974f0947a 418 {
aschut 2:7c9974f0947a 419 static double error1_integral = 0;
aschut 2:7c9974f0947a 420 static double error1_prev = error1; // initialization with this value only done once!
aschut 2:7c9974f0947a 421 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
aschut 6:464d2fdfd7de 422
aschut 2:7c9974f0947a 423 // Proportional part:
aschut 2:7c9974f0947a 424 double u_k1 = Kp1 * error1;
aschut 2:7c9974f0947a 425
aschut 2:7c9974f0947a 426 // Integral part
aschut 2:7c9974f0947a 427 error1_integral = error1_integral + error1 * Ts;
aschut 2:7c9974f0947a 428 double u_i1 = Ki1* error1_integral;
aschut 2:7c9974f0947a 429
aschut 2:7c9974f0947a 430 // Derivative part
aschut 2:7c9974f0947a 431 double error1_derivative = (error1 - error1_prev)/Ts;
aschut 2:7c9974f0947a 432 double filtered_error1_derivative = LowPassFilter.step(error1_derivative);
aschut 2:7c9974f0947a 433 double u_d1 = Kd1 * filtered_error1_derivative;
aschut 2:7c9974f0947a 434 error1_prev = error1;
aschut 2:7c9974f0947a 435
aschut 2:7c9974f0947a 436 // Sum all parts and return it
aschut 2:7c9974f0947a 437 return u_k1 + u_i1 + u_d1;
aschut 2:7c9974f0947a 438 }
aschut 2:7c9974f0947a 439 double PID_controller2(double error2)
aschut 2:7c9974f0947a 440 {
aschut 2:7c9974f0947a 441 static double error2_integral = 0;
aschut 2:7c9974f0947a 442 static double error2_prev = error2; // initialization with this value only done once!
aschut 2:7c9974f0947a 443 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //(BIQUAD_FILTER_TYPE type, T dbGain, T freq, T srate, T bandwidth);
aschut 6:464d2fdfd7de 444
aschut 2:7c9974f0947a 445 // Proportional part:
aschut 2:7c9974f0947a 446 double u_k2 = Kp2 * error2;
aschut 2:7c9974f0947a 447
aschut 2:7c9974f0947a 448 // Integral part
aschut 2:7c9974f0947a 449 error2_integral = error2_integral + error2 * Ts;
aschut 2:7c9974f0947a 450 double u_i2 = Ki2 * error2_integral;
aschut 2:7c9974f0947a 451
aschut 2:7c9974f0947a 452 // Derivative part
aschut 2:7c9974f0947a 453 double error2_derivative = (error2 - error2_prev)/Ts;
aschut 2:7c9974f0947a 454 double filtered_error2_derivative = LowPassFilter.step(error2_derivative);
aschut 2:7c9974f0947a 455 double u_d2 = Kd2 * filtered_error2_derivative;
aschut 2:7c9974f0947a 456 error2_prev = error2;
aschut 2:7c9974f0947a 457
aschut 2:7c9974f0947a 458 // Sum all parts and return it
aschut 2:7c9974f0947a 459 return u_k2 + u_i2 + u_d2;
aschut 2:7c9974f0947a 460 }
aschut 2:7c9974f0947a 461
aschut 2:7c9974f0947a 462 // Functies Motor
aschut 2:7c9974f0947a 463 void moter1_control(double u1)
aschut 2:7c9974f0947a 464 {
aschut 2:7c9974f0947a 465 direction1= u1 > 0.0f; //positief = CW
aschut 6:464d2fdfd7de 466 if (fabs(u1)> 0.7f) {
aschut 6:464d2fdfd7de 467 u1 = 0.7f;
aschut 2:7c9974f0947a 468 } else {
aschut 2:7c9974f0947a 469 u1= u1;
aschut 2:7c9974f0947a 470 }
aschut 3:2aaf54ce090b 471 pwmpin1.write(fabs(u1)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
aschut 2:7c9974f0947a 472 }
aschut 2:7c9974f0947a 473
aschut 2:7c9974f0947a 474 void moter2_control(double u2)
aschut 2:7c9974f0947a 475 {
aschut 2:7c9974f0947a 476 direction2= u2 < 0.0f; //positief = CW
aschut 2:7c9974f0947a 477 if (fabs(u2)> 0.99f) {
aschut 2:7c9974f0947a 478 u2 = 0.99f;
aschut 2:7c9974f0947a 479 } else {
aschut 2:7c9974f0947a 480 u2= u2;
aschut 2:7c9974f0947a 481 }
aschut 3:2aaf54ce090b 482 pwmpin2.write(fabs(u2)) ; //pwmduty cycle canonlybepositive, floatingpoint absolute value
aschut 2:7c9974f0947a 483 }
aschut 2:7c9974f0947a 484
aschut 2:7c9974f0947a 485 void PwmMotor(void)
aschut 2:7c9974f0947a 486 {
aschut 5:ef77da99d0d1 487 //Continues Read
aschut 5:ef77da99d0d1 488 Pot2 = pot.read();
aschut 5:ef77da99d0d1 489 Pot1 = pot0.read();
aschut 5:ef77da99d0d1 490 pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
aschut 5:ef77da99d0d1 491 pwm1 =(Pot1*2)-1;
aschut 11:08ba9cfc9c8f 492 Input1 = Bicep;
aschut 12:20ae1d20148d 493 Input2 = Palmair;
aschut 6:464d2fdfd7de 494
aschut 3:2aaf54ce090b 495 float Polshoek1 = Kinematics1(Input1);
aschut 3:2aaf54ce090b 496 float Polshoek4 = Limits1(Polshoek1);
aschut 3:2aaf54ce090b 497 PolsReference = Polshoek4;
aschut 6:464d2fdfd7de 498
aschut 2:7c9974f0947a 499 // Reference hoek berekenen, in graden
aschut 3:2aaf54ce090b 500 float Ellebooghoek1 = Kinematics2(Input2);
aschut 2:7c9974f0947a 501 float Ellebooghoek4 = Limits2(Ellebooghoek1);
aschut 2:7c9974f0947a 502 ElbowReference = Ellebooghoek4;
aschut 6:464d2fdfd7de 503
aschut 2:7c9974f0947a 504 // Positie motor berekenen, in graden
aschut 2:7c9974f0947a 505 Pulses1 = encoder1.getPulses();
aschut 2:7c9974f0947a 506 motor_position1 = -(Pulses1/1200)*360;
aschut 2:7c9974f0947a 507 Pulses2 = encoder2.getPulses();
aschut 2:7c9974f0947a 508 motor_position2 = -(Pulses2/4800)*360;
aschut 2:7c9974f0947a 509
aschut 2:7c9974f0947a 510 double error1 = PolsReference - motor_position1;
aschut 2:7c9974f0947a 511 double u1 = PID_controller1(error1);
aschut 2:7c9974f0947a 512 moter1_control(u1);
aschut 6:464d2fdfd7de 513
aschut 2:7c9974f0947a 514 double error2 = ElbowReference - motor_position2;
aschut 2:7c9974f0947a 515 double u2 = PID_controller2(error2);
aschut 2:7c9974f0947a 516 moter2_control(u2);
aschut 2:7c9974f0947a 517
aschut 2:7c9974f0947a 518 }
aschut 2:7c9974f0947a 519
aschut 2:7c9974f0947a 520 void MotorOn(void)
aschut 2:7c9974f0947a 521 {
aschut 2:7c9974f0947a 522 pwmpin1 = 0;
aschut 2:7c9974f0947a 523 pwmpin2 = 0;
aschut 2:7c9974f0947a 524 Pwm.attach (PwmMotor, Ts);
aschut 2:7c9974f0947a 525
aschut 2:7c9974f0947a 526 }
aschut 2:7c9974f0947a 527
aschut 6:464d2fdfd7de 528 //Servo functies
aschut 6:464d2fdfd7de 529 void servowait1(void)
aschut 6:464d2fdfd7de 530 {
aschut 6:464d2fdfd7de 531 double Pulslength1 = 0.0005 + (servo_position1 * 0.002); //in seconden, waarde tussen de 500 en de 2500 microseconden, als de waarde omhoog gaat beweegt de servo tegen de klok in
aschut 6:464d2fdfd7de 532 myservo1 = true;
aschut 6:464d2fdfd7de 533 wait(Pulslength1);
aschut 6:464d2fdfd7de 534 myservo1 = false;
aschut 6:464d2fdfd7de 535 }
aschut 6:464d2fdfd7de 536
aschut 6:464d2fdfd7de 537 void servowait2(void)
aschut 6:464d2fdfd7de 538 {
aschut 6:464d2fdfd7de 539 double Pulslength2 = 0.0005 + (servo_position2 * 0.002); //in seconden
aschut 6:464d2fdfd7de 540 myservo2 = true;
aschut 6:464d2fdfd7de 541 wait(Pulslength2);
aschut 6:464d2fdfd7de 542 myservo2 = false;
aschut 6:464d2fdfd7de 543 }
aschut 6:464d2fdfd7de 544
aschut 6:464d2fdfd7de 545 void servowait3(void)
aschut 6:464d2fdfd7de 546 {
aschut 6:464d2fdfd7de 547 double Pulslength3 = 0.0005 + (servo_position3 * 0.002); //in seconden
aschut 6:464d2fdfd7de 548 myservo3 = true;
aschut 6:464d2fdfd7de 549 wait(Pulslength3);
aschut 6:464d2fdfd7de 550 myservo3 = false;
aschut 6:464d2fdfd7de 551 }
aschut 6:464d2fdfd7de 552
aschut 4:babe09a69296 553 void ServoPeriod()
aschut 4:babe09a69296 554 {
aschut 6:464d2fdfd7de 555 servowait1();
aschut 6:464d2fdfd7de 556 servowait2();
aschut 6:464d2fdfd7de 557 servowait3();
aschut 4:babe09a69296 558 }
aschut 2:7c9974f0947a 559
aschut 6:464d2fdfd7de 560
aschut 6:464d2fdfd7de 561
aschut 2:7c9974f0947a 562 void ContinuousReader(void)
aschut 2:7c9974f0947a 563 {
aschut 2:7c9974f0947a 564 Pot2 = pot.read();
aschut 2:7c9974f0947a 565 Pot1 = pot0.read();
aschut 2:7c9974f0947a 566 pwm2 =(Pot2*2)-1; //scaling naar -1 tot 1
aschut 2:7c9974f0947a 567 pwm1 =(Pot1*2)-1;
aschut 2:7c9974f0947a 568 }
aschut 2:7c9974f0947a 569
aschut 11:08ba9cfc9c8f 570 void sample(){
aschut 11:08ba9cfc9c8f 571 pc.printf("Duim = %i\r\n", Duim);
aschut 11:08ba9cfc9c8f 572 pc.printf("Bicep = %i\r\n",Bicep);
aschut 11:08ba9cfc9c8f 573 pc.printf("Dorsaal = %i\r\n", Dorsaal);
aschut 11:08ba9cfc9c8f 574 pc.printf("Palmair = %i\r\n", Palmair);
aschut 9:ad4bd1b16eed 575
aschut 9:ad4bd1b16eed 576 }
aschut 9:ad4bd1b16eed 577
aschut 2:7c9974f0947a 578 // StateMachine
aschut 2:7c9974f0947a 579
aschut 6:464d2fdfd7de 580 enum states {MOTORS_OFF,CALIBRATION,HOMING1,HOMING2,DEMO,MOVEMENT,FREEZE};
aschut 0:90750f158475 581 int f = 1;
aschut 6:464d2fdfd7de 582 states currentState = MOTORS_OFF;
aschut 0:90750f158475 583 bool stateChanged = true; // Make sure the initialization of first state is executed
aschut 0:90750f158475 584
aschut 0:90750f158475 585 void ProcessStateMachine(void)
aschut 0:90750f158475 586 {
aschut 6:464d2fdfd7de 587 switch (currentState) {
aschut 6:464d2fdfd7de 588 case MOTORS_OFF:
aschut 6:464d2fdfd7de 589 // Actions
aschut 6:464d2fdfd7de 590 if (stateChanged) {
aschut 6:464d2fdfd7de 591 // state initialization: rood
aschut 6:464d2fdfd7de 592
aschut 6:464d2fdfd7de 593 led1 = 1;
aschut 6:464d2fdfd7de 594 led2 = 0;
aschut 6:464d2fdfd7de 595 led3 = 1;
aschut 6:464d2fdfd7de 596 wait (1);
aschut 6:464d2fdfd7de 597 stateChanged = false;
aschut 6:464d2fdfd7de 598 }
aschut 2:7c9974f0947a 599
aschut 6:464d2fdfd7de 600 // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
aschut 6:464d2fdfd7de 601 if (!button3) {
aschut 6:464d2fdfd7de 602 currentState = CALIBRATION ;
aschut 6:464d2fdfd7de 603 stateChanged = true;
aschut 6:464d2fdfd7de 604 } else {
aschut 6:464d2fdfd7de 605 currentState = MOTORS_OFF;
aschut 6:464d2fdfd7de 606 stateChanged = true;
aschut 6:464d2fdfd7de 607 }
aschut 6:464d2fdfd7de 608
aschut 6:464d2fdfd7de 609 break;
aschut 6:464d2fdfd7de 610
aschut 6:464d2fdfd7de 611 case CALIBRATION:
aschut 6:464d2fdfd7de 612 // Actions
aschut 6:464d2fdfd7de 613 if (stateChanged) {
aschut 6:464d2fdfd7de 614 // state initialization: oranje
aschut 4:babe09a69296 615 led1 = 0;
aschut 4:babe09a69296 616 led2 = 0;
aschut 4:babe09a69296 617 led3 = 1;
aschut 9:ad4bd1b16eed 618 sample_ticker.attach(&emgsample, 0.001); // Leest het ruwe EMG signaal af met een frequentie van 1000Hz
aschut 9:ad4bd1b16eed 619 timer_calibration.reset();
aschut 9:ad4bd1b16eed 620 timer_calibration.start();
aschut 9:ad4bd1b16eed 621 while(timer_calibration<20) { //Duim
aschut 9:ad4bd1b16eed 622 if(timer_calibration>0 && timer_calibration<20) {
aschut 10:193942c3900a 623
aschut 9:ad4bd1b16eed 624 if(emg1_filtered>temp_highest_emg1) {
aschut 9:ad4bd1b16eed 625 temp_highest_emg1= emg1_filtered;
aschut 9:ad4bd1b16eed 626 pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1);
aschut 9:ad4bd1b16eed 627 }
aschut 6:464d2fdfd7de 628
aschut 9:ad4bd1b16eed 629 if(emg2_filtered>temp_highest_emg2) {
aschut 9:ad4bd1b16eed 630 temp_highest_emg2= emg2_filtered;
aschut 9:ad4bd1b16eed 631 pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2);
aschut 9:ad4bd1b16eed 632 }
aschut 9:ad4bd1b16eed 633
aschut 9:ad4bd1b16eed 634 if(emg3_filtered>temp_highest_emg3) {
aschut 9:ad4bd1b16eed 635 temp_highest_emg3= emg3_filtered;
aschut 9:ad4bd1b16eed 636 pc.printf("Highest value Dorsaal= %f \r\n", temp_highest_emg3);
aschut 9:ad4bd1b16eed 637 }
aschut 9:ad4bd1b16eed 638
aschut 9:ad4bd1b16eed 639 if(emg4_filtered>temp_highest_emg4) {
aschut 9:ad4bd1b16eed 640 temp_highest_emg4= emg4_filtered;
aschut 9:ad4bd1b16eed 641 pc.printf("Highest value Palmair= %f \r\n", temp_highest_emg4);
aschut 9:ad4bd1b16eed 642 }
aschut 9:ad4bd1b16eed 643
aschut 9:ad4bd1b16eed 644 }
aschut 9:ad4bd1b16eed 645
aschut 9:ad4bd1b16eed 646 }
aschut 9:ad4bd1b16eed 647 pc.printf("threshold calculation\r\n");
aschut 9:ad4bd1b16eed 648 threshold1 = temp_highest_emg1*Duim_p_t;
aschut 9:ad4bd1b16eed 649 threshold2 = temp_highest_emg2*Bicep_p_t;
aschut 9:ad4bd1b16eed 650 threshold3 = temp_highest_emg3*Dorsaal_p_t;
aschut 9:ad4bd1b16eed 651 threshold4 = temp_highest_emg4*Palmair_p_t;
aschut 9:ad4bd1b16eed 652
aschut 9:ad4bd1b16eed 653 threshold1L = temp_highest_emg1*Duim_p_tL;
aschut 9:ad4bd1b16eed 654 threshold2L = temp_highest_emg2*Bicep_p_tL;
aschut 9:ad4bd1b16eed 655 threshold3L = temp_highest_emg3*Dorsaal_p_tL;
aschut 9:ad4bd1b16eed 656 threshold4L = temp_highest_emg4*Palmair_p_tL;
aschut 4:babe09a69296 657 stateChanged = false;
aschut 6:464d2fdfd7de 658
aschut 6:464d2fdfd7de 659 }
aschut 9:ad4bd1b16eed 660 sample_ticker.detach();
aschut 12:20ae1d20148d 661 timer_calibration.stop();
aschut 6:464d2fdfd7de 662 // State transition logic: automatisch terug naar motors off.
aschut 6:464d2fdfd7de 663
aschut 6:464d2fdfd7de 664 currentState = HOMING1;
aschut 6:464d2fdfd7de 665 stateChanged = true;
aschut 6:464d2fdfd7de 666 break;
aschut 6:464d2fdfd7de 667
aschut 6:464d2fdfd7de 668 case HOMING1:
aschut 6:464d2fdfd7de 669 // Actions
aschut 6:464d2fdfd7de 670 if (stateChanged) {
aschut 6:464d2fdfd7de 671 // state initialization: green
aschut 6:464d2fdfd7de 672 t.start();
aschut 6:464d2fdfd7de 673 led1 = 0;
aschut 6:464d2fdfd7de 674 led2 = 1;
aschut 6:464d2fdfd7de 675 led3 = 1;
aschut 0:90750f158475 676
aschut 6:464d2fdfd7de 677 if (!But1) {
aschut 6:464d2fdfd7de 678 led1 = 1;
aschut 6:464d2fdfd7de 679 float H1 = 0.99f;
aschut 6:464d2fdfd7de 680 moter1_control(H1);
aschut 6:464d2fdfd7de 681 wait(0.001f);
aschut 6:464d2fdfd7de 682 } else if (!But2) {
aschut 6:464d2fdfd7de 683 led1 = 1;
aschut 6:464d2fdfd7de 684 float H1 = -0.99f;
aschut 6:464d2fdfd7de 685 moter1_control(H1);
aschut 6:464d2fdfd7de 686 wait(0.001f);
aschut 6:464d2fdfd7de 687 }
aschut 6:464d2fdfd7de 688 encoder1.reset();
aschut 6:464d2fdfd7de 689 motor_position1 = 0;
aschut 6:464d2fdfd7de 690 pwmpin1 = 0;
aschut 6:464d2fdfd7de 691 pwmpin2 = 0;
aschut 6:464d2fdfd7de 692 ;
aschut 6:464d2fdfd7de 693
aschut 6:464d2fdfd7de 694 stateChanged = false;
aschut 6:464d2fdfd7de 695 }
aschut 6:464d2fdfd7de 696
aschut 6:464d2fdfd7de 697 // State transition logic: naar HOMING (button2), na 5 min naar MOTORS_OFF
aschut 6:464d2fdfd7de 698
aschut 6:464d2fdfd7de 699 if (!button3) {
aschut 12:20ae1d20148d 700 t.stop();
aschut 12:20ae1d20148d 701 t.reset();
aschut 6:464d2fdfd7de 702 currentState = HOMING2 ;
aschut 6:464d2fdfd7de 703 stateChanged = true;
aschut 6:464d2fdfd7de 704 wait(1);
aschut 6:464d2fdfd7de 705 } else if (t>300) {
aschut 6:464d2fdfd7de 706 t.stop();
aschut 6:464d2fdfd7de 707 t.reset();
aschut 6:464d2fdfd7de 708 currentState = MOTORS_OFF ;
aschut 6:464d2fdfd7de 709 stateChanged = true;
aschut 6:464d2fdfd7de 710 } else {
aschut 6:464d2fdfd7de 711 currentState = HOMING1 ;
aschut 6:464d2fdfd7de 712 stateChanged = true;
aschut 6:464d2fdfd7de 713 }
aschut 6:464d2fdfd7de 714 break;
aschut 2:7c9974f0947a 715
aschut 6:464d2fdfd7de 716 case HOMING2:
aschut 6:464d2fdfd7de 717 // Actions
aschut 12:20ae1d20148d 718
aschut 6:464d2fdfd7de 719 if (stateChanged) {
aschut 6:464d2fdfd7de 720 // state initialization: white
aschut 6:464d2fdfd7de 721 t.start();
aschut 6:464d2fdfd7de 722 led1 = 0;
aschut 6:464d2fdfd7de 723 led2 = 0;
aschut 6:464d2fdfd7de 724 led3 = 0;
aschut 6:464d2fdfd7de 725
aschut 6:464d2fdfd7de 726 if (!But1) {
aschut 6:464d2fdfd7de 727 led1 = 1;
aschut 6:464d2fdfd7de 728 float H2 = 0.98f;
aschut 6:464d2fdfd7de 729 moter2_control(H2);
aschut 6:464d2fdfd7de 730 wait(0.001f);
aschut 6:464d2fdfd7de 731 } else if (!But2) {
aschut 6:464d2fdfd7de 732 led1 = 1;
aschut 6:464d2fdfd7de 733 float H2 = -0.98f;
aschut 6:464d2fdfd7de 734 moter2_control(H2);
aschut 6:464d2fdfd7de 735 wait(0.001f);
aschut 6:464d2fdfd7de 736 }
aschut 6:464d2fdfd7de 737 encoder2.reset();
aschut 6:464d2fdfd7de 738 motor_position2 = 0;
aschut 6:464d2fdfd7de 739 pwmpin1 = 0;
aschut 6:464d2fdfd7de 740 pwmpin2 = 0;
aschut 6:464d2fdfd7de 741 ;
aschut 6:464d2fdfd7de 742
aschut 6:464d2fdfd7de 743 stateChanged = false;
aschut 6:464d2fdfd7de 744 }
aschut 6:464d2fdfd7de 745
aschut 6:464d2fdfd7de 746 // State transition logic: naar DEMO (button2), naar MOVEMENT(button3)
aschut 6:464d2fdfd7de 747 if (!button2) {
aschut 6:464d2fdfd7de 748 currentState = DEMO;
aschut 6:464d2fdfd7de 749 stateChanged = true;
aschut 12:20ae1d20148d 750 t.stop();
aschut 12:20ae1d20148d 751 t.reset();
aschut 6:464d2fdfd7de 752 } else if (!button3) {
aschut 6:464d2fdfd7de 753 currentState = MOVEMENT ;
aschut 6:464d2fdfd7de 754 stateChanged = true;
aschut 12:20ae1d20148d 755 t.stop();
aschut 12:20ae1d20148d 756 t.reset();
aschut 6:464d2fdfd7de 757 } else if (t>300) {
aschut 6:464d2fdfd7de 758 t.stop();
aschut 6:464d2fdfd7de 759 t.reset();
aschut 6:464d2fdfd7de 760 currentState = MOTORS_OFF ;
aschut 6:464d2fdfd7de 761 stateChanged = true;
aschut 6:464d2fdfd7de 762 } else {
aschut 6:464d2fdfd7de 763 currentState = HOMING2 ;
aschut 6:464d2fdfd7de 764 stateChanged = true;
aschut 6:464d2fdfd7de 765 }
aschut 6:464d2fdfd7de 766 break;
aschut 6:464d2fdfd7de 767
aschut 6:464d2fdfd7de 768 case DEMO:
aschut 6:464d2fdfd7de 769 // Actions
aschut 6:464d2fdfd7de 770 if (stateChanged) {
aschut 6:464d2fdfd7de 771 // state initialization: light blue
aschut 6:464d2fdfd7de 772 led1 = 0;
aschut 6:464d2fdfd7de 773 led2 = 1;
aschut 6:464d2fdfd7de 774 led3 = 0;
aschut 6:464d2fdfd7de 775
aschut 6:464d2fdfd7de 776 servo_position1 = 0.5;
aschut 6:464d2fdfd7de 777 servo_position2 = 0.5;
aschut 6:464d2fdfd7de 778 servo_position3 = 0.5;
aschut 6:464d2fdfd7de 779 ServoTick.attach(&ServoPeriod, Periodlength);
aschut 2:7c9974f0947a 780
aschut 6:464d2fdfd7de 781 wait(1.5 );
aschut 6:464d2fdfd7de 782 servo_position1 = 0.9;
aschut 6:464d2fdfd7de 783 servo_position2 = 0.9;
aschut 6:464d2fdfd7de 784 servo_position3 = 0.9;
aschut 6:464d2fdfd7de 785 wait(1.5);
aschut 6:464d2fdfd7de 786 servo_position1 = 0.1;
aschut 6:464d2fdfd7de 787 servo_position2 = 0.1;
aschut 6:464d2fdfd7de 788 servo_position3 = 0.1;
aschut 6:464d2fdfd7de 789 wait(1);
aschut 6:464d2fdfd7de 790
aschut 6:464d2fdfd7de 791 ServoTick.detach();
aschut 6:464d2fdfd7de 792
aschut 6:464d2fdfd7de 793
aschut 6:464d2fdfd7de 794 wait (1);
aschut 6:464d2fdfd7de 795
aschut 6:464d2fdfd7de 796 stateChanged = false;
aschut 6:464d2fdfd7de 797 }
aschut 6:464d2fdfd7de 798
aschut 6:464d2fdfd7de 799 // State transition logic: automatisch terug naar HOMING
aschut 6:464d2fdfd7de 800 currentState = HOMING2;
aschut 6:464d2fdfd7de 801 stateChanged = true;
aschut 6:464d2fdfd7de 802 break;
aschut 6:464d2fdfd7de 803
aschut 6:464d2fdfd7de 804 case MOVEMENT:
aschut 6:464d2fdfd7de 805 // Actions
aschut 6:464d2fdfd7de 806
aschut 6:464d2fdfd7de 807 if (stateChanged) {
aschut 6:464d2fdfd7de 808 // state initialization: purple
aschut 6:464d2fdfd7de 809 t.start(); // na 5 minuten terug naar Homing
aschut 6:464d2fdfd7de 810 led1 = 1;
aschut 6:464d2fdfd7de 811 led2 = 0;
aschut 6:464d2fdfd7de 812 led3 = 0;
aschut 6:464d2fdfd7de 813
aschut 6:464d2fdfd7de 814 // Tickers aan
aschut 6:464d2fdfd7de 815 if (counts == 0) {
aschut 6:464d2fdfd7de 816 pwmpin1 = 0;
aschut 6:464d2fdfd7de 817 pwmpin2 = 0;
aschut 6:464d2fdfd7de 818 Input1 = pwm1;
aschut 6:464d2fdfd7de 819 Input2 = pwm2;
aschut 6:464d2fdfd7de 820 Pwm.attach (PwmMotor, Ts);
aschut 10:193942c3900a 821 sample_ticker.attach(&emgsample, 0.001); // Leest het ruwe EMG signaal af met een frequentie van 1000Hz
aschut 11:08ba9cfc9c8f 822 threshold_check_ticker.attach(&threshold_check, 0.01);
aschut 11:08ba9cfc9c8f 823 sample_timer.attach(&sample, 0.5);
aschut 6:464d2fdfd7de 824 servo_position1 = Duim_krom;
aschut 6:464d2fdfd7de 825 servo_position2 = MWP_krom;
aschut 6:464d2fdfd7de 826 servo_position3 = Wijsvinger_krom;
aschut 6:464d2fdfd7de 827
aschut 6:464d2fdfd7de 828 ServoTick.attach(&ServoPeriod, Periodlength);
aschut 6:464d2fdfd7de 829 counts++;
aschut 6:464d2fdfd7de 830 wait(1);
aschut 6:464d2fdfd7de 831 }
aschut 6:464d2fdfd7de 832
aschut 6:464d2fdfd7de 833 // Servo positie
aschut 6:464d2fdfd7de 834
aschut 12:20ae1d20148d 835 if (Dorsaal == 1 && Duim == 0) {
aschut 6:464d2fdfd7de 836 servo_position1 = Duim_krom;
aschut 6:464d2fdfd7de 837 servo_position2 = MWP_krom;
aschut 6:464d2fdfd7de 838 servo_position3 = Wijsvinger_krom;
aschut 6:464d2fdfd7de 839 led1 = !led1;
aschut 6:464d2fdfd7de 840 }
aschut 12:20ae1d20148d 841 if (Dorsaal == 1 && Duim == 1) {
aschut 6:464d2fdfd7de 842 servo_position1 = Duim_recht;
aschut 6:464d2fdfd7de 843 servo_position2 = MWP_recht;
aschut 6:464d2fdfd7de 844 servo_position3 = Wijsvinger_recht;
aschut 6:464d2fdfd7de 845 led1 = !led1;
aschut 6:464d2fdfd7de 846 }
aschut 3:2aaf54ce090b 847
aschut 6:464d2fdfd7de 848 /*
aschut 6:464d2fdfd7de 849
aschut 6:464d2fdfd7de 850 // printen
aschut 6:464d2fdfd7de 851 if(count==500)
aschut 6:464d2fdfd7de 852 {
aschut 6:464d2fdfd7de 853 float tmp = t2.read();
aschut 6:464d2fdfd7de 854 pc.printf(" Elleboog positie: %f reference: %f, rotatie positie: %f reference: \r\n", motor_position2, ElbowReference, motor_position1, PolsReference);
aschut 6:464d2fdfd7de 855 count = 0;
aschut 6:464d2fdfd7de 856 }
aschut 6:464d2fdfd7de 857 count++;
aschut 6:464d2fdfd7de 858
aschut 6:464d2fdfd7de 859
aschut 6:464d2fdfd7de 860 */
aschut 3:2aaf54ce090b 861
aschut 6:464d2fdfd7de 862 stateChanged = false;
aschut 6:464d2fdfd7de 863 }
aschut 6:464d2fdfd7de 864
aschut 6:464d2fdfd7de 865 // State transition logic: naar FREEZE (button2), naar MOTORS_OFF(button3) anders naar MOVEMENT
aschut 6:464d2fdfd7de 866 if (!button2) {
aschut 6:464d2fdfd7de 867 currentState = FREEZE;
aschut 6:464d2fdfd7de 868 stateChanged = true;
aschut 12:20ae1d20148d 869 threshold_check_ticker.detach();
aschut 12:20ae1d20148d 870 t.stop();
aschut 12:20ae1d20148d 871 t.reset();
aschut 6:464d2fdfd7de 872 } else if (!button3) {
aschut 6:464d2fdfd7de 873 Pwm.detach ();
aschut 6:464d2fdfd7de 874 ServoTick.detach();
aschut 10:193942c3900a 875 sample_ticker.detach();
aschut 11:08ba9cfc9c8f 876 threshold_check_ticker.detach();
aschut 11:08ba9cfc9c8f 877 sample_timer.detach();
aschut 12:20ae1d20148d 878 t.stop();
aschut 12:20ae1d20148d 879 t.reset();
aschut 6:464d2fdfd7de 880 pwmpin2 = 0;
aschut 6:464d2fdfd7de 881 pwmpin1 = 0;
aschut 6:464d2fdfd7de 882 counts = 0;
aschut 6:464d2fdfd7de 883 currentState = MOTORS_OFF ;
aschut 6:464d2fdfd7de 884 stateChanged = true;
aschut 6:464d2fdfd7de 885 } else if (t>300) {
aschut 6:464d2fdfd7de 886 t.stop();
aschut 6:464d2fdfd7de 887 t.reset();
aschut 6:464d2fdfd7de 888 Pwm.detach ();
aschut 6:464d2fdfd7de 889 ServoTick.detach();
aschut 10:193942c3900a 890 sample_ticker.detach();
aschut 11:08ba9cfc9c8f 891 sample_timer.detach();
aschut 11:08ba9cfc9c8f 892 threshold_check_ticker.detach();
aschut 6:464d2fdfd7de 893 counts = 0;
aschut 6:464d2fdfd7de 894 currentState = HOMING1 ;
aschut 6:464d2fdfd7de 895 stateChanged = true;
aschut 6:464d2fdfd7de 896 } else {
aschut 6:464d2fdfd7de 897 currentState = MOVEMENT ;
aschut 6:464d2fdfd7de 898 stateChanged = true;
aschut 6:464d2fdfd7de 899 }
aschut 6:464d2fdfd7de 900 break;
aschut 6:464d2fdfd7de 901
aschut 2:7c9974f0947a 902 case FREEZE:
aschut 6:464d2fdfd7de 903 // Actions
aschut 6:464d2fdfd7de 904 if (stateChanged) {
aschut 6:464d2fdfd7de 905 // state initialization: blue
aschut 12:20ae1d20148d 906 Duim = 0;
aschut 12:20ae1d20148d 907 Bicep = 0;
aschut 12:20ae1d20148d 908 Dorsaal = 0;
aschut 12:20ae1d20148d 909 Palmair = 0;
aschut 6:464d2fdfd7de 910 led1 = 1;
aschut 6:464d2fdfd7de 911 led2 = 1;
aschut 6:464d2fdfd7de 912 led3 = 0;
aschut 12:20ae1d20148d 913 wait(1);
aschut 6:464d2fdfd7de 914 stateChanged = false;
aschut 6:464d2fdfd7de 915 }
aschut 6:464d2fdfd7de 916
aschut 6:464d2fdfd7de 917 // State transition logic: automatisch terug naar MOVEMENT.
aschut 12:20ae1d20148d 918 if (!button2) {
aschut 12:20ae1d20148d 919 currentState = MOVEMENT;
aschut 12:20ae1d20148d 920 stateChanged = true;
aschut 12:20ae1d20148d 921 counts = 0;
aschut 12:20ae1d20148d 922 }
aschut 12:20ae1d20148d 923 else {
aschut 12:20ae1d20148d 924 currentState = FREEZE ;
aschut 12:20ae1d20148d 925 stateChanged = false;
aschut 12:20ae1d20148d 926 }
aschut 12:20ae1d20148d 927
aschut 6:464d2fdfd7de 928 break;
aschut 6:464d2fdfd7de 929
aschut 6:464d2fdfd7de 930 }
aschut 0:90750f158475 931 }
aschut 6:464d2fdfd7de 932
aschut 0:90750f158475 933 int main()
aschut 0:90750f158475 934 {
aschut 6:464d2fdfd7de 935
aschut 2:7c9974f0947a 936 t2.start();
aschut 2:7c9974f0947a 937 int counter = 0;
aschut 6:464d2fdfd7de 938 pwmpin1.period_us(60);
aschut 4:babe09a69296 939 //PotRead.attach(ContinuousReader,Ts);
aschut 2:7c9974f0947a 940 pc.baud(115200);
aschut 2:7c9974f0947a 941
aschut 10:193942c3900a 942 //BiQuad Chain add
aschut 10:193942c3900a 943 highp1.add( &highp1_1 ).add( &highp1_2 ).add( &notch1_1 );
aschut 10:193942c3900a 944 highp2.add( &highp2_1 ).add( &highp2_2 ).add( &notch2_1 );
aschut 10:193942c3900a 945 highp3.add( &highp3_1 ).add( &highp3_2 ).add( &notch3_1 );
aschut 10:193942c3900a 946 highp4.add( &highp4_1 ).add( &highp4_2 ).add( &notch4_1 );
aschut 10:193942c3900a 947
aschut 6:464d2fdfd7de 948 while(true) {
aschut 6:464d2fdfd7de 949 led1 = 1;
aschut 6:464d2fdfd7de 950 led2 =1;
aschut 6:464d2fdfd7de 951 led3 =1;
aschut 6:464d2fdfd7de 952 /*
aschut 6:464d2fdfd7de 953 if(counter==10)
aschut 6:464d2fdfd7de 954 {
aschut 6:464d2fdfd7de 955 float tmp = t2.read();
aschut 6:464d2fdfd7de 956 printf("%f,%f,%f,%f\n\r",tmp,motor_position2,ElbowReference,Pulses2);
aschut 6:464d2fdfd7de 957 counter = 0;
aschut 6:464d2fdfd7de 958 }
aschut 6:464d2fdfd7de 959 counter++;
aschut 6:464d2fdfd7de 960 */
aschut 6:464d2fdfd7de 961 ProcessStateMachine();
aschut 6:464d2fdfd7de 962
aschut 6:464d2fdfd7de 963
aschut 6:464d2fdfd7de 964 wait(0.001);
aschut 0:90750f158475 965 }
aschut 6:464d2fdfd7de 966 }
aschut 0:90750f158475 967
aschut 0:90750f158475 968
aschut 0:90750f158475 969
aschut 0:90750f158475 970
aschut 0:90750f158475 971
aschut 0:90750f158475 972
aschut 2:7c9974f0947a 973
aschut 6:464d2fdfd7de 974
aschut 6:464d2fdfd7de 975
aschut 6:464d2fdfd7de 976
aschut 6:464d2fdfd7de 977
aschut 6:464d2fdfd7de 978
aschut 6:464d2fdfd7de 979