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@21:f4e9f6c28de1, 2014-10-31 (annotated)

Committer:
JKleinRot
Date:
Fri Oct 31 12:37:06 2014 +0000
Revision:
21:f4e9f6c28de1
Parent:
20:1cb0cf0d49ac
Child:
22:838a17065bc7
2014-10-31 Beide motoren draaien naar de goede positie. De EMG kalibratie werkt ook. Er kan een keuze worden gemaakt in de eerste case. Daarna gaat hij in hetzelfde door.

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