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@17:c694a0e63a89, 2014-10-29 (annotated)

Committer:
JKleinRot
Date:
Wed Oct 29 11:51:32 2014 +0000
Revision:
17:c694a0e63a89
Parent:
16:c34c5d9dfe1a
Child:
18:d3a7f8b4cb22
2014-10-29 Comments zijn af, pinindeling LCD scherm heel iets anders door al bezetten pinnen. Alle mogelijke cases gemaakt, alleen nog motoracties erin

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