![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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(¬ch_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(¬ch_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(¬ch_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(¬ch_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 | } |