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@19:38c9d177b6ee, 2014-10-30 (annotated)

Committer:
JKleinRot
Date:
Thu Oct 30 16:12:46 2014 +0000
Revision:
19:38c9d177b6ee
Parent:
18:d3a7f8b4cb22
Child:
20:1cb0cf0d49ac
2014-10-30 Laatste versie voor Tickers voor PID buiten de while(1)

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 15:3ebd0e666a9c 5 #include "arm_math.h" //Filter bibliotheek inladen, makkelijk de filters maken, minder grote lappen tekst
JKleinRot 0:859c89785d3f 6
JKleinRot 0:859c89785d3f 7 //Constanten definiëren en waarde geven
JKleinRot 11:e9b906b5f572 8 #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
JKleinRot 19:38c9d177b6ee 9 #define KP_arm1 0.5 //Factor proprotionele regelaar arm 1
JKleinRot 19:38c9d177b6ee 10 #define KI_arm1 0.1 //Factor integraal regelaar arm 1
JKleinRot 19:38c9d177b6ee 11 #define KD_arm1 0.01 //Factor afgeleide regelaar arm 1
JKleinRot 19:38c9d177b6ee 12 #define KP_arm2 0.3 //Factor proprotionele regelaar arm 2
JKleinRot 19:38c9d177b6ee 13 #define KI_arm2 0.1 //Factor integraal regelaar arm 2
JKleinRot 19:38c9d177b6ee 14 #define KD_arm2 0.1 //Factor afgeleide regelaar arm 2
JKleinRot 19:38c9d177b6ee 15 #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
JKleinRot 0:859c89785d3f 16
JKleinRot 16:c34c5d9dfe1a 17 //Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk
JKleinRot 16:c34c5d9dfe1a 18 //High Pass filter
JKleinRot 15:3ebd0e666a9c 19 #define A1 1
JKleinRot 18:d3a7f8b4cb22 20 #define A2 -1.561018075800718
JKleinRot 18:d3a7f8b4cb22 21 #define A3 0.641351538057563
JKleinRot 18:d3a7f8b4cb22 22 #define B1 0.800592403464570
JKleinRot 18:d3a7f8b4cb22 23 #define B2 -1.601184806929141
JKleinRot 18:d3a7f8b4cb22 24 #define B3 0.800592403464570
JKleinRot 13:54ee98850a15 25
JKleinRot 16:c34c5d9dfe1a 26 //Notch filter
JKleinRot 13:54ee98850a15 27 #define C1 1
JKleinRot 18:d3a7f8b4cb22 28 #define C2 -1.18733334554802E-16
JKleinRot 18:d3a7f8b4cb22 29 #define C3 0.939062505817492
JKleinRot 18:d3a7f8b4cb22 30 #define D1 0.969531252908746
JKleinRot 18:d3a7f8b4cb22 31 #define D2 -1.18733334554802E-16
JKleinRot 18:d3a7f8b4cb22 32 #define D3 0.969531252908746
JKleinRot 13:54ee98850a15 33
JKleinRot 16:c34c5d9dfe1a 34 //Low pass filter
JKleinRot 13:54ee98850a15 35 #define E1 1
JKleinRot 18:d3a7f8b4cb22 36 #define E2 -1.77863177782459
JKleinRot 18:d3a7f8b4cb22 37 #define E3 0.800802646665708
JKleinRot 18:d3a7f8b4cb22 38 #define F1 0.00554271721028068
JKleinRot 18:d3a7f8b4cb22 39 #define F2 0.0110854344205614
JKleinRot 18:d3a7f8b4cb22 40 #define F3 0.00554271721028068
JKleinRot 18:d3a7f8b4cb22 41
JKleinRot 15:3ebd0e666a9c 42
JKleinRot 0:859c89785d3f 43 //Pinverdeling en naamgeving variabelen
JKleinRot 17:c694a0e63a89 44 TextLCD lcd(PTD1, PTA12, PTB2, PTB3, PTC2, PTD3, TextLCD::LCD16x2); //LCD scherm
JKleinRot 16:c34c5d9dfe1a 45 MODSERIAL pc(USBTX, USBRX); //PC
JKleinRot 0:859c89785d3f 46
JKleinRot 16:c34c5d9dfe1a 47 PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
JKleinRot 16:c34c5d9dfe1a 48 DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1
JKleinRot 16:c34c5d9dfe1a 49 Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit)
JKleinRot 16:c34c5d9dfe1a 50 PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2
JKleinRot 16:c34c5d9dfe1a 51 DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2
JKleinRot 16:c34c5d9dfe1a 52 Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit)
JKleinRot 16:c34c5d9dfe1a 53 AnalogIn EMG_bi(PTB1); //Uitlezen EMG signaal biceps
JKleinRot 17:c694a0e63a89 54 AnalogIn EMG_tri(PTB0); //Uitlezen EMG signaal triceps
JKleinRot 16:c34c5d9dfe1a 55 //Blauw op 3,3 V en groen op GND, voeding beide encoders
JKleinRot 1:e5e1eb9d0025 56
JKleinRot 6:3b6fad529520 57 Ticker ticker_regelaar; //Ticker voor regelaar motor
JKleinRot 6:3b6fad529520 58 Ticker ticker_EMG; //Ticker voor EMG meten
JKleinRot 16:c34c5d9dfe1a 59 Timer biceps_kalibratie; //Timer voor kalibratiemeting EMG biceps
JKleinRot 16:c34c5d9dfe1a 60 Timer triceps_kalibratie; //Timer voor kalibratiemeting EMG triceps
JKleinRot 1:e5e1eb9d0025 61
JKleinRot 9:454e7da8ab65 62 //States definiëren
JKleinRot 17:c694a0e63a89 63 enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, START, B, T, BB, BT, TB, TT, BBB, BBT, BTB, BTT, TBB, TBT, TTB, TTT, BBBB, BBBT, BBTB, BBTT, BTBB, BTBT, BTTB, BTTT, TBBB, TBBT, TBTB, TBTT, TTBB, TTBT, TTTB, TTTT}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
JKleinRot 9:454e7da8ab65 64 uint8_t state = RUST; //State is rust aan het begin
JKleinRot 9:454e7da8ab65 65
JKleinRot 6:3b6fad529520 66 //Gedefinieerde datatypen en naamgeving en beginwaarden
JKleinRot 11:e9b906b5f572 67 float pwm_to_motor_arm1; //PWM output naar motor arm 1
JKleinRot 11:e9b906b5f572 68 float pwm_to_motor_arm2; //PWM output naar motor arm 2
JKleinRot 13:54ee98850a15 69 float error_arm1_kalibratie; //Error in pulsen arm 1
JKleinRot 13:54ee98850a15 70 float vorige_error_arm1 = 0; //Derivative actie van regelaar arm 1
JKleinRot 13:54ee98850a15 71 float integraal_arm1 = 0; //Integraal actie van regelaar arm 1
JKleinRot 13:54ee98850a15 72 float afgeleide_arm1; //Afgeleide actie van regelaar arm 1
JKleinRot 13:54ee98850a15 73 float error_arm2_kalibratie; //Error in pulsen arm 2
JKleinRot 13:54ee98850a15 74 float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2
JKleinRot 13:54ee98850a15 75 float integraal_arm2 = 0; //Integraal actie van regelaar arm 2
JKleinRot 13:54ee98850a15 76 float afgeleide_arm2; //Afgeleide actie van regelaar arm 2
JKleinRot 13:54ee98850a15 77 float xb; //Gemeten EMG waarde biceps
JKleinRot 16:c34c5d9dfe1a 78 float xt; //Gemeten EMG waarde triceps
JKleinRot 15:3ebd0e666a9c 79
JKleinRot 19:38c9d177b6ee 80 float referentie_arm1 = 0;
JKleinRot 19:38c9d177b6ee 81 float referentie_arm2 = 0;
JKleinRot 18:d3a7f8b4cb22 82
JKleinRot 16:c34c5d9dfe1a 83 arm_biquad_casd_df1_inst_f32 highpass_biceps; //Highpass_biceps IIR filter in direct form 1
JKleinRot 17:c694a0e63a89 84 float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter
JKleinRot 17:c694a0e63a89 85 float highpass_biceps_states[8]; //Aantal states van het filter, het aantal y(n-x) en x(n-x)
JKleinRot 13:54ee98850a15 86
JKleinRot 16:c34c5d9dfe1a 87 arm_biquad_casd_df1_inst_f32 notch_biceps; //Notch_biceps IIR filter in direct form 1
JKleinRot 17:c694a0e63a89 88 float notch_biceps_const[] = {D1, D2, D3, -C2, -C3, D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter
JKleinRot 17:c694a0e63a89 89 float notch_biceps_states[8]; //Aantal states van het filter
JKleinRot 15:3ebd0e666a9c 90
JKleinRot 16:c34c5d9dfe1a 91 arm_biquad_casd_df1_inst_f32 lowpass_biceps; //Lowpass_biceps IIR filter in direct form 1
JKleinRot 17:c694a0e63a89 92 float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3, F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter
JKleinRot 17:c694a0e63a89 93 float lowpass_biceps_states[8]; //Aantal states van het filter
JKleinRot 15:3ebd0e666a9c 94
JKleinRot 16:c34c5d9dfe1a 95 float xbf; //Gefilterde EMG waarde biceps
JKleinRot 16:c34c5d9dfe1a 96 float xbfmax = 0; //Maximale gefilterde EMG waarde biceps
JKleinRot 13:54ee98850a15 97
JKleinRot 16:c34c5d9dfe1a 98 arm_biquad_casd_df1_inst_f32 highpass_triceps; //Highpass_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
JKleinRot 17:c694a0e63a89 99 float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter
JKleinRot 17:c694a0e63a89 100 float highpass_triceps_states[8]; //Aantal states van het filter
JKleinRot 13:54ee98850a15 101
JKleinRot 16:c34c5d9dfe1a 102 arm_biquad_casd_df1_inst_f32 notch_triceps; //Notch_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
JKleinRot 17:c694a0e63a89 103 float notch_triceps_const[] = {D1, D2, D3, -C2, -C3, D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter
JKleinRot 17:c694a0e63a89 104 float notch_triceps_states[8]; //Aantal states van het filter
JKleinRot 13:54ee98850a15 105
JKleinRot 16:c34c5d9dfe1a 106 arm_biquad_casd_df1_inst_f32 lowpass_triceps; //Lowpass_biceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
JKleinRot 17:c694a0e63a89 107 float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3, F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter
JKleinRot 17:c694a0e63a89 108 float lowpass_triceps_states[8]; //Aantal states van het filter
JKleinRot 13:54ee98850a15 109
JKleinRot 16:c34c5d9dfe1a 110 float xtf; //Gefilterde EMG waarde triceps
JKleinRot 16:c34c5d9dfe1a 111 float xtfmax = 0; //Maximale gefilterde EMG waarde triceps
JKleinRot 15:3ebd0e666a9c 112
JKleinRot 16:c34c5d9dfe1a 113 float xbt; //Thresholdwaarde EMG biceps
JKleinRot 16:c34c5d9dfe1a 114 float xtt; //Thresholdwaarde EMG triceps
JKleinRot 7:dd3cba61b34b 115
JKleinRot 6:3b6fad529520 116 volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 117 void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
JKleinRot 10:52b22bff409a 118 {
JKleinRot 10:52b22bff409a 119 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 120 }
JKleinRot 1:e5e1eb9d0025 121
JKleinRot 6:3b6fad529520 122 volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 123 void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
JKleinRot 10:52b22bff409a 124 {
JKleinRot 10:52b22bff409a 125 regelaar_EMG_flag = true;
JKleinRot 4:69540b9c0646 126 }
JKleinRot 4:69540b9c0646 127
JKleinRot 10:52b22bff409a 128 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 129 {
JKleinRot 19:38c9d177b6ee 130 if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum
JKleinRot 2:0cb899f2800a 131 *in = min;
JKleinRot 2:0cb899f2800a 132 }
JKleinRot 19:38c9d177b6ee 133 if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum
JKleinRot 2:0cb899f2800a 134 *in = max;
JKleinRot 19:38c9d177b6ee 135 } else { //In alle andere gevallen is de waarde de waarde zelf
JKleinRot 2:0cb899f2800a 136 *in = *in;
JKleinRot 2:0cb899f2800a 137 }
JKleinRot 2:0cb899f2800a 138 }
JKleinRot 1:e5e1eb9d0025 139
JKleinRot 16:c34c5d9dfe1a 140 void arm1_naar_thuispositie() //Brengt arm 1 naar de beginpositie
JKleinRot 10:52b22bff409a 141 {
JKleinRot 19:38c9d177b6ee 142 referentie_arm1 = referentie_arm1 + 180.0/200.0;
JKleinRot 19:38c9d177b6ee 143
JKleinRot 19:38c9d177b6ee 144 if (referentie_arm1 >= 180) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
JKleinRot 19:38c9d177b6ee 145 referentie_arm1 = 180;
JKleinRot 10:52b22bff409a 146
JKleinRot 19:38c9d177b6ee 147 }
JKleinRot 19:38c9d177b6ee 148 error_arm1_kalibratie = (referentie_arm1 - puls_motor_arm1.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
JKleinRot 19:38c9d177b6ee 149 integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
JKleinRot 19:38c9d177b6ee 150 afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
JKleinRot 19:38c9d177b6ee 151 pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1;//Output naar motor na PID regelaar
JKleinRot 19:38c9d177b6ee 152 keep_in_range(&pwm_to_motor_arm1, -1, 1);
JKleinRot 19:38c9d177b6ee 153
JKleinRot 19:38c9d177b6ee 154 if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 10:52b22bff409a 155 dir_motor_arm1.write(1);
JKleinRot 19:38c9d177b6ee 156 } else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 157 dir_motor_arm1.write(0);
JKleinRot 7:dd3cba61b34b 158 }
JKleinRot 18:d3a7f8b4cb22 159
JKleinRot 11:e9b906b5f572 160 pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 16:c34c5d9dfe1a 161 pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
JKleinRot 16:c34c5d9dfe1a 162 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen
JKleinRot 10:52b22bff409a 163
JKleinRot 19:38c9d177b6ee 164 state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 19:38c9d177b6ee 165 pc.printf("KALIBRATIE_ARM1 afgerond\n"); //Tekst voor controle Arm 1 naar thuispositie
JKleinRot 19:38c9d177b6ee 166
JKleinRot 10:52b22bff409a 167 }
JKleinRot 7:dd3cba61b34b 168
JKleinRot 16:c34c5d9dfe1a 169 void arm2_naar_thuispositie() //Brengt arm 2 naar beginpositie
JKleinRot 10:52b22bff409a 170 {
JKleinRot 19:38c9d177b6ee 171 referentie_arm2 = referentie_arm2 + 35/200;
JKleinRot 19:38c9d177b6ee 172 error_arm2_kalibratie = (referentie_arm2 - puls_motor_arm2.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
JKleinRot 16:c34c5d9dfe1a 173 integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
JKleinRot 16:c34c5d9dfe1a 174 afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
JKleinRot 16:c34c5d9dfe1a 175 pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;//Output naar motor na PID regelaar
JKleinRot 16:c34c5d9dfe1a 176 keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 10:52b22bff409a 177
JKleinRot 19:38c9d177b6ee 178 if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 7:dd3cba61b34b 179 dir_motor_arm2.write(1);
JKleinRot 19:38c9d177b6ee 180 } else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 181 dir_motor_arm2.write(0);
JKleinRot 7:dd3cba61b34b 182 }
JKleinRot 11:e9b906b5f572 183 pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 16:c34c5d9dfe1a 184 pc.printf("pulsmotorgetposition %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
JKleinRot 16:c34c5d9dfe1a 185 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2); //Huidige PWM waarde naar pc sturen
JKleinRot 10:52b22bff409a 186
JKleinRot 19:38c9d177b6ee 187 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 16:c34c5d9dfe1a 188 state = EMG_KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 16:c34c5d9dfe1a 189 pc.printf("KALIBRATIE_ARM2 afgerond\n\r"); //Tekst voor controle Arm 2 naar thuispositie
JKleinRot 7:dd3cba61b34b 190 }
JKleinRot 7:dd3cba61b34b 191 }
JKleinRot 0:859c89785d3f 192
JKleinRot 16:c34c5d9dfe1a 193 void filter_biceps() //Filtert het EMG signaal van de biceps
JKleinRot 13:54ee98850a15 194 {
JKleinRot 16:c34c5d9dfe1a 195 arm_biquad_cascade_df1_f32(&highpass_biceps, &xb, &xbf, 1); //Highpass filter voor het EMG signaal van de biceps, filtert de xb en schrijft het over in de xbf, 1 EMG waarde filteren per keer
JKleinRot 19:38c9d177b6ee 196
JKleinRot 16:c34c5d9dfe1a 197 arm_biquad_cascade_df1_f32(&notch_biceps, &xbf, &xbf, 1); //Notch filter voor het EMG signaal van de biceps, filtert de xbf uit de highpass en schrijft het opnieuw in de xbf, 1 EMG waarde filteren per keer
JKleinRot 16:c34c5d9dfe1a 198
JKleinRot 16:c34c5d9dfe1a 199 xbf = fabs(xbf); //Rectifier, schrijft de absolute waarde van de xbf uit de notch opnieuw in de xbf, fabs omdat het een float is
JKleinRot 15:3ebd0e666a9c 200
JKleinRot 16:c34c5d9dfe1a 201 arm_biquad_cascade_df1_f32(&lowpass_biceps, &xbf, &xbf, 1); //Lowpass filter voor het EMG signaal van de biceps, filtert de xbf uit de rectifier en schrijft het opnieuw in de xbf, 1 EMG waarde filteren per keer
JKleinRot 19:38c9d177b6ee 202
JKleinRot 16:c34c5d9dfe1a 203 pc.printf("xbf is %f\n\r", xbf); //De gefilterde EMG waarde van de biceps naar pc sturen
JKleinRot 16:c34c5d9dfe1a 204 }
JKleinRot 15:3ebd0e666a9c 205
JKleinRot 16:c34c5d9dfe1a 206 void filter_triceps() //Filtert het EMG signaal van de triceps
JKleinRot 16:c34c5d9dfe1a 207 {
JKleinRot 16:c34c5d9dfe1a 208 arm_biquad_cascade_df1_f32(&highpass_triceps, &xt, &xtf, 1); //Highpass filter voor het EMG signaal van de triceps, filtert de xt en schrijft het over in de xtf, 1 EMG waarde filteren per keer
JKleinRot 16:c34c5d9dfe1a 209
JKleinRot 16:c34c5d9dfe1a 210 arm_biquad_cascade_df1_f32(&notch_triceps, &xtf, &xtf, 1); //Notch filter voor het EMG signaal van de triceps, filtert de xtf uit de highpass en schrijft het opnieuw in de xtf, 1 EMG waarde filteren per keer
JKleinRot 15:3ebd0e666a9c 211
JKleinRot 16:c34c5d9dfe1a 212 xtf = fabs(xtf); //Rectifier, schrijft de absolute waarde van de xtf uit de notch opnieuw in de xtf, fabs omdat het een float is
JKleinRot 16:c34c5d9dfe1a 213
JKleinRot 16:c34c5d9dfe1a 214 arm_biquad_cascade_df1_f32(&lowpass_triceps, &xtf, &xtf, 1); //Lowpass filter voor het EMG signaal van de triceps, filtert de xtf uit de rectifier en schrijft het opnieuw in de xtf, 1 EMG waarde filteren per keer
JKleinRot 16:c34c5d9dfe1a 215
JKleinRot 16:c34c5d9dfe1a 216 pc.printf("xtf is %f\n\r", xtf); //De gefilterde EMG waarde van de triceps naar pc sturen
JKleinRot 15:3ebd0e666a9c 217
JKleinRot 13:54ee98850a15 218 }
JKleinRot 13:54ee98850a15 219
JKleinRot 16:c34c5d9dfe1a 220 int main() //Main script
JKleinRot 15:3ebd0e666a9c 221 {
JKleinRot 19:38c9d177b6ee 222 while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
JKleinRot 10:52b22bff409a 223 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 19:38c9d177b6ee 224
JKleinRot 10:52b22bff409a 225
JKleinRot 19:38c9d177b6ee 226 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 227
JKleinRot 19:38c9d177b6ee 228 case RUST: { //Aanzetten
JKleinRot 19:38c9d177b6ee 229 pc.printf("RUST\n\r"); //Begin RUST naar pc sturen
JKleinRot 16:c34c5d9dfe1a 230 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 16:c34c5d9dfe1a 231 lcd.printf(" BMT K9 GR. 4 "); //Tekst op LCD scherm
JKleinRot 16:c34c5d9dfe1a 232 lcd.locate(0,1); //Zet de tekst op de tweede regel
JKleinRot 16:c34c5d9dfe1a 233 lcd.printf("HOI! IK BEN PIPO"); //Tekst op LCD scherm
JKleinRot 16:c34c5d9dfe1a 234 wait(2); //Twee seconden wachten
JKleinRot 16:c34c5d9dfe1a 235 lcd.cls(); //Maak LCD scherm leeg
JKleinRot 14:e1816efa712d 236 pc.printf("RUST afgerond\n\r"); //Tekst voor controle Aanzetten
JKleinRot 16:c34c5d9dfe1a 237 state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
JKleinRot 16:c34c5d9dfe1a 238 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 239 }
JKleinRot 10:52b22bff409a 240
JKleinRot 19:38c9d177b6ee 241 case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie
JKleinRot 16:c34c5d9dfe1a 242 pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM1 naar pc sturen
JKleinRot 16:c34c5d9dfe1a 243 wait(1); //Een seconde wachten
JKleinRot 15:3ebd0e666a9c 244
JKleinRot 10:52b22bff409a 245 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 246
JKleinRot 19:38c9d177b6ee 247 while(state == KALIBRATIE_ARM1) {
JKleinRot 16:c34c5d9dfe1a 248 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 249 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 250
JKleinRot 10:52b22bff409a 251 arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 252 }
JKleinRot 10:52b22bff409a 253 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 254 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 255 }
JKleinRot 10:52b22bff409a 256
JKleinRot 19:38c9d177b6ee 257 case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
JKleinRot 19:38c9d177b6ee 258 pc.printf("KALIBRATIE_ARM2\n\r"); //Begin KALIBRATIE_ARM2 naar pc sturen
JKleinRot 16:c34c5d9dfe1a 259 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 260
JKleinRot 10:52b22bff409a 261 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 262
JKleinRot 19:38c9d177b6ee 263 while(state == KALIBRATIE_ARM2) {
JKleinRot 16:c34c5d9dfe1a 264 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 265 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 266
JKleinRot 10:52b22bff409a 267 arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 268 }
JKleinRot 11:e9b906b5f572 269 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 270 ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
JKleinRot 16:c34c5d9dfe1a 271 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 272 }
JKleinRot 15:3ebd0e666a9c 273
JKleinRot 19:38c9d177b6ee 274 case EMG_KALIBRATIE_BICEPS: { //Kalibratie biceps voor bepalen threshold
JKleinRot 16:c34c5d9dfe1a 275 pc.printf("EMG__KALIBRATIE_BICEPS\n\r"); //Begin EMG_KALIBRATIE_BICEPS naar pc sturen
JKleinRot 19:38c9d177b6ee 276
JKleinRot 16:c34c5d9dfe1a 277 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 16:c34c5d9dfe1a 278 lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm
JKleinRot 16:c34c5d9dfe1a 279 lcd.locate(0,1); //Zet tekst op tweede regel
JKleinRot 16:c34c5d9dfe1a 280 lcd.printf(" 2 X BICEPS AAN "); //Tekst op LCD scherm
JKleinRot 15:3ebd0e666a9c 281
JKleinRot 17:c694a0e63a89 282 arm_biquad_cascade_df1_init_f32(&highpass_biceps, 2, highpass_biceps_const, highpass_biceps_states); //Highpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
JKleinRot 17:c694a0e63a89 283 arm_biquad_cascade_df1_init_f32(&notch_biceps, 2, notch_biceps_const, notch_biceps_states); //Notchfilter biceps met bijbehorende filtercoëfficiënten en states definiëren
JKleinRot 17:c694a0e63a89 284 arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 2, lowpass_biceps_const, lowpass_biceps_states); //Lowpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
JKleinRot 15:3ebd0e666a9c 285
JKleinRot 16:c34c5d9dfe1a 286 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 16:c34c5d9dfe1a 287 biceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt
JKleinRot 15:3ebd0e666a9c 288
JKleinRot 19:38c9d177b6ee 289 while(biceps_kalibratie.read() <= 5) { //Zolang de timer nog geen 5 seconden heeft gemeten
JKleinRot 19:38c9d177b6ee 290
JKleinRot 15:3ebd0e666a9c 291 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 15:3ebd0e666a9c 292 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 293
JKleinRot 15:3ebd0e666a9c 294 xb = EMG_bi.read(); //EMG meten van biceps
JKleinRot 15:3ebd0e666a9c 295
JKleinRot 17:c694a0e63a89 296 filter_biceps(); //Voer acties uit om EMG signaal van de biceps te filteren
JKleinRot 15:3ebd0e666a9c 297
JKleinRot 19:38c9d177b6ee 298 if(int(biceps_kalibratie.read()) == 0) { //Wanneer de timer nog geen een seconde heeft gemeten
JKleinRot 17:c694a0e63a89 299 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 300 lcd.printf(" 5 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 301 pc.printf("4"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 302 }
JKleinRot 19:38c9d177b6ee 303 if(int(biceps_kalibratie.read()) == 1) { //Wanneer de timer nog geen twee seconden heeft gemeten
JKleinRot 17:c694a0e63a89 304 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 305 lcd.printf(" 4 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 306 pc.printf("3"); //Controle naar pc sturen
JKleinRot 14:e1816efa712d 307 }
JKleinRot 19:38c9d177b6ee 308 if(int(biceps_kalibratie.read()) == 2) { //Wanneer de timer nog geen drie seconden heeft gemeten
JKleinRot 17:c694a0e63a89 309 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 310 lcd.printf(" 3 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 311 pc.printf("2"); //Controle naar pc sturen
JKleinRot 14:e1816efa712d 312 }
JKleinRot 19:38c9d177b6ee 313 if(int(biceps_kalibratie.read()) == 3) { //Wanneer de timer nog geen vier seconden heeft gemeten
JKleinRot 17:c694a0e63a89 314 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 315 lcd.printf(" 2 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 316 pc.printf("1"); //Controle naar pc sturen
JKleinRot 14:e1816efa712d 317 }
JKleinRot 19:38c9d177b6ee 318 if(int(biceps_kalibratie.read()) == 4) { //Wanneer de timer nog geen vijf seconden heeft gemeten
JKleinRot 17:c694a0e63a89 319 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 320 lcd.printf(" 1 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 321 pc.printf("1"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 322 }
JKleinRot 15:3ebd0e666a9c 323
JKleinRot 15:3ebd0e666a9c 324 }
JKleinRot 19:38c9d177b6ee 325 if(xbf >= xbfmax) { //Als de gefilterde EMG waarde groter is dan xbfmax
JKleinRot 17:c694a0e63a89 326 xbfmax = xbf; //Dan wordt deze gefilterde EMG waarde de nieuwe xbfmax
JKleinRot 15:3ebd0e666a9c 327 }
JKleinRot 19:38c9d177b6ee 328
JKleinRot 17:c694a0e63a89 329 xbt = xbfmax * 0.8; //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten
JKleinRot 17:c694a0e63a89 330 pc.printf("threshold is %f\n\r", xbt); //Thresholdwaarde naar pc sturen
JKleinRot 17:c694a0e63a89 331 state = EMG_KALIBRATIE_TRICEPS; //Gaat door naar kalibratie van de EMG van de triceps
JKleinRot 17:c694a0e63a89 332 break; //Stopt alle acties in deze case
JKleinRot 15:3ebd0e666a9c 333 }
JKleinRot 15:3ebd0e666a9c 334
JKleinRot 19:38c9d177b6ee 335 case EMG_KALIBRATIE_TRICEPS: {
JKleinRot 17:c694a0e63a89 336 pc.printf("EMG__KALIBRATIE_TRICEPS\n\r"); //Begin EMG_KALIBRATIE_TRICEPS naar pc sturen
JKleinRot 15:3ebd0e666a9c 337
JKleinRot 17:c694a0e63a89 338 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 339 lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 340 lcd.locate(0,1); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 341 lcd.printf(" 2 X TRICEPS AAN"); //Tekst op LCD scherm
JKleinRot 15:3ebd0e666a9c 342
JKleinRot 19:38c9d177b6ee 343 arm_biquad_cascade_df1_init_f32(&highpass_triceps, 2, highpass_triceps_const, highpass_triceps_states); //Highpassfilter triceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
JKleinRot 17:c694a0e63a89 344 arm_biquad_cascade_df1_init_f32(&notch_triceps, 2, notch_triceps_const, notch_triceps_states); //Notchfilter triceps met bijbehorende filtercoëfficiënten en states definiëren
JKleinRot 17:c694a0e63a89 345 arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 2, lowpass_triceps_const, lowpass_triceps_states); //Lowpassfilter triceps met bijbehorende filtercoëfficiënten en states definiëren, cascade van 2 tweede orde filters
JKleinRot 15:3ebd0e666a9c 346
JKleinRot 17:c694a0e63a89 347 //ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Deze ticker loopt nog
JKleinRot 17:c694a0e63a89 348 triceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt
JKleinRot 15:3ebd0e666a9c 349
JKleinRot 19:38c9d177b6ee 350 while(triceps_kalibratie.read() <= 5) { //Zolang de timer nog geen 5 seconden heeft gemeten
JKleinRot 15:3ebd0e666a9c 351 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 15:3ebd0e666a9c 352 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 353
JKleinRot 17:c694a0e63a89 354 xt = EMG_tri.read(); //EMG meten van ticeps
JKleinRot 15:3ebd0e666a9c 355
JKleinRot 17:c694a0e63a89 356 filter_triceps(); //Voer acties uit om EMG signaal van de triceps te meten
JKleinRot 15:3ebd0e666a9c 357
JKleinRot 19:38c9d177b6ee 358 if(int(triceps_kalibratie.read()) == 0) { //Wanneer de timer nog geen twee seconden heeft gemeten
JKleinRot 17:c694a0e63a89 359 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 360 lcd.printf(" 5 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 361 pc.printf("4"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 362 }
JKleinRot 19:38c9d177b6ee 363 if(int(triceps_kalibratie.read()) == 1) { //Wanneer de timer nog geen twee seconden heeft gemeten
JKleinRot 17:c694a0e63a89 364 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 365 lcd.printf(" 4 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 366 pc.printf("3"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 367 }
JKleinRot 19:38c9d177b6ee 368 if(int(triceps_kalibratie.read()) == 2) { //Wanneer de timer nog geen drie seconden heeft gemeten
JKleinRot 17:c694a0e63a89 369 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 370 lcd.printf(" 3 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 371 pc.printf("2"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 372 }
JKleinRot 19:38c9d177b6ee 373 if(int(triceps_kalibratie.read()) == 3) { //Wanneer de timer nog geen vier seconden heeft gemeten
JKleinRot 17:c694a0e63a89 374 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 375 lcd.printf(" 2 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 376 pc.printf("1"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 377 }
JKleinRot 19:38c9d177b6ee 378 if(int(triceps_kalibratie.read()) == 4) { //Wanneer de timer nog geen vijf seconden heeft gemeten
JKleinRot 17:c694a0e63a89 379 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 380 lcd.printf(" 1 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 381 pc.printf("1"); //Controle naar pc sturen
JKleinRot 14:e1816efa712d 382 }
JKleinRot 15:3ebd0e666a9c 383
JKleinRot 15:3ebd0e666a9c 384 }
JKleinRot 19:38c9d177b6ee 385 if(xtf >= xtfmax) { //Als de gefilterde EMG waarde groter is dan xtfmax
JKleinRot 17:c694a0e63a89 386 xtfmax = xtf; //Dan wordt deze gefilterde EMG waarde de nieuwe xtfmax
JKleinRot 10:52b22bff409a 387 }
JKleinRot 19:38c9d177b6ee 388
JKleinRot 17:c694a0e63a89 389 xtt = xtfmax * 0.8; //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten
JKleinRot 17:c694a0e63a89 390 pc.printf("threshold is %f\n\r", xtt); //Thresholdwaarde naar pc sturen
JKleinRot 17:c694a0e63a89 391 state = START; //Gaat door naar het slaan, kalibratie nu afgerond
JKleinRot 17:c694a0e63a89 392 break; //Stopt alle acties in deze case
JKleinRot 10:52b22bff409a 393 }
JKleinRot 15:3ebd0e666a9c 394
JKleinRot 17:c694a0e63a89 395 case START: {
JKleinRot 17:c694a0e63a89 396 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 397 lcd.printf(" START "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 398
JKleinRot 17:c694a0e63a89 399 pc.printf("START\n\r"); //Controle naar pc sturen
JKleinRot 19:38c9d177b6ee 400
JKleinRot 19:38c9d177b6ee 401 while(state == START) {
JKleinRot 17:c694a0e63a89 402 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 17:c694a0e63a89 403 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 404
JKleinRot 17:c694a0e63a89 405 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 406 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 407 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 408 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 15:3ebd0e666a9c 409
JKleinRot 19:38c9d177b6ee 410 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 411 state = B; //Ga door naar state B
JKleinRot 15:3ebd0e666a9c 412 }
JKleinRot 19:38c9d177b6ee 413 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 414 state = T; //Ga door naar state T
JKleinRot 15:3ebd0e666a9c 415 }
JKleinRot 15:3ebd0e666a9c 416 }
JKleinRot 17:c694a0e63a89 417 break; //Stopt met de acties in deze case
JKleinRot 16:c34c5d9dfe1a 418 }
JKleinRot 19:38c9d177b6ee 419
JKleinRot 19:38c9d177b6ee 420 case B: {
JKleinRot 17:c694a0e63a89 421 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 422 lcd.printf(" B "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 423 pc.printf("B\n\r"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 424
JKleinRot 19:38c9d177b6ee 425 while(state == B) {
JKleinRot 19:38c9d177b6ee 426 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 19:38c9d177b6ee 427 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 19:38c9d177b6ee 428
JKleinRot 19:38c9d177b6ee 429 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 19:38c9d177b6ee 430 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 19:38c9d177b6ee 431 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 19:38c9d177b6ee 432 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 19:38c9d177b6ee 433
JKleinRot 19:38c9d177b6ee 434 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 435 state = BB; //Ga door naar state BB
JKleinRot 19:38c9d177b6ee 436 }
JKleinRot 19:38c9d177b6ee 437 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 438 state = BT; //Ga door naar state BT
JKleinRot 19:38c9d177b6ee 439 }
JKleinRot 19:38c9d177b6ee 440 }
JKleinRot 19:38c9d177b6ee 441 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 442 }
JKleinRot 19:38c9d177b6ee 443
JKleinRot 19:38c9d177b6ee 444 case T: {
JKleinRot 19:38c9d177b6ee 445 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 446 lcd.printf(" T "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 447 pc.printf("T\n\r"); //Controle naar pc sturen
JKleinRot 19:38c9d177b6ee 448
JKleinRot 19:38c9d177b6ee 449 while(state == T) {
JKleinRot 17:c694a0e63a89 450 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 17:c694a0e63a89 451 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 452
JKleinRot 17:c694a0e63a89 453 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 454 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 455 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 456 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 16:c34c5d9dfe1a 457
JKleinRot 19:38c9d177b6ee 458 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 459 state = TB; //Ga door naar state TB
JKleinRot 16:c34c5d9dfe1a 460 }
JKleinRot 19:38c9d177b6ee 461 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 462 state = TT; //Ga door naar state TT
JKleinRot 16:c34c5d9dfe1a 463 }
JKleinRot 16:c34c5d9dfe1a 464 }
JKleinRot 17:c694a0e63a89 465 break; //Stop met alle acties in deze case
JKleinRot 16:c34c5d9dfe1a 466 }
JKleinRot 16:c34c5d9dfe1a 467
JKleinRot 19:38c9d177b6ee 468 case BB: {
JKleinRot 17:c694a0e63a89 469 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 470 lcd.printf(" BB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 471 pc.printf("BB\n\r"); //Controle naar pc sturen
JKleinRot 16:c34c5d9dfe1a 472
JKleinRot 19:38c9d177b6ee 473 while(state == BB) {
JKleinRot 17:c694a0e63a89 474 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 17:c694a0e63a89 475 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 16:c34c5d9dfe1a 476
JKleinRot 17:c694a0e63a89 477 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 478 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 479 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 480 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 16:c34c5d9dfe1a 481
JKleinRot 19:38c9d177b6ee 482 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 483 state = BBB; //Ga door naar state BBB
JKleinRot 16:c34c5d9dfe1a 484 }
JKleinRot 19:38c9d177b6ee 485 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 486 state = BBT; //Ga door naar state BBT
JKleinRot 16:c34c5d9dfe1a 487 }
JKleinRot 16:c34c5d9dfe1a 488 }
JKleinRot 17:c694a0e63a89 489 break; //Stop met alle acties in deze case
JKleinRot 16:c34c5d9dfe1a 490 }
JKleinRot 19:38c9d177b6ee 491
JKleinRot 19:38c9d177b6ee 492 case BT: {
JKleinRot 17:c694a0e63a89 493 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 494 lcd.printf(" BT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 495 pc.printf("BT\n\r"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 496
JKleinRot 19:38c9d177b6ee 497 while(state == BT) {
JKleinRot 17:c694a0e63a89 498 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 17:c694a0e63a89 499 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 500
JKleinRot 17:c694a0e63a89 501 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 502 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 503 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 504 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 17:c694a0e63a89 505
JKleinRot 19:38c9d177b6ee 506 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 507 state = BTB; //Ga door naar state BTB
JKleinRot 17:c694a0e63a89 508 }
JKleinRot 19:38c9d177b6ee 509 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 510 state = BTT; //Ga door naar state BTT
JKleinRot 17:c694a0e63a89 511 }
JKleinRot 17:c694a0e63a89 512 }
JKleinRot 19:38c9d177b6ee 513 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 514 }
JKleinRot 19:38c9d177b6ee 515
JKleinRot 19:38c9d177b6ee 516 case TB: {
JKleinRot 17:c694a0e63a89 517 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 518 lcd.printf(" TB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 519 pc.printf("TB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 520
JKleinRot 19:38c9d177b6ee 521 while(state == TB) {
JKleinRot 17:c694a0e63a89 522 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 17:c694a0e63a89 523 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 524
JKleinRot 17:c694a0e63a89 525 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 526 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 527 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 528 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 17:c694a0e63a89 529
JKleinRot 19:38c9d177b6ee 530 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 531 state = TBB; //Ga door naar state TBB
JKleinRot 17:c694a0e63a89 532 }
JKleinRot 19:38c9d177b6ee 533 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 534 state = TBT; //Ga door naar state TBT
JKleinRot 17:c694a0e63a89 535 }
JKleinRot 17:c694a0e63a89 536 }
JKleinRot 19:38c9d177b6ee 537 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 538 }
JKleinRot 17:c694a0e63a89 539
JKleinRot 19:38c9d177b6ee 540 case TT: {
JKleinRot 17:c694a0e63a89 541 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 542 lcd.printf(" TT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 543 pc.printf("TT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 544
JKleinRot 19:38c9d177b6ee 545 while(state == TT) {
JKleinRot 17:c694a0e63a89 546 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 17:c694a0e63a89 547 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 548
JKleinRot 17:c694a0e63a89 549 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 550 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 551 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 552 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 17:c694a0e63a89 553
JKleinRot 19:38c9d177b6ee 554 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 555 state = TTB; //Ga door naar state TTB
JKleinRot 17:c694a0e63a89 556 }
JKleinRot 19:38c9d177b6ee 557 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 558 state = TTT; //Ga door naar state TTT
JKleinRot 17:c694a0e63a89 559 }
JKleinRot 17:c694a0e63a89 560 }
JKleinRot 19:38c9d177b6ee 561 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 562 }
JKleinRot 19:38c9d177b6ee 563
JKleinRot 19:38c9d177b6ee 564 case BBB: {
JKleinRot 17:c694a0e63a89 565 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 566 lcd.printf(" BBB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 567 pc.printf("BBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 568
JKleinRot 19:38c9d177b6ee 569 while(state == BBB) {
JKleinRot 17:c694a0e63a89 570 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 17:c694a0e63a89 571 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 572
JKleinRot 17:c694a0e63a89 573 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 574 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 575 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 576 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 17:c694a0e63a89 577
JKleinRot 19:38c9d177b6ee 578 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 579 state = BBBB; //Ga door naar state BBBB
JKleinRot 17:c694a0e63a89 580 }
JKleinRot 19:38c9d177b6ee 581 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 582 state = BBBT; //Ga door naar state BBBT
JKleinRot 17:c694a0e63a89 583 }
JKleinRot 17:c694a0e63a89 584 }
JKleinRot 19:38c9d177b6ee 585 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 586 }
JKleinRot 17:c694a0e63a89 587
JKleinRot 19:38c9d177b6ee 588
JKleinRot 19:38c9d177b6ee 589 case BBT: {
JKleinRot 17:c694a0e63a89 590 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 591 lcd.printf(" BBT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 592 pc.printf("BBT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 593
JKleinRot 19:38c9d177b6ee 594 while(state == BBT) {
JKleinRot 17:c694a0e63a89 595 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 17:c694a0e63a89 596 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 597
JKleinRot 17:c694a0e63a89 598 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 599 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 600 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 601 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 17:c694a0e63a89 602
JKleinRot 19:38c9d177b6ee 603 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 604 state = BBTB; //Ga door naar state BBTB
JKleinRot 17:c694a0e63a89 605 }
JKleinRot 19:38c9d177b6ee 606 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 607 state = BBTT; //Ga door naar state BBTT
JKleinRot 17:c694a0e63a89 608 }
JKleinRot 17:c694a0e63a89 609 }
JKleinRot 19:38c9d177b6ee 610 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 611 }
JKleinRot 19:38c9d177b6ee 612
JKleinRot 19:38c9d177b6ee 613 case BTB: {
JKleinRot 17:c694a0e63a89 614 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 615 lcd.printf(" BTB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 616 pc.printf("BTB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 617
JKleinRot 19:38c9d177b6ee 618 while(state == BTB) {
JKleinRot 17:c694a0e63a89 619 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 17:c694a0e63a89 620 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 621
JKleinRot 17:c694a0e63a89 622 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 623 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 624 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 625 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 17:c694a0e63a89 626
JKleinRot 19:38c9d177b6ee 627 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 628 state = BTBB; //Ga door naar state BTBB
JKleinRot 17:c694a0e63a89 629 }
JKleinRot 19:38c9d177b6ee 630 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 631 state = BTBT; //Ga door naar state BTBT
JKleinRot 17:c694a0e63a89 632 }
JKleinRot 17:c694a0e63a89 633 }
JKleinRot 19:38c9d177b6ee 634 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 635 }
JKleinRot 19:38c9d177b6ee 636
JKleinRot 19:38c9d177b6ee 637 case BTT: {
JKleinRot 17:c694a0e63a89 638 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 639 lcd.printf(" BTT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 640 pc.printf("BTT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 641
JKleinRot 19:38c9d177b6ee 642 while(state == BTT) {
JKleinRot 17:c694a0e63a89 643 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 17:c694a0e63a89 644 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 645
JKleinRot 17:c694a0e63a89 646 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 647 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 648 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 649 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 17:c694a0e63a89 650
JKleinRot 19:38c9d177b6ee 651 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 652 state = BTTB; //Ga door naar state BTTB
JKleinRot 17:c694a0e63a89 653 }
JKleinRot 19:38c9d177b6ee 654 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 655 state = BTTT; //Ga door naar state BTTT
JKleinRot 17:c694a0e63a89 656 }
JKleinRot 17:c694a0e63a89 657 }
JKleinRot 19:38c9d177b6ee 658 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 659 }
JKleinRot 19:38c9d177b6ee 660
JKleinRot 19:38c9d177b6ee 661 case TBB: {
JKleinRot 17:c694a0e63a89 662 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 663 lcd.printf(" TBB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 664 pc.printf("TBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 665
JKleinRot 19:38c9d177b6ee 666 while(state == TBB) {
JKleinRot 17:c694a0e63a89 667 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 17:c694a0e63a89 668 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 669
JKleinRot 17:c694a0e63a89 670 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 671 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 672 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 673 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 17:c694a0e63a89 674
JKleinRot 19:38c9d177b6ee 675 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 676 state = TBBB; //Ga door naar state TBBB
JKleinRot 17:c694a0e63a89 677 }
JKleinRot 19:38c9d177b6ee 678 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 679 state = TBBT; //Ga door naar state TBBT
JKleinRot 17:c694a0e63a89 680 }
JKleinRot 17:c694a0e63a89 681 }
JKleinRot 19:38c9d177b6ee 682 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 683 }
JKleinRot 19:38c9d177b6ee 684
JKleinRot 19:38c9d177b6ee 685 case TBT: {
JKleinRot 17:c694a0e63a89 686 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 687 lcd.printf(" TBT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 688 pc.printf("TBT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 689
JKleinRot 19:38c9d177b6ee 690 while(state == TBT) {
JKleinRot 17:c694a0e63a89 691 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 17:c694a0e63a89 692 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 693
JKleinRot 17:c694a0e63a89 694 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 695 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 696 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 697 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 17:c694a0e63a89 698
JKleinRot 19:38c9d177b6ee 699 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 700 state = TBTB; //Ga door naar state TBTB
JKleinRot 17:c694a0e63a89 701 }
JKleinRot 19:38c9d177b6ee 702 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 703 state = TBTT; //Ga door naar state TBTT
JKleinRot 17:c694a0e63a89 704 }
JKleinRot 17:c694a0e63a89 705 }
JKleinRot 19:38c9d177b6ee 706 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 707 }
JKleinRot 19:38c9d177b6ee 708
JKleinRot 19:38c9d177b6ee 709 case TTB: {
JKleinRot 17:c694a0e63a89 710 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 711 lcd.printf(" BBB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 712 pc.printf("BBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 713
JKleinRot 19:38c9d177b6ee 714 while(state == BBB) {
JKleinRot 17:c694a0e63a89 715 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 17:c694a0e63a89 716 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 717
JKleinRot 17:c694a0e63a89 718 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 719 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 720 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 721 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 17:c694a0e63a89 722
JKleinRot 19:38c9d177b6ee 723 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 724 state = TTBB; //Ga door naar state TTBB
JKleinRot 17:c694a0e63a89 725 }
JKleinRot 19:38c9d177b6ee 726 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 727 state = TTBT; //Ga door naar state TTBT
JKleinRot 17:c694a0e63a89 728 }
JKleinRot 17:c694a0e63a89 729 }
JKleinRot 19:38c9d177b6ee 730 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 731 }
JKleinRot 19:38c9d177b6ee 732
JKleinRot 19:38c9d177b6ee 733 case TTT: {
JKleinRot 17:c694a0e63a89 734 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 735 lcd.printf(" TTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 736 pc.printf("TTT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 737
JKleinRot 19:38c9d177b6ee 738 while(state == TTT) {
JKleinRot 17:c694a0e63a89 739 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 17:c694a0e63a89 740 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 741
JKleinRot 17:c694a0e63a89 742 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 743 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 744 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 745 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 17:c694a0e63a89 746
JKleinRot 19:38c9d177b6ee 747 if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 748 state = TTTB; //Ga door naar state TTTB
JKleinRot 17:c694a0e63a89 749 }
JKleinRot 19:38c9d177b6ee 750 if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 751 state = TTTT; //Ga door naar state TTTT
JKleinRot 17:c694a0e63a89 752 }
JKleinRot 17:c694a0e63a89 753 }
JKleinRot 19:38c9d177b6ee 754 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 755 }
JKleinRot 19:38c9d177b6ee 756
JKleinRot 19:38c9d177b6ee 757 case BBBB: {
JKleinRot 17:c694a0e63a89 758 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 759 lcd.printf(" BBBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 760 pc.printf("BBBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 761
JKleinRot 19:38c9d177b6ee 762 while(state == BBBB) {
JKleinRot 17:c694a0e63a89 763 //Motoractie
JKleinRot 19:38c9d177b6ee 764 }
JKleinRot 19:38c9d177b6ee 765 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 766 }
JKleinRot 19:38c9d177b6ee 767
JKleinRot 19:38c9d177b6ee 768 case BBBT: {
JKleinRot 17:c694a0e63a89 769 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 770 lcd.printf(" BBBT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 771 pc.printf("BBBT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 772
JKleinRot 19:38c9d177b6ee 773 while(state == BBBT) {
JKleinRot 17:c694a0e63a89 774 //Motoractie
JKleinRot 19:38c9d177b6ee 775 }
JKleinRot 19:38c9d177b6ee 776 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 777 }
JKleinRot 19:38c9d177b6ee 778
JKleinRot 19:38c9d177b6ee 779 case BBTB: {
JKleinRot 17:c694a0e63a89 780 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 781 lcd.printf(" BBTB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 782 pc.printf("BBTB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 783
JKleinRot 19:38c9d177b6ee 784 while(state == BBTB) {
JKleinRot 17:c694a0e63a89 785 //Motoractie
JKleinRot 19:38c9d177b6ee 786 }
JKleinRot 19:38c9d177b6ee 787 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 788 }
JKleinRot 19:38c9d177b6ee 789
JKleinRot 19:38c9d177b6ee 790 case BBTT: {
JKleinRot 17:c694a0e63a89 791 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 792 lcd.printf(" BBTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 793 pc.printf("BBTT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 794
JKleinRot 19:38c9d177b6ee 795 while(state == BBTT) {
JKleinRot 17:c694a0e63a89 796 //Motoractie
JKleinRot 19:38c9d177b6ee 797 }
JKleinRot 19:38c9d177b6ee 798 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 799 }
JKleinRot 19:38c9d177b6ee 800
JKleinRot 19:38c9d177b6ee 801 case BTBB: {
JKleinRot 17:c694a0e63a89 802 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 803 lcd.printf(" BTBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 804 pc.printf("BTBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 805
JKleinRot 19:38c9d177b6ee 806 while(state == BTBB) {
JKleinRot 17:c694a0e63a89 807 //Motoractie
JKleinRot 19:38c9d177b6ee 808 }
JKleinRot 19:38c9d177b6ee 809 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 810 }
JKleinRot 19:38c9d177b6ee 811
JKleinRot 19:38c9d177b6ee 812 case BTBT: {
JKleinRot 17:c694a0e63a89 813 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 814 lcd.printf(" BTBT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 815 pc.printf("BTBT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 816
JKleinRot 19:38c9d177b6ee 817 while(state == BTBT) {
JKleinRot 17:c694a0e63a89 818 //Motoractie
JKleinRot 19:38c9d177b6ee 819 }
JKleinRot 19:38c9d177b6ee 820 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 821 }
JKleinRot 19:38c9d177b6ee 822
JKleinRot 19:38c9d177b6ee 823 case BTTB: {
JKleinRot 17:c694a0e63a89 824 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 825 lcd.printf(" BTTB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 826 pc.printf("BTTB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 827
JKleinRot 19:38c9d177b6ee 828 while(state == BTTB) {
JKleinRot 17:c694a0e63a89 829 //Motoractie
JKleinRot 19:38c9d177b6ee 830 }
JKleinRot 19:38c9d177b6ee 831 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 832 }
JKleinRot 19:38c9d177b6ee 833
JKleinRot 19:38c9d177b6ee 834 case BTTT: {
JKleinRot 17:c694a0e63a89 835 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 836 lcd.printf(" BTTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 837 pc.printf("BTTT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 838
JKleinRot 19:38c9d177b6ee 839 while(state == BTTT) {
JKleinRot 17:c694a0e63a89 840 //Motoractie
JKleinRot 19:38c9d177b6ee 841 }
JKleinRot 19:38c9d177b6ee 842 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 843 }
JKleinRot 19:38c9d177b6ee 844
JKleinRot 19:38c9d177b6ee 845 case TBBB: {
JKleinRot 17:c694a0e63a89 846 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 847 lcd.printf(" TBBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 848 pc.printf("TBBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 849
JKleinRot 19:38c9d177b6ee 850 while(state == TBBB) {
JKleinRot 17:c694a0e63a89 851 //Motoractie
JKleinRot 19:38c9d177b6ee 852 }
JKleinRot 19:38c9d177b6ee 853 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 854 }
JKleinRot 19:38c9d177b6ee 855
JKleinRot 19:38c9d177b6ee 856 case TBBT: {
JKleinRot 17:c694a0e63a89 857 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 858 lcd.printf(" TBBT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 859 pc.printf("TBBT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 860
JKleinRot 19:38c9d177b6ee 861 while(state == TBBT) {
JKleinRot 17:c694a0e63a89 862 //Motoractie
JKleinRot 19:38c9d177b6ee 863 }
JKleinRot 19:38c9d177b6ee 864 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 865 }
JKleinRot 19:38c9d177b6ee 866
JKleinRot 19:38c9d177b6ee 867 case TBTB: {
JKleinRot 17:c694a0e63a89 868 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 869 lcd.printf(" TBBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 870 pc.printf("TBBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 871
JKleinRot 19:38c9d177b6ee 872 while(state == TBBB) {
JKleinRot 17:c694a0e63a89 873 //Motoractie
JKleinRot 19:38c9d177b6ee 874 }
JKleinRot 19:38c9d177b6ee 875 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 876 }
JKleinRot 19:38c9d177b6ee 877
JKleinRot 19:38c9d177b6ee 878 case TBTT: {
JKleinRot 17:c694a0e63a89 879 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 880 lcd.printf(" TBTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 881 pc.printf("TBTT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 882
JKleinRot 19:38c9d177b6ee 883 while(state == TBTT) {
JKleinRot 17:c694a0e63a89 884 //Motoractie
JKleinRot 19:38c9d177b6ee 885 }
JKleinRot 19:38c9d177b6ee 886 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 887 }
JKleinRot 19:38c9d177b6ee 888
JKleinRot 19:38c9d177b6ee 889 case TTBB: {
JKleinRot 17:c694a0e63a89 890 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 891 lcd.printf(" TTBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 892 pc.printf("TTBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 893
JKleinRot 19:38c9d177b6ee 894 while(state == TTBB) {
JKleinRot 17:c694a0e63a89 895 //Motoractie
JKleinRot 19:38c9d177b6ee 896 }
JKleinRot 19:38c9d177b6ee 897 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 898 }
JKleinRot 19:38c9d177b6ee 899
JKleinRot 19:38c9d177b6ee 900 case TTBT: {
JKleinRot 17:c694a0e63a89 901 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 902 lcd.printf(" TTBT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 903 pc.printf("TTBT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 904
JKleinRot 19:38c9d177b6ee 905 while(state == TTBT) {
JKleinRot 17:c694a0e63a89 906 //Motoractie
JKleinRot 19:38c9d177b6ee 907 }
JKleinRot 19:38c9d177b6ee 908 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 909 }
JKleinRot 19:38c9d177b6ee 910
JKleinRot 19:38c9d177b6ee 911 case TTTB: {
JKleinRot 17:c694a0e63a89 912 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 913 lcd.printf(" TTTB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 914 pc.printf("TTTB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 915
JKleinRot 19:38c9d177b6ee 916 while(state == TTTB) {
JKleinRot 17:c694a0e63a89 917 //Motoractie
JKleinRot 19:38c9d177b6ee 918 }
JKleinRot 19:38c9d177b6ee 919 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 920 }
JKleinRot 19:38c9d177b6ee 921
JKleinRot 19:38c9d177b6ee 922 case TTTT: {
JKleinRot 17:c694a0e63a89 923 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 924 lcd.printf(" TTTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 925 pc.printf("TTBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 926
JKleinRot 19:38c9d177b6ee 927 while(state == TTTT) {
JKleinRot 19:38c9d177b6ee 928 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 19:38c9d177b6ee 929 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 19:38c9d177b6ee 930
JKleinRot 19:38c9d177b6ee 931 //Positie arm 1 is goed
JKleinRot 19:38c9d177b6ee 932 //Snelheid arm 2
JKleinRot 19:38c9d177b6ee 933
JKleinRot 19:38c9d177b6ee 934 float referentie_speed_TTTT;
JKleinRot 19:38c9d177b6ee 935 float error_speed_TTTT;
JKleinRot 19:38c9d177b6ee 936 float integraal_speed_TTTT = 0;
JKleinRot 19:38c9d177b6ee 937 float afgeleide_speed_TTTT;
JKleinRot 19:38c9d177b6ee 938 float vorige_error_speed_TTTT = 0;
JKleinRot 19:38c9d177b6ee 939 float pwm_to_motor_speed_TTTT;
JKleinRot 19:38c9d177b6ee 940 #define KPs 0.001
JKleinRot 19:38c9d177b6ee 941 #define KIs 0
JKleinRot 19:38c9d177b6ee 942 #define KDs 0
JKleinRot 19:38c9d177b6ee 943
JKleinRot 19:38c9d177b6ee 944 referentie_speed_TTTT = referentie_speed_TTTT + 1902/40;
JKleinRot 19:38c9d177b6ee 945 error_speed_TTTT = (referentie_speed_TTTT - puls_motor_arm2.getSpeed());
JKleinRot 19:38c9d177b6ee 946 integraal_speed_TTTT = integraal_speed_TTTT + error_speed_TTTT*SAMPLETIME_REGELAAR;
JKleinRot 19:38c9d177b6ee 947 afgeleide_speed_TTTT = (error_speed_TTTT - vorige_error_speed_TTTT)/SAMPLETIME_REGELAAR;
JKleinRot 19:38c9d177b6ee 948 pwm_to_motor_speed_TTTT = error_speed_TTTT*KPs + integraal_speed_TTTT*KIs + afgeleide_speed_TTTT*KDs;
JKleinRot 19:38c9d177b6ee 949 keep_in_range(&pwm_to_motor_speed_TTTT, -1, 1);
JKleinRot 19:38c9d177b6ee 950
JKleinRot 19:38c9d177b6ee 951 if(pwm_to_motor_speed_TTTT > 0) {
JKleinRot 19:38c9d177b6ee 952 dir_motor_arm2.write(1);
JKleinRot 19:38c9d177b6ee 953 } else {
JKleinRot 19:38c9d177b6ee 954 dir_motor_arm2.write(0);
JKleinRot 19:38c9d177b6ee 955 }
JKleinRot 19:38c9d177b6ee 956
JKleinRot 19:38c9d177b6ee 957 pwm_motor_arm1.write(0);
JKleinRot 19:38c9d177b6ee 958
JKleinRot 19:38c9d177b6ee 959 if(puls_motor_arm2.getPosition() >= 97) {
JKleinRot 19:38c9d177b6ee 960 pwm_motor_arm1.write(-fabs(pwm_to_motor_speed_TTTT));
JKleinRot 19:38c9d177b6ee 961 }
JKleinRot 19:38c9d177b6ee 962
JKleinRot 19:38c9d177b6ee 963 if(referentie_speed_TTTT == 1902) {
JKleinRot 19:38c9d177b6ee 964 referentie_speed_TTTT = 1902;
JKleinRot 19:38c9d177b6ee 965 }
JKleinRot 19:38c9d177b6ee 966
JKleinRot 19:38c9d177b6ee 967 referentie_arm1 = referentie_arm1 + 180/200;
JKleinRot 19:38c9d177b6ee 968
JKleinRot 19:38c9d177b6ee 969 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 970 }
JKleinRot 19:38c9d177b6ee 971
JKleinRot 19:38c9d177b6ee 972 default: {
JKleinRot 19:38c9d177b6ee 973 //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
JKleinRot 19:38c9d177b6ee 974 state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
JKleinRot 19:38c9d177b6ee 975 }
JKleinRot 19:38c9d177b6ee 976
JKleinRot 19:38c9d177b6ee 977 pc.printf("state: %u\n\r",state);
JKleinRot 17:c694a0e63a89 978 }
JKleinRot 19:38c9d177b6ee 979 //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 9:454e7da8ab65 980 }
JKleinRot 19:38c9d177b6ee 981 }