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@7:d7aafc5b9efc, 2019-04-16 (annotated)

Committer:
aschut
Date:
Tue Apr 16 13:33:37 2019 +0000
Revision:
7:d7aafc5b9efc
Parent:
6:464d2fdfd7de 
met werkende kalibratie, rest nog niet

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