Janine Klein Rot / Mbed 2 deprecated PIPO-2

2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range

Dependencies:   Encoder MODSERIAL TextLCD mbed mbed-dsp

main.cpp@14:e1816efa712d, 2014-10-27 (annotated)

Committer:
JKleinRot
Date:
Mon Oct 27 14:43:50 2014 +0000
Revision:
14:e1816efa712d
Parent:
13:54ee98850a15
Child:
15:3ebd0e666a9c
2014-10-27. Offset weggehaald, hoogdoorlaatfilter doet dat. Timer voor 5 seconden kalibratie meten.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JKleinRot 0:859c89785d3f 1 #include "mbed.h" //Mbed bibliotheek inladen, standaard functies
JKleinRot 0:859c89785d3f 2 #include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC
JKleinRot 0:859c89785d3f 3 #include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder
JKleinRot 0:859c89785d3f 4 #include "TextLCD.h" //LCD scherm bibliotheek inladen, communicatie met LCD scherm
JKleinRot 0:859c89785d3f 5
JKleinRot 0:859c89785d3f 6 //Constanten definiëren en waarde geven
JKleinRot 11:e9b906b5f572 7 #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
JKleinRot 13:54ee98850a15 8 #define KP_arm1 0.3 //Factor proprotionele regelaar arm 1
JKleinRot 13:54ee98850a15 9 #define KI_arm1 0 //Factor integraal regelaar arm 1
JKleinRot 13:54ee98850a15 10 #define KD_arm1 0.02 //Factor afgeleide regelaar arm 1
JKleinRot 13:54ee98850a15 11 #define KP_arm2 0.001 //Factor proprotionele regelaar arm 2
JKleinRot 13:54ee98850a15 12 #define KI_arm2 0 //Factor integraal regelaar arm 2
JKleinRot 13:54ee98850a15 13 #define KD_arm2 0 //Factor afgeleide regelaar arm 2
JKleinRot 11:e9b906b5f572 14 #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
JKleinRot 11:e9b906b5f572 15 #define PULS_ARM1_HOME_KALIBRATIE 180 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
JKleinRot 11:e9b906b5f572 16 #define PULS_ARM2_HOME_KALIBRATIE 393 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
JKleinRot 0:859c89785d3f 17
JKleinRot 13:54ee98850a15 18 //High Pass filter Filtercoëfficiënten
JKleinRot 13:54ee98850a15 19 #define A1 1
JKleinRot 13:54ee98850a15 20 #define A2 -3.1806
JKleinRot 13:54ee98850a15 21 #define A3 3.8612
JKleinRot 13:54ee98850a15 22 #define A4 -2.1122
JKleinRot 13:54ee98850a15 23 #define A5 0.4383
JKleinRot 13:54ee98850a15 24 #define B1 4.1660E-4
JKleinRot 13:54ee98850a15 25 #define B2 0.0017
JKleinRot 13:54ee98850a15 26 #define B3 0.0025
JKleinRot 13:54ee98850a15 27 #define B4 0.0017
JKleinRot 13:54ee98850a15 28 #define B5 4.1660E-4
JKleinRot 13:54ee98850a15 29
JKleinRot 13:54ee98850a15 30 //Notch filter Filtercoëfficiënten
JKleinRot 13:54ee98850a15 31 #define C1 1
JKleinRot 13:54ee98850a15 32 #define C2 -1.1873E-16
JKleinRot 13:54ee98850a15 33 #define C3 0.9391
JKleinRot 13:54ee98850a15 34 #define D1 0.9695
JKleinRot 13:54ee98850a15 35 #define D2 -1.1873E-16
JKleinRot 13:54ee98850a15 36 #define D3 0.9695
JKleinRot 13:54ee98850a15 37
JKleinRot 13:54ee98850a15 38 //Low pass filter Filtercoëfficiënten
JKleinRot 13:54ee98850a15 39 #define E1 1
JKleinRot 13:54ee98850a15 40 #define E2 3.9179
JKleinRot 13:54ee98850a15 41 #define E3 5.7571
JKleinRot 13:54ee98850a15 42 #define E4 3.7603
JKleinRot 13:54ee98850a15 43 #define E5 0.9212
JKleinRot 13:54ee98850a15 44 #define F1 0.9598
JKleinRot 13:54ee98850a15 45 #define F2 3.8391
JKleinRot 13:54ee98850a15 46 #define F3 5.7587
JKleinRot 13:54ee98850a15 47 #define F4 3.8391
JKleinRot 13:54ee98850a15 48 #define F5 0.9598
JKleinRot 13:54ee98850a15 49
JKleinRot 0:859c89785d3f 50 //Pinverdeling en naamgeving variabelen
JKleinRot 0:859c89785d3f 51 TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm
JKleinRot 0:859c89785d3f 52 MODSERIAL pc(USBTX, USBRX); //PC
JKleinRot 0:859c89785d3f 53
JKleinRot 1:e5e1eb9d0025 54 PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
JKleinRot 9:454e7da8ab65 55 DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1
JKleinRot 12:996f9f8e3acc 56 Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit)
JKleinRot 11:e9b906b5f572 57 PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2
JKleinRot 9:454e7da8ab65 58 DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2
JKleinRot 12:996f9f8e3acc 59 Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit)
JKleinRot 6:3b6fad529520 60 AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps
JKleinRot 12:996f9f8e3acc 61 //Blauw op 3,3 V en groen op GND
JKleinRot 1:e5e1eb9d0025 62
JKleinRot 6:3b6fad529520 63 Ticker ticker_regelaar; //Ticker voor regelaar motor
JKleinRot 6:3b6fad529520 64 Ticker ticker_EMG; //Ticker voor EMG meten
JKleinRot 14:e1816efa712d 65 Timer biceps_kalibratie;
JKleinRot 1:e5e1eb9d0025 66
JKleinRot 9:454e7da8ab65 67 //States definiëren
JKleinRot 14:e1816efa712d 68 enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
JKleinRot 9:454e7da8ab65 69 uint8_t state = RUST; //State is rust aan het begin
JKleinRot 9:454e7da8ab65 70
JKleinRot 6:3b6fad529520 71 //Gedefinieerde datatypen en naamgeving en beginwaarden
JKleinRot 6:3b6fad529520 72 char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm
JKleinRot 6:3b6fad529520 73 char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm
JKleinRot 0:859c89785d3f 74
JKleinRot 11:e9b906b5f572 75 float pwm_to_motor_arm1; //PWM output naar motor arm 1
JKleinRot 11:e9b906b5f572 76 float pwm_to_motor_arm2; //PWM output naar motor arm 2
JKleinRot 13:54ee98850a15 77 float error_arm1_kalibratie; //Error in pulsen arm 1
JKleinRot 13:54ee98850a15 78 float vorige_error_arm1 = 0; //Derivative actie van regelaar arm 1
JKleinRot 13:54ee98850a15 79 float integraal_arm1 = 0; //Integraal actie van regelaar arm 1
JKleinRot 13:54ee98850a15 80 float afgeleide_arm1; //Afgeleide actie van regelaar arm 1
JKleinRot 13:54ee98850a15 81 float error_arm2_kalibratie; //Error in pulsen arm 2
JKleinRot 13:54ee98850a15 82 float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2
JKleinRot 13:54ee98850a15 83 float integraal_arm2 = 0; //Integraal actie van regelaar arm 2
JKleinRot 13:54ee98850a15 84 float afgeleide_arm2; //Afgeleide actie van regelaar arm 2
JKleinRot 13:54ee98850a15 85 float xbk = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 86 float xbk1 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 87 float xbk2 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 88 float xbk3 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 89 float xbk4 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 90 float xbk5 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 91 float xbk6 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 92 float xbk7 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 93 float xbk8 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 94 float xbk9 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 95 float xbk10 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 96 float meanxbk; //Offset van biceps kalibratie
JKleinRot 13:54ee98850a15 97 float xb; //Gemeten EMG waarde biceps
JKleinRot 13:54ee98850a15 98 float xbo; //Gemeten EMG waarde biceps min de offset, deze gaat in de filters
JKleinRot 13:54ee98850a15 99
JKleinRot 13:54ee98850a15 100 //High Pass filter Y(n-x) waarden instellen op nul en definiëren als float
JKleinRot 13:54ee98850a15 101 float xbhp; //Zelfde als xbo, maar makkelijker in notatie in filter
JKleinRot 13:54ee98850a15 102 float xbhp1 = 0;
JKleinRot 13:54ee98850a15 103 float xbhp2 = 0;
JKleinRot 13:54ee98850a15 104 float xbhp3 = 0;
JKleinRot 13:54ee98850a15 105 float xbhp4 = 0;
JKleinRot 13:54ee98850a15 106 float xbhp5 = 0;
JKleinRot 13:54ee98850a15 107 float ybhp;
JKleinRot 13:54ee98850a15 108 float ybhp1 = 0;
JKleinRot 13:54ee98850a15 109 float ybhp2 = 0;
JKleinRot 13:54ee98850a15 110 float ybhp3 = 0;
JKleinRot 13:54ee98850a15 111 float ybhp4 = 0;
JKleinRot 13:54ee98850a15 112 float ybhp5 = 0;
JKleinRot 13:54ee98850a15 113
JKleinRot 13:54ee98850a15 114 //Notch filter
JKleinRot 13:54ee98850a15 115 float xbn;
JKleinRot 13:54ee98850a15 116 float xbn1 = 0;
JKleinRot 13:54ee98850a15 117 float xbn2 = 0;
JKleinRot 13:54ee98850a15 118 float ybn;
JKleinRot 13:54ee98850a15 119 float ybn1 = 0;
JKleinRot 13:54ee98850a15 120 float ybn2 = 0;
JKleinRot 13:54ee98850a15 121
JKleinRot 13:54ee98850a15 122 //Rectifier
JKleinRot 13:54ee98850a15 123 float rb;
JKleinRot 13:54ee98850a15 124 float rbr;
JKleinRot 13:54ee98850a15 125
JKleinRot 13:54ee98850a15 126 //Low pass filter
JKleinRot 13:54ee98850a15 127 float xblp;
JKleinRot 13:54ee98850a15 128 float xblp1 = 0;
JKleinRot 13:54ee98850a15 129 float xblp2 = 0;
JKleinRot 13:54ee98850a15 130 float xblp3 = 0;
JKleinRot 13:54ee98850a15 131 float xblp4 = 0;
JKleinRot 13:54ee98850a15 132 float xblp5 = 0;
JKleinRot 13:54ee98850a15 133 float yblp;
JKleinRot 13:54ee98850a15 134 float yblp1 = 0;
JKleinRot 13:54ee98850a15 135 float yblp2 = 0;
JKleinRot 13:54ee98850a15 136 float yblp3 = 0;
JKleinRot 13:54ee98850a15 137 float yblp4 = 0;
JKleinRot 13:54ee98850a15 138 float yblp5 = 0;
JKleinRot 13:54ee98850a15 139
JKleinRot 13:54ee98850a15 140 //Gefilterde biceps EMG
JKleinRot 13:54ee98850a15 141 float xbf;
JKleinRot 13:54ee98850a15 142 float ybf;
JKleinRot 7:dd3cba61b34b 143
JKleinRot 6:3b6fad529520 144 volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 145 void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
JKleinRot 10:52b22bff409a 146 {
JKleinRot 10:52b22bff409a 147 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 148 }
JKleinRot 1:e5e1eb9d0025 149
JKleinRot 6:3b6fad529520 150 volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 151 void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
JKleinRot 10:52b22bff409a 152 {
JKleinRot 10:52b22bff409a 153 regelaar_EMG_flag = true;
JKleinRot 4:69540b9c0646 154 }
JKleinRot 4:69540b9c0646 155
JKleinRot 10:52b22bff409a 156 void keep_in_range(float * in, float min, float max) //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt
JKleinRot 10:52b22bff409a 157 {
JKleinRot 10:52b22bff409a 158 if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum
JKleinRot 2:0cb899f2800a 159 *in = min;
JKleinRot 2:0cb899f2800a 160 }
JKleinRot 10:52b22bff409a 161 if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum
JKleinRot 2:0cb899f2800a 162 *in = max;
JKleinRot 10:52b22bff409a 163 } else { //In alle andere gevallen is de waarde de waarde zelf
JKleinRot 2:0cb899f2800a 164 *in = *in;
JKleinRot 2:0cb899f2800a 165 }
JKleinRot 2:0cb899f2800a 166 }
JKleinRot 1:e5e1eb9d0025 167
JKleinRot 10:52b22bff409a 168 void arm1_naar_thuispositie()
JKleinRot 10:52b22bff409a 169 {
JKleinRot 13:54ee98850a15 170 error_arm1_kalibratie = (PULS_ARM1_HOME_KALIBRATIE - puls_motor_arm1.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
JKleinRot 13:54ee98850a15 171 integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
JKleinRot 13:54ee98850a15 172 afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
JKleinRot 13:54ee98850a15 173 pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1; //Output naar motor na PID regelaar
JKleinRot 7:dd3cba61b34b 174 keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 10:52b22bff409a 175
JKleinRot 11:e9b906b5f572 176 if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 10:52b22bff409a 177 dir_motor_arm1.write(1);
JKleinRot 11:e9b906b5f572 178 } else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 179 dir_motor_arm1.write(0);
JKleinRot 7:dd3cba61b34b 180 }
JKleinRot 11:e9b906b5f572 181 pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 10:52b22bff409a 182 pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition());
JKleinRot 9:454e7da8ab65 183 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1);
JKleinRot 10:52b22bff409a 184
JKleinRot 11:e9b906b5f572 185 if (pwm_to_motor_arm1 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
JKleinRot 11:e9b906b5f572 186 state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 13:54ee98850a15 187 pc.printf("KALIBRATIE_ARM1 afgerond\n"); //Tekst voor controle Arm 1 naar thuispositie
JKleinRot 10:52b22bff409a 188 }
JKleinRot 10:52b22bff409a 189 }
JKleinRot 7:dd3cba61b34b 190
JKleinRot 10:52b22bff409a 191 void arm2_naar_thuispositie()
JKleinRot 10:52b22bff409a 192 {
JKleinRot 13:54ee98850a15 193 error_arm2_kalibratie = (PULS_ARM2_HOME_KALIBRATIE - puls_motor_arm2.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
JKleinRot 13:54ee98850a15 194 integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
JKleinRot 13:54ee98850a15 195 afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
JKleinRot 13:54ee98850a15 196 pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2; //Output naar motor na PID regelaar
JKleinRot 7:dd3cba61b34b 197 keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 10:52b22bff409a 198
JKleinRot 11:e9b906b5f572 199 if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 7:dd3cba61b34b 200 dir_motor_arm2.write(1);
JKleinRot 11:e9b906b5f572 201 } else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 202 dir_motor_arm2.write(0);
JKleinRot 7:dd3cba61b34b 203 }
JKleinRot 11:e9b906b5f572 204 pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 13:54ee98850a15 205 pc.printf("pulsmotorgetposition %d ", puls_motor_arm2.getPosition());
JKleinRot 13:54ee98850a15 206 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2);
JKleinRot 10:52b22bff409a 207
JKleinRot 11:e9b906b5f572 208 if (pwm_to_motor_arm2 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
JKleinRot 14:e1816efa712d 209 state = EMG_KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 14:e1816efa712d 210 pc.printf("KALIBRATIE_ARM2 afgerond\n\r"); //Tekst voor controle Arm 2 naar thuispositie
JKleinRot 7:dd3cba61b34b 211 }
JKleinRot 7:dd3cba61b34b 212 }
JKleinRot 0:859c89785d3f 213
JKleinRot 13:54ee98850a15 214 void filter_biceps()
JKleinRot 13:54ee98850a15 215 {
JKleinRot 13:54ee98850a15 216 //High pass
JKleinRot 13:54ee98850a15 217 xbhp = xbo; //Input in filter
JKleinRot 13:54ee98850a15 218 ybhp = -A2*ybhp1 - A3*ybhp2 - A4*ybhp3 - A5*ybhp4 + B1*xbhp + B2*xbhp1 + B3*xbhp2 + B4*xbhp3 + B5*xbhp4; //Filterformule in z-domein
JKleinRot 13:54ee98850a15 219
JKleinRot 13:54ee98850a15 220 //Waarden voor de volgende ronde benoemen
JKleinRot 13:54ee98850a15 221 xbhp4 = xbhp3;
JKleinRot 13:54ee98850a15 222 xbhp3 = xbhp2;
JKleinRot 13:54ee98850a15 223 xbhp2 = xbhp1;
JKleinRot 13:54ee98850a15 224 xbhp1 = xbhp;
JKleinRot 13:54ee98850a15 225
JKleinRot 13:54ee98850a15 226 ybhp4 = ybhp3;
JKleinRot 13:54ee98850a15 227 ybhp3 = ybhp2;
JKleinRot 13:54ee98850a15 228 ybhp2 = ybhp1;
JKleinRot 13:54ee98850a15 229 ybhp1 = ybhp;
JKleinRot 14:e1816efa712d 230 xbn = ybhp; //De input in het notchfilter is de waarde uit het high pass filter
JKleinRot 13:54ee98850a15 231
JKleinRot 13:54ee98850a15 232 //Notch
JKleinRot 13:54ee98850a15 233 ybn = -C2*ybn1 - C3*ybn2 + D1*xbn + D2*xbn1 + D3*xbn2; //Filterfunctie in z-domein
JKleinRot 13:54ee98850a15 234
JKleinRot 13:54ee98850a15 235 //Waarden voor de volgende ronde benoemen
JKleinRot 13:54ee98850a15 236 xbn2 = xbn1;
JKleinRot 13:54ee98850a15 237 xbn1 = xbn;
JKleinRot 13:54ee98850a15 238
JKleinRot 13:54ee98850a15 239 ybn2 = ybn1;
JKleinRot 13:54ee98850a15 240 ybn1 = ybn;
JKleinRot 14:e1816efa712d 241 rb = ybn; //De input in de rectify is de waarde uit het notch filter
JKleinRot 13:54ee98850a15 242
JKleinRot 13:54ee98850a15 243 //Rectify
JKleinRot 13:54ee98850a15 244 rbr = fabs(rb); //Absolute waarde van de waarde uit notchfilter
JKleinRot 13:54ee98850a15 245
JKleinRot 14:e1816efa712d 246 xblp = rbr; //De input in het low pass filter is de waarde uit de rectify
JKleinRot 13:54ee98850a15 247
JKleinRot 13:54ee98850a15 248 //Low pass
JKleinRot 13:54ee98850a15 249 yblp = -E2*yblp1 - E3*yblp2 - E4*yblp3 - E5*yblp4 + F1*xblp + F2*xblp1 + F3*xblp2 + F4*xblp3 + F5*xblp4; //Filterfunctie in z-domein
JKleinRot 13:54ee98850a15 250
JKleinRot 13:54ee98850a15 251 xblp4 = xblp3;
JKleinRot 13:54ee98850a15 252 xblp3 = xblp2;
JKleinRot 13:54ee98850a15 253 xblp2 = xblp1;
JKleinRot 13:54ee98850a15 254 xblp1 = xblp;
JKleinRot 13:54ee98850a15 255
JKleinRot 13:54ee98850a15 256 yblp4 = yblp3;
JKleinRot 13:54ee98850a15 257 yblp3 = yblp2;
JKleinRot 13:54ee98850a15 258 yblp2 = yblp1;
JKleinRot 13:54ee98850a15 259 yblp1 = yblp;
JKleinRot 14:e1816efa712d 260 xbf = yblp; //De uiteindelijk gefilterde EMG waarde is de output uit het low pass filter
JKleinRot 13:54ee98850a15 261 }
JKleinRot 13:54ee98850a15 262
JKleinRot 0:859c89785d3f 263
JKleinRot 10:52b22bff409a 264 int main()
JKleinRot 10:52b22bff409a 265 {
JKleinRot 11:e9b906b5f572 266 while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
JKleinRot 10:52b22bff409a 267 //PC baud rate instellen
JKleinRot 10:52b22bff409a 268 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 10:52b22bff409a 269
JKleinRot 10:52b22bff409a 270 switch(state) { //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case
JKleinRot 10:52b22bff409a 271
JKleinRot 11:e9b906b5f572 272 case RUST: { //Aanzetten
JKleinRot 10:52b22bff409a 273 lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
JKleinRot 10:52b22bff409a 274 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
JKleinRot 10:52b22bff409a 275 wait(2); //Twee seconden wachten
JKleinRot 14:e1816efa712d 276 pc.printf("RUST afgerond\n\r"); //Tekst voor controle Aanzetten
JKleinRot 10:52b22bff409a 277 state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
JKleinRot 10:52b22bff409a 278 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 279 }
JKleinRot 10:52b22bff409a 280
JKleinRot 10:52b22bff409a 281 case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie
JKleinRot 14:e1816efa712d 282 pc.printf("KALIBRATIE_ARM1\n\r");
JKleinRot 10:52b22bff409a 283 wait(1); //Een seconde wachten
JKleinRot 14:e1816efa712d 284
JKleinRot 10:52b22bff409a 285 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 286
JKleinRot 10:52b22bff409a 287 while(state == KALIBRATIE_ARM1) {
JKleinRot 10:52b22bff409a 288 while(regelaar_ticker_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 10:52b22bff409a 289 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 290
JKleinRot 10:52b22bff409a 291 arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 292 }
JKleinRot 10:52b22bff409a 293 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 294 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 295 }
JKleinRot 10:52b22bff409a 296
JKleinRot 11:e9b906b5f572 297 case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
JKleinRot 14:e1816efa712d 298 pc.printf("KALIBRATIE_ARM1\n\r");
JKleinRot 10:52b22bff409a 299 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 300
JKleinRot 10:52b22bff409a 301 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 302
JKleinRot 10:52b22bff409a 303 while(state == KALIBRATIE_ARM2) {
JKleinRot 10:52b22bff409a 304 while(regelaar_ticker_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 10:52b22bff409a 305 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 306
JKleinRot 10:52b22bff409a 307 arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 308 }
JKleinRot 11:e9b906b5f572 309 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 310 ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
JKleinRot 10:52b22bff409a 311 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 312 }
JKleinRot 14:e1816efa712d 313
JKleinRot 14:e1816efa712d 314 case EMG_KALIBRATIE_BICEPS:
JKleinRot 14:e1816efa712d 315 {
JKleinRot 14:e1816efa712d 316 pc.printf("EMG__KALIBRATIE_BICEPS\n\r");
JKleinRot 14:e1816efa712d 317
JKleinRot 14:e1816efa712d 318 lcd_r1 = " SPAN IN 5 SEC. ";
JKleinRot 14:e1816efa712d 319 lcd_r2 = " 2 X BICEPS AAN ";
JKleinRot 14:e1816efa712d 320
JKleinRot 14:e1816efa712d 321 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);
JKleinRot 14:e1816efa712d 322 biceps_kalibratie.start();
JKleinRot 14:e1816efa712d 323
JKleinRot 14:e1816efa712d 324 if(biceps_kalibratie.read() <= 5){
JKleinRot 10:52b22bff409a 325 while(regelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 14:e1816efa712d 326 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 13:54ee98850a15 327
JKleinRot 14:e1816efa712d 328 xb = EMG_bi.read(); //EMG meten van biceps
JKleinRot 14:e1816efa712d 329
JKleinRot 14:e1816efa712d 330 filter_biceps();
JKleinRot 13:54ee98850a15 331
JKleinRot 14:e1816efa712d 332 if(biceps_kalibratie.read() == 1){
JKleinRot 14:e1816efa712d 333 lcd_r1 = " 4 ";
JKleinRot 14:e1816efa712d 334 }
JKleinRot 14:e1816efa712d 335 if(biceps_kalibratie.read() == 2){
JKleinRot 14:e1816efa712d 336 lcd_r1 = " 3 ";
JKleinRot 14:e1816efa712d 337 }
JKleinRot 14:e1816efa712d 338 if(biceps_kalibratie.read() == 3){
JKleinRot 14:e1816efa712d 339 lcd_r1 = " 2 ";
JKleinRot 14:e1816efa712d 340 }
JKleinRot 14:e1816efa712d 341 if(biceps_kalibratie.read() == 4){
JKleinRot 14:e1816efa712d 342 lcd_r1 = " 1 ";
JKleinRot 14:e1816efa712d 343 }
JKleinRot 14:e1816efa712d 344
JKleinRot 10:52b22bff409a 345 }
JKleinRot 13:54ee98850a15 346
JKleinRot 14:e1816efa712d 347 state = EMG_KALIBRATIE_TRICEPS;
JKleinRot 10:52b22bff409a 348 }
JKleinRot 13:54ee98850a15 349
JKleinRot 11:e9b906b5f572 350 default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
JKleinRot 11:e9b906b5f572 351 state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
JKleinRot 10:52b22bff409a 352 }
JKleinRot 9:454e7da8ab65 353 }
JKleinRot 14:e1816efa712d 354 pc.printf("state: %u\n\r",state);
JKleinRot 3:adc3052039e7 355 }
JKleinRot 0:859c89785d3f 356 }