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@18:d3a7f8b4cb22, 2014-10-30 (annotated)

Committer:
JKleinRot
Date:
Thu Oct 30 11:35:04 2014 +0000
Revision:
18:d3a7f8b4cb22
Parent:
17:c694a0e63a89
Child:
19:38c9d177b6ee
2014-10-30 Nieuwe filterco?ffici?nten. En referentie telkens hoger in kleine stapjes;

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