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@13:54ee98850a15, 2014-10-24 (annotated)

Committer:
JKleinRot
Date:
Fri Oct 24 11:06:15 2014 +0000
Revision:
13:54ee98850a15
Parent:
12:996f9f8e3acc
Child:
14:e1816efa712d
2014-10-24 Filterformules gemaakt, offset metingen gedaan. Offset bepalen nog niet gelukt

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 1:e5e1eb9d0025 65
JKleinRot 9:454e7da8ab65 66 //States definiëren
JKleinRot 13:54ee98850a15 67 enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_OFFSET_BICEPS, EMG_BICEPS, KALIBRATIE_BICEPS, EMG_OFFSET_TRICEPS, EMG_TRICEPS, KALIBRATIE_TRICEPS}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
JKleinRot 9:454e7da8ab65 68 uint8_t state = RUST; //State is rust aan het begin
JKleinRot 9:454e7da8ab65 69
JKleinRot 6:3b6fad529520 70 //Gedefinieerde datatypen en naamgeving en beginwaarden
JKleinRot 6:3b6fad529520 71 char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm
JKleinRot 6:3b6fad529520 72 char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm
JKleinRot 0:859c89785d3f 73
JKleinRot 11:e9b906b5f572 74 float pwm_to_motor_arm1; //PWM output naar motor arm 1
JKleinRot 11:e9b906b5f572 75 float pwm_to_motor_arm2; //PWM output naar motor arm 2
JKleinRot 13:54ee98850a15 76 float error_arm1_kalibratie; //Error in pulsen arm 1
JKleinRot 13:54ee98850a15 77 float vorige_error_arm1 = 0; //Derivative actie van regelaar arm 1
JKleinRot 13:54ee98850a15 78 float integraal_arm1 = 0; //Integraal actie van regelaar arm 1
JKleinRot 13:54ee98850a15 79 float afgeleide_arm1; //Afgeleide actie van regelaar arm 1
JKleinRot 13:54ee98850a15 80 float error_arm2_kalibratie; //Error in pulsen arm 2
JKleinRot 13:54ee98850a15 81 float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2
JKleinRot 13:54ee98850a15 82 float integraal_arm2 = 0; //Integraal actie van regelaar arm 2
JKleinRot 13:54ee98850a15 83 float afgeleide_arm2; //Afgeleide actie van regelaar arm 2
JKleinRot 13:54ee98850a15 84 float xbk = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 85 float xbk1 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 86 float xbk2 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 87 float xbk3 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 88 float xbk4 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 89 float xbk5 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 90 float xbk6 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 91 float xbk7 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 92 float xbk8 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 93 float xbk9 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 94 float xbk10 = 0; //Gemeten EMG waarde biceps in de kalibratie
JKleinRot 13:54ee98850a15 95 float meanxbk; //Offset van biceps kalibratie
JKleinRot 13:54ee98850a15 96 float xb; //Gemeten EMG waarde biceps
JKleinRot 13:54ee98850a15 97 float xbo; //Gemeten EMG waarde biceps min de offset, deze gaat in de filters
JKleinRot 13:54ee98850a15 98
JKleinRot 13:54ee98850a15 99 //High Pass filter Y(n-x) waarden instellen op nul en definiëren als float
JKleinRot 13:54ee98850a15 100 float xbhp; //Zelfde als xbo, maar makkelijker in notatie in filter
JKleinRot 13:54ee98850a15 101 float xbhp1 = 0;
JKleinRot 13:54ee98850a15 102 float xbhp2 = 0;
JKleinRot 13:54ee98850a15 103 float xbhp3 = 0;
JKleinRot 13:54ee98850a15 104 float xbhp4 = 0;
JKleinRot 13:54ee98850a15 105 float xbhp5 = 0;
JKleinRot 13:54ee98850a15 106 float ybhp;
JKleinRot 13:54ee98850a15 107 float ybhp1 = 0;
JKleinRot 13:54ee98850a15 108 float ybhp2 = 0;
JKleinRot 13:54ee98850a15 109 float ybhp3 = 0;
JKleinRot 13:54ee98850a15 110 float ybhp4 = 0;
JKleinRot 13:54ee98850a15 111 float ybhp5 = 0;
JKleinRot 13:54ee98850a15 112
JKleinRot 13:54ee98850a15 113 //Notch filter
JKleinRot 13:54ee98850a15 114 float xbn;
JKleinRot 13:54ee98850a15 115 float xbn1 = 0;
JKleinRot 13:54ee98850a15 116 float xbn2 = 0;
JKleinRot 13:54ee98850a15 117 float ybn;
JKleinRot 13:54ee98850a15 118 float ybn1 = 0;
JKleinRot 13:54ee98850a15 119 float ybn2 = 0;
JKleinRot 13:54ee98850a15 120
JKleinRot 13:54ee98850a15 121 //Rectifier
JKleinRot 13:54ee98850a15 122 float rb;
JKleinRot 13:54ee98850a15 123 float rbr;
JKleinRot 13:54ee98850a15 124
JKleinRot 13:54ee98850a15 125 //Low pass filter
JKleinRot 13:54ee98850a15 126 float xblp;
JKleinRot 13:54ee98850a15 127 float xblp1 = 0;
JKleinRot 13:54ee98850a15 128 float xblp2 = 0;
JKleinRot 13:54ee98850a15 129 float xblp3 = 0;
JKleinRot 13:54ee98850a15 130 float xblp4 = 0;
JKleinRot 13:54ee98850a15 131 float xblp5 = 0;
JKleinRot 13:54ee98850a15 132 float yblp;
JKleinRot 13:54ee98850a15 133 float yblp1 = 0;
JKleinRot 13:54ee98850a15 134 float yblp2 = 0;
JKleinRot 13:54ee98850a15 135 float yblp3 = 0;
JKleinRot 13:54ee98850a15 136 float yblp4 = 0;
JKleinRot 13:54ee98850a15 137 float yblp5 = 0;
JKleinRot 13:54ee98850a15 138
JKleinRot 13:54ee98850a15 139 //Gefilterde biceps EMG
JKleinRot 13:54ee98850a15 140 float xbf;
JKleinRot 13:54ee98850a15 141 float ybf;
JKleinRot 7:dd3cba61b34b 142
JKleinRot 6:3b6fad529520 143 volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 144 void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
JKleinRot 10:52b22bff409a 145 {
JKleinRot 10:52b22bff409a 146 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 147 }
JKleinRot 1:e5e1eb9d0025 148
JKleinRot 6:3b6fad529520 149 volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 150 void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
JKleinRot 10:52b22bff409a 151 {
JKleinRot 10:52b22bff409a 152 regelaar_EMG_flag = true;
JKleinRot 4:69540b9c0646 153 }
JKleinRot 4:69540b9c0646 154
JKleinRot 10:52b22bff409a 155 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 156 {
JKleinRot 10:52b22bff409a 157 if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum
JKleinRot 2:0cb899f2800a 158 *in = min;
JKleinRot 2:0cb899f2800a 159 }
JKleinRot 10:52b22bff409a 160 if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum
JKleinRot 2:0cb899f2800a 161 *in = max;
JKleinRot 10:52b22bff409a 162 } else { //In alle andere gevallen is de waarde de waarde zelf
JKleinRot 2:0cb899f2800a 163 *in = *in;
JKleinRot 2:0cb899f2800a 164 }
JKleinRot 2:0cb899f2800a 165 }
JKleinRot 1:e5e1eb9d0025 166
JKleinRot 10:52b22bff409a 167 void arm1_naar_thuispositie()
JKleinRot 10:52b22bff409a 168 {
JKleinRot 13:54ee98850a15 169 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 170 integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
JKleinRot 13:54ee98850a15 171 afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
JKleinRot 13:54ee98850a15 172 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 173 keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 10:52b22bff409a 174
JKleinRot 11:e9b906b5f572 175 if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 10:52b22bff409a 176 dir_motor_arm1.write(1);
JKleinRot 11:e9b906b5f572 177 } else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 178 dir_motor_arm1.write(0);
JKleinRot 7:dd3cba61b34b 179 }
JKleinRot 11:e9b906b5f572 180 pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 10:52b22bff409a 181 pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition());
JKleinRot 9:454e7da8ab65 182 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1);
JKleinRot 10:52b22bff409a 183
JKleinRot 11:e9b906b5f572 184 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 185 state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 13:54ee98850a15 186 pc.printf("KALIBRATIE_ARM1 afgerond\n"); //Tekst voor controle Arm 1 naar thuispositie
JKleinRot 10:52b22bff409a 187 }
JKleinRot 10:52b22bff409a 188 }
JKleinRot 7:dd3cba61b34b 189
JKleinRot 10:52b22bff409a 190 void arm2_naar_thuispositie()
JKleinRot 10:52b22bff409a 191 {
JKleinRot 13:54ee98850a15 192 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 193 integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
JKleinRot 13:54ee98850a15 194 afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
JKleinRot 13:54ee98850a15 195 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 196 keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 10:52b22bff409a 197
JKleinRot 11:e9b906b5f572 198 if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 7:dd3cba61b34b 199 dir_motor_arm2.write(1);
JKleinRot 11:e9b906b5f572 200 } else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 201 dir_motor_arm2.write(0);
JKleinRot 7:dd3cba61b34b 202 }
JKleinRot 11:e9b906b5f572 203 pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 13:54ee98850a15 204 pc.printf("pulsmotorgetposition %d ", puls_motor_arm2.getPosition());
JKleinRot 13:54ee98850a15 205 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2);
JKleinRot 10:52b22bff409a 206
JKleinRot 11:e9b906b5f572 207 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 13:54ee98850a15 208 state = EMG_OFFSET_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 13:54ee98850a15 209 pc.printf("KALIBRATIE_ARM2 afgerond\n"); //Tekst voor controle Arm 2 naar thuispositie
JKleinRot 7:dd3cba61b34b 210 }
JKleinRot 7:dd3cba61b34b 211 }
JKleinRot 0:859c89785d3f 212
JKleinRot 13:54ee98850a15 213 void filter_biceps()
JKleinRot 13:54ee98850a15 214 {
JKleinRot 13:54ee98850a15 215 //High pass
JKleinRot 13:54ee98850a15 216 xbhp = xbo; //Input in filter
JKleinRot 13:54ee98850a15 217 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 218
JKleinRot 13:54ee98850a15 219 //Waarden voor de volgende ronde benoemen
JKleinRot 13:54ee98850a15 220 xbhp4 = xbhp3;
JKleinRot 13:54ee98850a15 221 xbhp3 = xbhp2;
JKleinRot 13:54ee98850a15 222 xbhp2 = xbhp1;
JKleinRot 13:54ee98850a15 223 xbhp1 = xbhp;
JKleinRot 13:54ee98850a15 224 xbn = 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 13:54ee98850a15 230 ybn1 = ybhp; //De vorige waarde 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 xblp = xbn;
JKleinRot 13:54ee98850a15 239
JKleinRot 13:54ee98850a15 240 ybn2 = ybn1;
JKleinRot 13:54ee98850a15 241 ybn1 = ybn;
JKleinRot 13:54ee98850a15 242 rb = ybn;
JKleinRot 13:54ee98850a15 243
JKleinRot 13:54ee98850a15 244 //Rectify
JKleinRot 13:54ee98850a15 245 rbr = fabs(rb); //Absolute waarde van de waarde uit notchfilter
JKleinRot 13:54ee98850a15 246
JKleinRot 13:54ee98850a15 247 yblp1 = rbr;
JKleinRot 13:54ee98850a15 248
JKleinRot 13:54ee98850a15 249 //Low pass
JKleinRot 13:54ee98850a15 250 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 251
JKleinRot 13:54ee98850a15 252 xblp4 = xblp3;
JKleinRot 13:54ee98850a15 253 xblp3 = xblp2;
JKleinRot 13:54ee98850a15 254 xblp2 = xblp1;
JKleinRot 13:54ee98850a15 255 xblp1 = xblp;
JKleinRot 13:54ee98850a15 256 xbf = xblp;
JKleinRot 13:54ee98850a15 257
JKleinRot 13:54ee98850a15 258 yblp4 = yblp3;
JKleinRot 13:54ee98850a15 259 yblp3 = yblp2;
JKleinRot 13:54ee98850a15 260 yblp2 = yblp1;
JKleinRot 13:54ee98850a15 261 yblp1 = yblp;
JKleinRot 13:54ee98850a15 262 ybf = yblp;
JKleinRot 13:54ee98850a15 263 }
JKleinRot 13:54ee98850a15 264
JKleinRot 0:859c89785d3f 265
JKleinRot 10:52b22bff409a 266 int main()
JKleinRot 10:52b22bff409a 267 {
JKleinRot 11:e9b906b5f572 268 while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
JKleinRot 10:52b22bff409a 269 //PC baud rate instellen
JKleinRot 10:52b22bff409a 270 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 10:52b22bff409a 271
JKleinRot 10:52b22bff409a 272 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 273
JKleinRot 11:e9b906b5f572 274 case RUST: { //Aanzetten
JKleinRot 10:52b22bff409a 275 lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
JKleinRot 10:52b22bff409a 276 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
JKleinRot 10:52b22bff409a 277 wait(2); //Twee seconden wachten
JKleinRot 13:54ee98850a15 278 pc.printf("RUST afgerond\n"); //Tekst voor controle Aanzetten
JKleinRot 10:52b22bff409a 279 state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
JKleinRot 10:52b22bff409a 280 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 281 }
JKleinRot 10:52b22bff409a 282
JKleinRot 10:52b22bff409a 283 case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie
JKleinRot 13:54ee98850a15 284 pc.printf("KALIBRATIE_ARM1\n");
JKleinRot 10:52b22bff409a 285 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 286 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 287
JKleinRot 10:52b22bff409a 288 while(state == KALIBRATIE_ARM1) {
JKleinRot 10:52b22bff409a 289 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 290 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 291
JKleinRot 10:52b22bff409a 292 arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 293 }
JKleinRot 10:52b22bff409a 294 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 295 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 296 }
JKleinRot 10:52b22bff409a 297
JKleinRot 11:e9b906b5f572 298 case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
JKleinRot 13:54ee98850a15 299 pc.printf("KALIBRATIE_ARM1\n");
JKleinRot 10:52b22bff409a 300 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 301
JKleinRot 10:52b22bff409a 302 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 303
JKleinRot 10:52b22bff409a 304 while(state == KALIBRATIE_ARM2) {
JKleinRot 10:52b22bff409a 305 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 306 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 307
JKleinRot 10:52b22bff409a 308 arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 309 }
JKleinRot 11:e9b906b5f572 310 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 311 ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
JKleinRot 10:52b22bff409a 312 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 313 }
JKleinRot 10:52b22bff409a 314
JKleinRot 13:54ee98850a15 315 case EMG_OFFSET_BICEPS: { //Kalibratie EMG signaal biceps
JKleinRot 13:54ee98850a15 316 pc.printf("EMG_OFFSET_BICEPS\n");
JKleinRot 10:52b22bff409a 317 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 318
JKleinRot 10:52b22bff409a 319 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken
JKleinRot 10:52b22bff409a 320
JKleinRot 13:54ee98850a15 321 pc.printf("Ticker voor kalibratie compleet\n"); //Tekst voor controle Ticker voor kalibratie
JKleinRot 10:52b22bff409a 322
JKleinRot 10:52b22bff409a 323 //5 seconden EMG biceps meten
JKleinRot 10:52b22bff409a 324
JKleinRot 10:52b22bff409a 325 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 326 lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm
JKleinRot 10:52b22bff409a 327 lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm
JKleinRot 10:52b22bff409a 328
JKleinRot 13:54ee98850a15 329 while(state == EMG_OFFSET_BICEPS){
JKleinRot 10:52b22bff409a 330 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 10:52b22bff409a 331 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 13:54ee98850a15 332
JKleinRot 13:54ee98850a15 333 pc.printf("Ga EMG meten\n");
JKleinRot 10:52b22bff409a 334
JKleinRot 10:52b22bff409a 335 xbk = EMG_bi.read(); //EMG signaal uitlezen
JKleinRot 13:54ee98850a15 336 xbk10 = xbk9;
JKleinRot 13:54ee98850a15 337 xbk9 = xbk8;
JKleinRot 13:54ee98850a15 338 xbk8 = xbk7;
JKleinRot 13:54ee98850a15 339 xbk7 = xbk6;
JKleinRot 13:54ee98850a15 340 xbk6 = xbk5;
JKleinRot 13:54ee98850a15 341 xbk5 = xbk4;
JKleinRot 13:54ee98850a15 342 xbk4 = xbk3;
JKleinRot 13:54ee98850a15 343 xbk3 = xbk2;
JKleinRot 13:54ee98850a15 344 xbk2 = xbk1;
JKleinRot 13:54ee98850a15 345 xbk1 = xbk;
JKleinRot 13:54ee98850a15 346 pc.printf("xbk10 is %f\n", xbk10);
JKleinRot 13:54ee98850a15 347
JKleinRot 13:54ee98850a15 348 if(xbk10 > 0){
JKleinRot 13:54ee98850a15 349 pc.printf("10 waarden gemeten\n");
JKleinRot 13:54ee98850a15 350 state = EMG_BICEPS;
JKleinRot 13:54ee98850a15 351 }
JKleinRot 10:52b22bff409a 352 }
JKleinRot 10:52b22bff409a 353 break;
JKleinRot 13:54ee98850a15 354 }
JKleinRot 13:54ee98850a15 355
JKleinRot 13:54ee98850a15 356 case EMG_BICEPS:
JKleinRot 13:54ee98850a15 357 {
JKleinRot 13:54ee98850a15 358 pc.printf("EMG_BICEPS\n");
JKleinRot 13:54ee98850a15 359 meanxbk = (xbk1 + xbk2 + xbk3 + xbk4 + xbk5 + xbk6 + xbk7 + xbk8 + xbk9 + xbk10)/10; //Offset bepalen van de eerste 10 gemeten waarden
JKleinRot 13:54ee98850a15 360
JKleinRot 13:54ee98850a15 361 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 13:54ee98850a15 362 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 13:54ee98850a15 363
JKleinRot 13:54ee98850a15 364 xb = EMG_bi.read(); //EMG meten van biceps
JKleinRot 13:54ee98850a15 365 xbo = xb - meanxbk; //Gemeten waarden zonder offset
JKleinRot 13:54ee98850a15 366
JKleinRot 13:54ee98850a15 367 filter_biceps();
JKleinRot 10:52b22bff409a 368
JKleinRot 10:52b22bff409a 369 }
JKleinRot 13:54ee98850a15 370
JKleinRot 11:e9b906b5f572 371 default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
JKleinRot 11:e9b906b5f572 372 state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
JKleinRot 10:52b22bff409a 373 }
JKleinRot 9:454e7da8ab65 374 }
JKleinRot 10:52b22bff409a 375 pc.printf("state: %u\n",state);
JKleinRot 3:adc3052039e7 376 }
JKleinRot 0:859c89785d3f 377 }