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