2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
main.cpp@32:80fc2de5b511, 2014-11-05 (annotated)
- Committer:
- JKleinRot
- Date:
- Wed Nov 05 18:29:00 2014 +0000
- Revision:
- 32:80fc2de5b511
- Parent:
- 31:36fe36657f8d
2014-11-05 Laatste versie PIPO 2 groep 4. Kalibratie positie en EMG compleet. Keuze maken in doel werkt. Snelheidsregelaar werkt niet, PWM gestuurd. Niet alle cases voor doelen kunnen controleren, werken niet allemaal goed. Case BBBB werkt wel.
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 | 25:71e52c56be13 | 9 | #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten |
JKleinRot | 27:5ac1fc1005d7 | 10 | #define KP_arm1 0.025 //Factor proprotionele regelaar arm 1 |
JKleinRot | 25:71e52c56be13 | 11 | #define KI_arm1 0 //Factor integraal regelaar arm 1 |
JKleinRot | 25:71e52c56be13 | 12 | #define KD_arm1 0 //Factor afgeleide regelaar arm 1 |
JKleinRot | 23:5267c928ae2b | 13 | #define KP_arm2 0.01 //Factor proprotionele regelaar arm 2 |
JKleinRot | 21:f4e9f6c28de1 | 14 | #define KI_arm2 0 //Factor integraal regelaar arm 2 |
JKleinRot | 29:f95f0cc84349 | 15 | #define KD_arm2 0.0008 //Factor afgeleide regelaar arm 2 |
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 | 23:5267c928ae2b | 49 | Encoder puls_motor_arm1(PTD2, PTD0); //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 | 25:71e52c56be13 | 57 | Ticker ticker_regelaar; //Ticker voor flag veranderen referentie |
JKleinRot | 25:71e52c56be13 | 58 | Ticker ticker_EMG; //Ticker voor flag EMG meten |
JKleinRot | 25:71e52c56be13 | 59 | Ticker ticker_motor_arm1_pid; //Ticker voor PID regelaar motor arm 1 |
JKleinRot | 25:71e52c56be13 | 60 | Ticker ticker_motor_arm2_pid; //Ticker voor PID regelaar motor arm 2 |
JKleinRot | 25:71e52c56be13 | 61 | |
JKleinRot | 16:c34c5d9dfe1a | 62 | Timer biceps_kalibratie; //Timer voor kalibratiemeting EMG biceps |
JKleinRot | 16:c34c5d9dfe1a | 63 | Timer triceps_kalibratie; //Timer voor kalibratiemeting EMG triceps |
JKleinRot | 25:71e52c56be13 | 64 | Timer EMG; //Timer voor aantal seconden EMG meten |
JKleinRot | 1:e5e1eb9d0025 | 65 | |
JKleinRot | 9:454e7da8ab65 | 66 | //States definiëren |
JKleinRot | 25:71e52c56be13 | 67 | 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, WACHT, THUISPOSITIE_MIDDEN, THUISPOSITIE_RECHTS}; //Alle states benoemen, ieder krijgt een getal beginnend met 0 |
JKleinRot | 9:454e7da8ab65 | 68 | uint8_t state = RUST; //State is rust aan het begin |
JKleinRot | 9:454e7da8ab65 | 69 | |
JKleinRot | 6:3b6fad529520 | 70 | //Gedefinieerde datatypen en naamgeving en beginwaarden |
JKleinRot | 11:e9b906b5f572 | 71 | float pwm_to_motor_arm1; //PWM output naar motor arm 1 |
JKleinRot | 11:e9b906b5f572 | 72 | float pwm_to_motor_arm2; //PWM output naar motor arm 2 |
JKleinRot | 13:54ee98850a15 | 73 | float error_arm1_kalibratie; //Error in pulsen arm 1 |
JKleinRot | 13:54ee98850a15 | 74 | float vorige_error_arm1 = 0; //Derivative actie van regelaar arm 1 |
JKleinRot | 13:54ee98850a15 | 75 | float integraal_arm1 = 0; //Integraal actie van regelaar arm 1 |
JKleinRot | 13:54ee98850a15 | 76 | float afgeleide_arm1; //Afgeleide actie van regelaar arm 1 |
JKleinRot | 13:54ee98850a15 | 77 | float error_arm2_kalibratie; //Error in pulsen arm 2 |
JKleinRot | 13:54ee98850a15 | 78 | float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2 |
JKleinRot | 13:54ee98850a15 | 79 | float integraal_arm2 = 0; //Integraal actie van regelaar arm 2 |
JKleinRot | 13:54ee98850a15 | 80 | float afgeleide_arm2; //Afgeleide actie van regelaar arm 2 |
JKleinRot | 13:54ee98850a15 | 81 | float xb; //Gemeten EMG waarde biceps |
JKleinRot | 16:c34c5d9dfe1a | 82 | float xt; //Gemeten EMG waarde triceps |
JKleinRot | 19:38c9d177b6ee | 83 | float referentie_arm1 = 0; |
JKleinRot | 19:38c9d177b6ee | 84 | float referentie_arm2 = 0; |
JKleinRot | 23:5267c928ae2b | 85 | float t = 0; |
JKleinRot | 23:5267c928ae2b | 86 | |
JKleinRot | 25:71e52c56be13 | 87 | //Gedefinieerde filters met constanten en gefilterde waarden |
JKleinRot | 16:c34c5d9dfe1a | 88 | arm_biquad_casd_df1_inst_f32 highpass_biceps; //Highpass_biceps IIR filter in direct form 1 |
JKleinRot | 17:c694a0e63a89 | 89 | float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter |
JKleinRot | 17:c694a0e63a89 | 90 | float highpass_biceps_states[8]; //Aantal states van het filter, het aantal y(n-x) en x(n-x) |
JKleinRot | 13:54ee98850a15 | 91 | |
JKleinRot | 16:c34c5d9dfe1a | 92 | arm_biquad_casd_df1_inst_f32 notch_biceps; //Notch_biceps IIR filter in direct form 1 |
JKleinRot | 17:c694a0e63a89 | 93 | float notch_biceps_const[] = {D1, D2, D3, -C2, -C3, D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter |
JKleinRot | 17:c694a0e63a89 | 94 | float notch_biceps_states[8]; //Aantal states van het filter |
JKleinRot | 15:3ebd0e666a9c | 95 | |
JKleinRot | 16:c34c5d9dfe1a | 96 | arm_biquad_casd_df1_inst_f32 lowpass_biceps; //Lowpass_biceps IIR filter in direct form 1 |
JKleinRot | 17:c694a0e63a89 | 97 | float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3, F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter |
JKleinRot | 17:c694a0e63a89 | 98 | float lowpass_biceps_states[8]; //Aantal states van het filter |
JKleinRot | 15:3ebd0e666a9c | 99 | |
JKleinRot | 16:c34c5d9dfe1a | 100 | float xbf; //Gefilterde EMG waarde biceps |
JKleinRot | 16:c34c5d9dfe1a | 101 | float xbfmax = 0; //Maximale gefilterde EMG waarde biceps |
JKleinRot | 13:54ee98850a15 | 102 | |
JKleinRot | 16:c34c5d9dfe1a | 103 | 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 | 104 | float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3, B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter |
JKleinRot | 17:c694a0e63a89 | 105 | float highpass_triceps_states[8]; //Aantal states van het filter |
JKleinRot | 13:54ee98850a15 | 106 | |
JKleinRot | 16:c34c5d9dfe1a | 107 | 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 | 108 | float notch_triceps_const[] = {D1, D2, D3, -C2, -C3, D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter |
JKleinRot | 17:c694a0e63a89 | 109 | float notch_triceps_states[8]; //Aantal states van het filter |
JKleinRot | 13:54ee98850a15 | 110 | |
JKleinRot | 16:c34c5d9dfe1a | 111 | 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 | 112 | float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3, F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter |
JKleinRot | 17:c694a0e63a89 | 113 | float lowpass_triceps_states[8]; //Aantal states van het filter |
JKleinRot | 13:54ee98850a15 | 114 | |
JKleinRot | 16:c34c5d9dfe1a | 115 | float xtf; //Gefilterde EMG waarde triceps |
JKleinRot | 16:c34c5d9dfe1a | 116 | float xtfmax = 0; //Maximale gefilterde EMG waarde triceps |
JKleinRot | 15:3ebd0e666a9c | 117 | |
JKleinRot | 16:c34c5d9dfe1a | 118 | float xbt; //Thresholdwaarde EMG biceps |
JKleinRot | 16:c34c5d9dfe1a | 119 | float xtt; //Thresholdwaarde EMG triceps |
JKleinRot | 7:dd3cba61b34b | 120 | |
JKleinRot | 6:3b6fad529520 | 121 | volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma |
JKleinRot | 10:52b22bff409a | 122 | void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true |
JKleinRot | 10:52b22bff409a | 123 | { |
JKleinRot | 10:52b22bff409a | 124 | regelaar_ticker_flag = true; |
JKleinRot | 1:e5e1eb9d0025 | 125 | } |
JKleinRot | 1:e5e1eb9d0025 | 126 | |
JKleinRot | 6:3b6fad529520 | 127 | volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma |
JKleinRot | 10:52b22bff409a | 128 | void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true |
JKleinRot | 10:52b22bff409a | 129 | { |
JKleinRot | 10:52b22bff409a | 130 | regelaar_EMG_flag = true; |
JKleinRot | 4:69540b9c0646 | 131 | } |
JKleinRot | 4:69540b9c0646 | 132 | |
JKleinRot | 10:52b22bff409a | 133 | 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 | 134 | { |
JKleinRot | 19:38c9d177b6ee | 135 | if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum |
JKleinRot | 2:0cb899f2800a | 136 | *in = min; |
JKleinRot | 2:0cb899f2800a | 137 | } |
JKleinRot | 19:38c9d177b6ee | 138 | if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum |
JKleinRot | 2:0cb899f2800a | 139 | *in = max; |
JKleinRot | 19:38c9d177b6ee | 140 | } else { //In alle andere gevallen is de waarde de waarde zelf |
JKleinRot | 2:0cb899f2800a | 141 | *in = *in; |
JKleinRot | 2:0cb899f2800a | 142 | } |
JKleinRot | 2:0cb899f2800a | 143 | } |
JKleinRot | 1:e5e1eb9d0025 | 144 | |
JKleinRot | 25:71e52c56be13 | 145 | void motor_arm1_pid() //PID regelaar van motor arm 1 |
JKleinRot | 10:52b22bff409a | 146 | { |
JKleinRot | 19:38c9d177b6ee | 147 | 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 | 148 | integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar |
JKleinRot | 20:1cb0cf0d49ac | 149 | afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar |
JKleinRot | 20:1cb0cf0d49ac | 150 | pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1;//Output naar motor na PID regelaar |
JKleinRot | 25:71e52c56be13 | 151 | vorige_error_arm1 = error_arm1_kalibratie; //Vorige error is de volgende keer de huidige error van deze keer |
JKleinRot | 25:71e52c56be13 | 152 | keep_in_range(&pwm_to_motor_arm1, -1, 1); //Stelt 1 en -1 in als de maximale en minimale waarde die de pwm mag hebben |
JKleinRot | 19:38c9d177b6ee | 153 | |
JKleinRot | 21:f4e9f6c28de1 | 154 | if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1 |
JKleinRot | 23:5267c928ae2b | 155 | dir_motor_arm1.write(0); |
JKleinRot | 21:f4e9f6c28de1 | 156 | } else { //Anders richting nul |
JKleinRot | 23:5267c928ae2b | 157 | dir_motor_arm1.write(1); |
JKleinRot | 7:dd3cba61b34b | 158 | } |
JKleinRot | 18:d3a7f8b4cb22 | 159 | |
JKleinRot | 11:e9b906b5f572 | 160 | pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM |
JKleinRot | 20:1cb0cf0d49ac | 161 | } |
JKleinRot | 10:52b22bff409a | 162 | |
JKleinRot | 25:71e52c56be13 | 163 | void motor_arm2_pid() //PID regelaar van motor arm 2 |
JKleinRot | 20:1cb0cf0d49ac | 164 | { |
JKleinRot | 20:1cb0cf0d49ac | 165 | 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 | 166 | integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar |
JKleinRot | 20:1cb0cf0d49ac | 167 | afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar |
JKleinRot | 20:1cb0cf0d49ac | 168 | pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;//Output naar motor na PID regelaar |
JKleinRot | 25:71e52c56be13 | 169 | vorige_error_arm2 = error_arm2_kalibratie; //Vorige error is de volgende keer de huidige error van deze keer |
JKleinRot | 25:71e52c56be13 | 170 | keep_in_range(&pwm_to_motor_arm2, -1, 1); //Stelt 1 en -1 in als de maximale en minimale waarde die de pwm mag hebben |
JKleinRot | 19:38c9d177b6ee | 171 | |
JKleinRot | 20:1cb0cf0d49ac | 172 | if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1 |
JKleinRot | 20:1cb0cf0d49ac | 173 | dir_motor_arm2.write(1); |
JKleinRot | 20:1cb0cf0d49ac | 174 | } else { //Anders richting nul |
JKleinRot | 20:1cb0cf0d49ac | 175 | dir_motor_arm2.write(0); |
JKleinRot | 20:1cb0cf0d49ac | 176 | } |
JKleinRot | 20:1cb0cf0d49ac | 177 | pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM |
JKleinRot | 10:52b22bff409a | 178 | } |
JKleinRot | 7:dd3cba61b34b | 179 | |
JKleinRot | 16:c34c5d9dfe1a | 180 | void filter_biceps() //Filtert het EMG signaal van de biceps |
JKleinRot | 13:54ee98850a15 | 181 | { |
JKleinRot | 16:c34c5d9dfe1a | 182 | 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 | 183 | |
JKleinRot | 16:c34c5d9dfe1a | 184 | 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 | 185 | |
JKleinRot | 16:c34c5d9dfe1a | 186 | 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 | 187 | |
JKleinRot | 16:c34c5d9dfe1a | 188 | 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 | 189 | |
JKleinRot | 16:c34c5d9dfe1a | 190 | pc.printf("xbf is %f\n\r", xbf); //De gefilterde EMG waarde van de biceps naar pc sturen |
JKleinRot | 16:c34c5d9dfe1a | 191 | } |
JKleinRot | 15:3ebd0e666a9c | 192 | |
JKleinRot | 16:c34c5d9dfe1a | 193 | void filter_triceps() //Filtert het EMG signaal van de triceps |
JKleinRot | 16:c34c5d9dfe1a | 194 | { |
JKleinRot | 16:c34c5d9dfe1a | 195 | 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 | 196 | |
JKleinRot | 16:c34c5d9dfe1a | 197 | 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 | 198 | |
JKleinRot | 16:c34c5d9dfe1a | 199 | 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 | 200 | |
JKleinRot | 16:c34c5d9dfe1a | 201 | 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 | 202 | |
JKleinRot | 16:c34c5d9dfe1a | 203 | pc.printf("xtf is %f\n\r", xtf); //De gefilterde EMG waarde van de triceps naar pc sturen |
JKleinRot | 15:3ebd0e666a9c | 204 | |
JKleinRot | 13:54ee98850a15 | 205 | } |
JKleinRot | 13:54ee98850a15 | 206 | |
JKleinRot | 25:71e52c56be13 | 207 | void EMG_meten() //Meet de EMG van de biceps en de triceps |
JKleinRot | 22:838a17065bc7 | 208 | { |
JKleinRot | 22:838a17065bc7 | 209 | xb = EMG_bi.read(); //EMG signaal biceps uitlezen |
JKleinRot | 22:838a17065bc7 | 210 | filter_biceps(); //EMG signaal biceps filteren |
JKleinRot | 22:838a17065bc7 | 211 | xt = EMG_tri.read(); //EMG signaal triceps uitlezen |
JKleinRot | 22:838a17065bc7 | 212 | filter_triceps(); //EMG signaal triceps filteren |
JKleinRot | 22:838a17065bc7 | 213 | } |
JKleinRot | 22:838a17065bc7 | 214 | |
JKleinRot | 22:838a17065bc7 | 215 | |
JKleinRot | 16:c34c5d9dfe1a | 216 | int main() //Main script |
JKleinRot | 15:3ebd0e666a9c | 217 | { |
JKleinRot | 25:71e52c56be13 | 218 | ticker_motor_arm1_pid.attach(motor_arm1_pid,SAMPLETIME_REGELAAR); //Iedere SAMPLETIME_REGELAAR wordt door de PID regelaar bekeken of er een andere referentie is en stuurt indien nodig een pwm naar de motor |
JKleinRot | 25:71e52c56be13 | 219 | ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); //Iederen SAMPLETIME_REGELAAR wordt door de PID regelaar bekeken of er een andere referentie is en stuurt indien nodig een pwm naar de motor |
JKleinRot | 21:f4e9f6c28de1 | 220 | |
JKleinRot | 19:38c9d177b6ee | 221 | while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan |
JKleinRot | 10:52b22bff409a | 222 | pc.baud(38400); //PC baud rate is 38400 bits/seconde |
JKleinRot | 10:52b22bff409a | 223 | |
JKleinRot | 19:38c9d177b6ee | 224 | 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 | 225 | |
JKleinRot | 19:38c9d177b6ee | 226 | case RUST: { //Aanzetten |
JKleinRot | 19:38c9d177b6ee | 227 | pc.printf("RUST\n\r"); //Begin RUST naar pc sturen |
JKleinRot | 16:c34c5d9dfe1a | 228 | lcd.locate(0,0); //Zet de tekst op de eerste regel |
JKleinRot | 16:c34c5d9dfe1a | 229 | lcd.printf(" BMT K9 GR. 4 "); //Tekst op LCD scherm |
JKleinRot | 16:c34c5d9dfe1a | 230 | lcd.locate(0,1); //Zet de tekst op de tweede regel |
JKleinRot | 16:c34c5d9dfe1a | 231 | lcd.printf("HOI! IK BEN PIPO"); //Tekst op LCD scherm |
JKleinRot | 16:c34c5d9dfe1a | 232 | wait(2); //Twee seconden wachten |
JKleinRot | 16:c34c5d9dfe1a | 233 | lcd.cls(); //Maak LCD scherm leeg |
JKleinRot | 14:e1816efa712d | 234 | pc.printf("RUST afgerond\n\r"); //Tekst voor controle Aanzetten |
JKleinRot | 25:71e52c56be13 | 235 | state = KALIBRATIE_ARM1; //Door naar de volgende state |
JKleinRot | 16:c34c5d9dfe1a | 236 | break; //Stopt acties in deze case |
JKleinRot | 10:52b22bff409a | 237 | } |
JKleinRot | 10:52b22bff409a | 238 | |
JKleinRot | 19:38c9d177b6ee | 239 | case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie |
JKleinRot | 16:c34c5d9dfe1a | 240 | pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM1 naar pc sturen |
JKleinRot | 16:c34c5d9dfe1a | 241 | wait(1); //Een seconde wachten |
JKleinRot | 15:3ebd0e666a9c | 242 | |
JKleinRot | 10:52b22bff409a | 243 | ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken |
JKleinRot | 10:52b22bff409a | 244 | |
JKleinRot | 19:38c9d177b6ee | 245 | while(state == KALIBRATIE_ARM1) { |
JKleinRot | 16:c34c5d9dfe1a | 246 | 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 | 247 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 10:52b22bff409a | 248 | |
JKleinRot | 32:80fc2de5b511 | 249 | referentie_arm1 = referentie_arm1 + 203.0/200.0; //Referentie loopt in één seconde op tot het gewenste aantal pulsen |
JKleinRot | 21:f4e9f6c28de1 | 250 | |
JKleinRot | 25:71e52c56be13 | 251 | pc.printf("pulsmotorgetposition1 %d ", puls_motor_arm1.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen |
JKleinRot | 25:71e52c56be13 | 252 | pc.printf("pwmmotor1 %f ", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen |
JKleinRot | 25:71e52c56be13 | 253 | pc.printf("referentie1 %f\n\r", referentie_arm1); //Huidige referentie naar pc sturen |
JKleinRot | 25:71e52c56be13 | 254 | |
JKleinRot | 32:80fc2de5b511 | 255 | if (puls_motor_arm1.getPosition() >= 203) { //Als het gewenste aantal pulsen behaald is |
JKleinRot | 32:80fc2de5b511 | 256 | referentie_arm1 = 203; //Blijft de referentie op dat aantal pulsen staan |
JKleinRot | 25:71e52c56be13 | 257 | state = KALIBRATIE_ARM2; //Door naar de volgende state |
JKleinRot | 20:1cb0cf0d49ac | 258 | } |
JKleinRot | 10:52b22bff409a | 259 | } |
JKleinRot | 10:52b22bff409a | 260 | wait(1); //Een seconde wachten |
JKleinRot | 11:e9b906b5f572 | 261 | break; //Stopt acties in deze case |
JKleinRot | 10:52b22bff409a | 262 | } |
JKleinRot | 10:52b22bff409a | 263 | |
JKleinRot | 19:38c9d177b6ee | 264 | case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie |
JKleinRot | 19:38c9d177b6ee | 265 | pc.printf("KALIBRATIE_ARM2\n\r"); //Begin KALIBRATIE_ARM2 naar pc sturen |
JKleinRot | 16:c34c5d9dfe1a | 266 | wait(1); //Een seconde wachten |
JKleinRot | 10:52b22bff409a | 267 | |
JKleinRot | 10:52b22bff409a | 268 | //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken |
JKleinRot | 10:52b22bff409a | 269 | |
JKleinRot | 19:38c9d177b6ee | 270 | while(state == KALIBRATIE_ARM2) { |
JKleinRot | 16:c34c5d9dfe1a | 271 | 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 | 272 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 10:52b22bff409a | 273 | |
JKleinRot | 25:71e52c56be13 | 274 | referentie_arm2 = referentie_arm2 + 123.0/200.0; //Referentie loopt in één seconde op tot het gewenste aantal pulsen |
JKleinRot | 21:f4e9f6c28de1 | 275 | |
JKleinRot | 25:71e52c56be13 | 276 | pc.printf("pulsmotorgetposition2 %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen |
JKleinRot | 25:71e52c56be13 | 277 | pc.printf("pwmmotor2 %f ", pwm_to_motor_arm2); //Huidige PWM waarde naar motor naar pc sturen |
JKleinRot | 25:71e52c56be13 | 278 | pc.printf("referentie2 %f\n\r", referentie_arm2); //Huidige referentie naar pc sturen |
JKleinRot | 25:71e52c56be13 | 279 | |
JKleinRot | 25:71e52c56be13 | 280 | if(puls_motor_arm2.getPosition() >= 123) { //Als het gewenste aantal pulsen behaald is |
JKleinRot | 25:71e52c56be13 | 281 | referentie_arm2 = 123; //Blijft de referentie op dat aantal pulsen staan |
JKleinRot | 25:71e52c56be13 | 282 | state = EMG_KALIBRATIE_BICEPS; //Door naar de volgende state |
JKleinRot | 20:1cb0cf0d49ac | 283 | } |
JKleinRot | 10:52b22bff409a | 284 | } |
JKleinRot | 11:e9b906b5f572 | 285 | wait(1); //Een seconde wachten |
JKleinRot | 16:c34c5d9dfe1a | 286 | break; //Stopt acties in deze case |
JKleinRot | 10:52b22bff409a | 287 | } |
JKleinRot | 15:3ebd0e666a9c | 288 | |
JKleinRot | 19:38c9d177b6ee | 289 | case EMG_KALIBRATIE_BICEPS: { //Kalibratie biceps voor bepalen threshold |
JKleinRot | 16:c34c5d9dfe1a | 290 | pc.printf("EMG__KALIBRATIE_BICEPS\n\r"); //Begin EMG_KALIBRATIE_BICEPS naar pc sturen |
JKleinRot | 19:38c9d177b6ee | 291 | |
JKleinRot | 16:c34c5d9dfe1a | 292 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 16:c34c5d9dfe1a | 293 | lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm |
JKleinRot | 16:c34c5d9dfe1a | 294 | lcd.locate(0,1); //Zet tekst op tweede regel |
JKleinRot | 16:c34c5d9dfe1a | 295 | lcd.printf(" 2 X BICEPS AAN "); //Tekst op LCD scherm |
JKleinRot | 25:71e52c56be13 | 296 | wait(1); //Een seconden wachten |
JKleinRot | 25:71e52c56be13 | 297 | lcd.cls(); //LCD scherm leegmaken |
JKleinRot | 15:3ebd0e666a9c | 298 | |
JKleinRot | 17:c694a0e63a89 | 299 | 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 | 300 | 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 | 301 | 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 | 302 | |
JKleinRot | 16:c34c5d9dfe1a | 303 | ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconde de flag op laten steken |
JKleinRot | 16:c34c5d9dfe1a | 304 | biceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt |
JKleinRot | 15:3ebd0e666a9c | 305 | |
JKleinRot | 19:38c9d177b6ee | 306 | while(biceps_kalibratie.read() <= 5) { //Zolang de timer nog geen 5 seconden heeft gemeten |
JKleinRot | 19:38c9d177b6ee | 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 | 19:38c9d177b6ee | 315 | if(int(biceps_kalibratie.read()) == 0) { //Wanneer de timer nog geen een seconde heeft gemeten |
JKleinRot | 17:c694a0e63a89 | 316 | lcd.locate(0,0); //Zet de tekst op de eerste regel |
JKleinRot | 17:c694a0e63a89 | 317 | lcd.printf(" 5 "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 318 | pc.printf("4"); //Controle naar pc sturen |
JKleinRot | 17:c694a0e63a89 | 319 | } |
JKleinRot | 19:38c9d177b6ee | 320 | if(int(biceps_kalibratie.read()) == 1) { //Wanneer de timer nog geen twee seconden heeft gemeten |
JKleinRot | 17:c694a0e63a89 | 321 | lcd.locate(0,0); //Zet de tekst op de eerste regel |
JKleinRot | 17:c694a0e63a89 | 322 | lcd.printf(" 4 "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 323 | pc.printf("3"); //Controle naar pc sturen |
JKleinRot | 14:e1816efa712d | 324 | } |
JKleinRot | 19:38c9d177b6ee | 325 | if(int(biceps_kalibratie.read()) == 2) { //Wanneer de timer nog geen drie seconden heeft gemeten |
JKleinRot | 17:c694a0e63a89 | 326 | lcd.locate(0,0); //Zet de tekst op de eerste regel |
JKleinRot | 17:c694a0e63a89 | 327 | lcd.printf(" 3 "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 328 | pc.printf("2"); //Controle naar pc sturen |
JKleinRot | 14:e1816efa712d | 329 | } |
JKleinRot | 19:38c9d177b6ee | 330 | if(int(biceps_kalibratie.read()) == 3) { //Wanneer de timer nog geen vier seconden heeft gemeten |
JKleinRot | 17:c694a0e63a89 | 331 | lcd.locate(0,0); //Zet de tekst op de eerste regel |
JKleinRot | 17:c694a0e63a89 | 332 | lcd.printf(" 2 "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 333 | pc.printf("1"); //Controle naar pc sturen |
JKleinRot | 14:e1816efa712d | 334 | } |
JKleinRot | 19:38c9d177b6ee | 335 | if(int(biceps_kalibratie.read()) == 4) { //Wanneer de timer nog geen vijf seconden heeft gemeten |
JKleinRot | 17:c694a0e63a89 | 336 | lcd.locate(0,0); //Zet de tekst op de eerste regel |
JKleinRot | 17:c694a0e63a89 | 337 | lcd.printf(" 1 "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 338 | pc.printf("1"); //Controle naar pc sturen |
JKleinRot | 15:3ebd0e666a9c | 339 | } |
JKleinRot | 15:3ebd0e666a9c | 340 | |
JKleinRot | 21:f4e9f6c28de1 | 341 | if(xbf >= xbfmax) { //Als de gefilterde EMG waarde groter is dan xbfmax |
JKleinRot | 21:f4e9f6c28de1 | 342 | xbfmax = xbf; //Dan wordt deze gefilterde EMG waarde de nieuwe xbfmax |
JKleinRot | 21:f4e9f6c28de1 | 343 | } |
JKleinRot | 15:3ebd0e666a9c | 344 | } |
JKleinRot | 19:38c9d177b6ee | 345 | |
JKleinRot | 27:5ac1fc1005d7 | 346 | xbt = xbfmax * 0.80; //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten |
JKleinRot | 25:71e52c56be13 | 347 | pc.printf("maximale biceps %f", xbfmax); //Maximale gefilterde EMG waarde naar pc sturen |
JKleinRot | 17:c694a0e63a89 | 348 | pc.printf("threshold is %f\n\r", xbt); //Thresholdwaarde naar pc sturen |
JKleinRot | 17:c694a0e63a89 | 349 | state = EMG_KALIBRATIE_TRICEPS; //Gaat door naar kalibratie van de EMG van de triceps |
JKleinRot | 17:c694a0e63a89 | 350 | break; //Stopt alle acties in deze case |
JKleinRot | 15:3ebd0e666a9c | 351 | } |
JKleinRot | 15:3ebd0e666a9c | 352 | |
JKleinRot | 25:71e52c56be13 | 353 | case EMG_KALIBRATIE_TRICEPS: { //Kalibratie triceps voor bepalen threshold |
JKleinRot | 17:c694a0e63a89 | 354 | pc.printf("EMG__KALIBRATIE_TRICEPS\n\r"); //Begin EMG_KALIBRATIE_TRICEPS naar pc sturen |
JKleinRot | 15:3ebd0e666a9c | 355 | |
JKleinRot | 17:c694a0e63a89 | 356 | lcd.locate(0,0); //Zet de tekst op de eerste regel |
JKleinRot | 17:c694a0e63a89 | 357 | lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 358 | lcd.locate(0,1); //Zet tekst op eerste regel |
JKleinRot | 17:c694a0e63a89 | 359 | lcd.printf(" 2 X TRICEPS AAN"); //Tekst op LCD scherm |
JKleinRot | 25:71e52c56be13 | 360 | wait(1); //Een seconde wachten |
JKleinRot | 25:71e52c56be13 | 361 | lcd.cls(); //LCD scherm leegmaken |
JKleinRot | 15:3ebd0e666a9c | 362 | |
JKleinRot | 19:38c9d177b6ee | 363 | 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 | 364 | 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 | 365 | 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 | 366 | |
JKleinRot | 17:c694a0e63a89 | 367 | triceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt |
JKleinRot | 15:3ebd0e666a9c | 368 | |
JKleinRot | 19:38c9d177b6ee | 369 | while(triceps_kalibratie.read() <= 5) { //Zolang de timer nog geen 5 seconden heeft gemeten |
JKleinRot | 15:3ebd0e666a9c | 370 | while(regelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel |
JKleinRot | 15:3ebd0e666a9c | 371 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 15:3ebd0e666a9c | 372 | |
JKleinRot | 17:c694a0e63a89 | 373 | xt = EMG_tri.read(); //EMG meten van ticeps |
JKleinRot | 15:3ebd0e666a9c | 374 | |
JKleinRot | 17:c694a0e63a89 | 375 | filter_triceps(); //Voer acties uit om EMG signaal van de triceps te meten |
JKleinRot | 15:3ebd0e666a9c | 376 | |
JKleinRot | 19:38c9d177b6ee | 377 | if(int(triceps_kalibratie.read()) == 0) { //Wanneer de timer nog geen twee seconden heeft gemeten |
JKleinRot | 17:c694a0e63a89 | 378 | lcd.locate(0,0); //Zet de tekst op de eerste regel |
JKleinRot | 17:c694a0e63a89 | 379 | lcd.printf(" 5 "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 380 | pc.printf("4"); //Controle naar pc sturen |
JKleinRot | 17:c694a0e63a89 | 381 | } |
JKleinRot | 19:38c9d177b6ee | 382 | if(int(triceps_kalibratie.read()) == 1) { //Wanneer de timer nog geen twee seconden heeft gemeten |
JKleinRot | 17:c694a0e63a89 | 383 | lcd.locate(0,0); //Zet de tekst op de eerste regel |
JKleinRot | 17:c694a0e63a89 | 384 | lcd.printf(" 4 "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 385 | pc.printf("3"); //Controle naar pc sturen |
JKleinRot | 15:3ebd0e666a9c | 386 | } |
JKleinRot | 19:38c9d177b6ee | 387 | if(int(triceps_kalibratie.read()) == 2) { //Wanneer de timer nog geen drie seconden heeft gemeten |
JKleinRot | 17:c694a0e63a89 | 388 | lcd.locate(0,0); //Zet de tekst op de eerste regel |
JKleinRot | 17:c694a0e63a89 | 389 | lcd.printf(" 3 "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 390 | pc.printf("2"); //Controle naar pc sturen |
JKleinRot | 15:3ebd0e666a9c | 391 | } |
JKleinRot | 19:38c9d177b6ee | 392 | if(int(triceps_kalibratie.read()) == 3) { //Wanneer de timer nog geen vier seconden heeft gemeten |
JKleinRot | 17:c694a0e63a89 | 393 | lcd.locate(0,0); //Zet de tekst op de eerste regel |
JKleinRot | 17:c694a0e63a89 | 394 | lcd.printf(" 2 "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 395 | pc.printf("1"); //Controle naar pc sturen |
JKleinRot | 15:3ebd0e666a9c | 396 | } |
JKleinRot | 19:38c9d177b6ee | 397 | if(int(triceps_kalibratie.read()) == 4) { //Wanneer de timer nog geen vijf seconden heeft gemeten |
JKleinRot | 17:c694a0e63a89 | 398 | lcd.locate(0,0); //Zet de tekst op de eerste regel |
JKleinRot | 17:c694a0e63a89 | 399 | lcd.printf(" 1 "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 400 | pc.printf("1"); //Controle naar pc sturen |
JKleinRot | 14:e1816efa712d | 401 | } |
JKleinRot | 15:3ebd0e666a9c | 402 | |
JKleinRot | 21:f4e9f6c28de1 | 403 | |
JKleinRot | 21:f4e9f6c28de1 | 404 | if(xtf >= xtfmax) { //Als de gefilterde EMG waarde groter is dan xtfmax |
JKleinRot | 21:f4e9f6c28de1 | 405 | xtfmax = xtf; //Dan wordt deze gefilterde EMG waarde de nieuwe xtfmax |
JKleinRot | 21:f4e9f6c28de1 | 406 | } |
JKleinRot | 10:52b22bff409a | 407 | } |
JKleinRot | 19:38c9d177b6ee | 408 | |
JKleinRot | 27:5ac1fc1005d7 | 409 | xtt = xtfmax * 0.80; //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten |
JKleinRot | 25:71e52c56be13 | 410 | pc.printf("maximale triceps %f", xtfmax); //Maximale gefilterde EMG waarde naar pc sturen |
JKleinRot | 17:c694a0e63a89 | 411 | pc.printf("threshold is %f\n\r", xtt); //Thresholdwaarde naar pc sturen |
JKleinRot | 17:c694a0e63a89 | 412 | state = START; //Gaat door naar het slaan, kalibratie nu afgerond |
JKleinRot | 17:c694a0e63a89 | 413 | break; //Stopt alle acties in deze case |
JKleinRot | 10:52b22bff409a | 414 | } |
JKleinRot | 15:3ebd0e666a9c | 415 | |
JKleinRot | 25:71e52c56be13 | 416 | case START: { //Eerste keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 417 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 17:c694a0e63a89 | 418 | lcd.printf(" START "); //Tekst op LCD scherm |
JKleinRot | 19:38c9d177b6ee | 419 | |
JKleinRot | 17:c694a0e63a89 | 420 | pc.printf("START\n\r"); //Controle naar pc sturen |
JKleinRot | 19:38c9d177b6ee | 421 | |
JKleinRot | 27:5ac1fc1005d7 | 422 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 27:5ac1fc1005d7 | 423 | |
JKleinRot | 27:5ac1fc1005d7 | 424 | while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten |
JKleinRot | 27:5ac1fc1005d7 | 425 | 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 | 27:5ac1fc1005d7 | 426 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 27:5ac1fc1005d7 | 427 | |
JKleinRot | 27:5ac1fc1005d7 | 428 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 27:5ac1fc1005d7 | 429 | } |
JKleinRot | 27:5ac1fc1005d7 | 430 | |
JKleinRot | 19:38c9d177b6ee | 431 | while(state == START) { |
JKleinRot | 17:c694a0e63a89 | 432 | 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 | 433 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 15:3ebd0e666a9c | 434 | |
JKleinRot | 17:c694a0e63a89 | 435 | xb = EMG_bi.read(); //EMG signaal biceps uitlezen |
JKleinRot | 17:c694a0e63a89 | 436 | filter_biceps(); //EMG signaal biceps filteren |
JKleinRot | 17:c694a0e63a89 | 437 | xt = EMG_tri.read(); //EMG signaal triceps uitlezen |
JKleinRot | 17:c694a0e63a89 | 438 | filter_triceps(); //EMG signaal triceps filteren |
JKleinRot | 15:3ebd0e666a9c | 439 | |
JKleinRot | 25:71e52c56be13 | 440 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 17:c694a0e63a89 | 441 | state = B; //Ga door naar state B |
JKleinRot | 15:3ebd0e666a9c | 442 | } |
JKleinRot | 25:71e52c56be13 | 443 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 17:c694a0e63a89 | 444 | state = T; //Ga door naar state T |
JKleinRot | 15:3ebd0e666a9c | 445 | } |
JKleinRot | 15:3ebd0e666a9c | 446 | } |
JKleinRot | 17:c694a0e63a89 | 447 | break; //Stopt met de acties in deze case |
JKleinRot | 16:c34c5d9dfe1a | 448 | } |
JKleinRot | 19:38c9d177b6ee | 449 | |
JKleinRot | 25:71e52c56be13 | 450 | case B: { //Tweede keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 451 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 19:38c9d177b6ee | 452 | lcd.printf(" B "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 453 | pc.printf("B\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 454 | |
JKleinRot | 27:5ac1fc1005d7 | 455 | EMG.reset(); |
JKleinRot | 25:71e52c56be13 | 456 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 457 | |
JKleinRot | 25:71e52c56be13 | 458 | while(EMG.read() <= 2) { //Timer nog geen drie seconden gemeten |
JKleinRot | 22:838a17065bc7 | 459 | 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 | 22:838a17065bc7 | 460 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 461 | |
JKleinRot | 25:71e52c56be13 | 462 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 463 | } |
JKleinRot | 23:5267c928ae2b | 464 | |
JKleinRot | 19:38c9d177b6ee | 465 | while(state == B) { |
JKleinRot | 19:38c9d177b6ee | 466 | 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 | 467 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 468 | |
JKleinRot | 25:71e52c56be13 | 469 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 23:5267c928ae2b | 470 | |
JKleinRot | 25:71e52c56be13 | 471 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 472 | state = BB; //Ga door naar state BB |
JKleinRot | 19:38c9d177b6ee | 473 | } |
JKleinRot | 21:f4e9f6c28de1 | 474 | if(xtf>= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 475 | state = BT; //Ga door naar state BT |
JKleinRot | 19:38c9d177b6ee | 476 | } |
JKleinRot | 19:38c9d177b6ee | 477 | } |
JKleinRot | 19:38c9d177b6ee | 478 | break; //Stop met alle acties in deze case |
JKleinRot | 19:38c9d177b6ee | 479 | } |
JKleinRot | 19:38c9d177b6ee | 480 | |
JKleinRot | 25:71e52c56be13 | 481 | case T: { //Tweede keuze maken voor doel |
JKleinRot | 19:38c9d177b6ee | 482 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 19:38c9d177b6ee | 483 | lcd.printf(" T "); //Tekst op LCD scherm |
JKleinRot | 19:38c9d177b6ee | 484 | pc.printf("T\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 485 | |
JKleinRot | 27:5ac1fc1005d7 | 486 | EMG.reset(); |
JKleinRot | 25:71e52c56be13 | 487 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 488 | |
JKleinRot | 26:438a498e5526 | 489 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 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 | 22:838a17065bc7 | 491 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 492 | |
JKleinRot | 25:71e52c56be13 | 493 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 494 | } |
JKleinRot | 19:38c9d177b6ee | 495 | |
JKleinRot | 19:38c9d177b6ee | 496 | while(state == T) { |
JKleinRot | 17:c694a0e63a89 | 497 | 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 | 498 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 15:3ebd0e666a9c | 499 | |
JKleinRot | 25:71e52c56be13 | 500 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 16:c34c5d9dfe1a | 501 | |
JKleinRot | 25:71e52c56be13 | 502 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 503 | state = TB; //Ga door naar state TB |
JKleinRot | 16:c34c5d9dfe1a | 504 | } |
JKleinRot | 25:71e52c56be13 | 505 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 506 | state = TT; //Ga door naar state TT |
JKleinRot | 16:c34c5d9dfe1a | 507 | } |
JKleinRot | 16:c34c5d9dfe1a | 508 | } |
JKleinRot | 17:c694a0e63a89 | 509 | break; //Stop met alle acties in deze case |
JKleinRot | 16:c34c5d9dfe1a | 510 | } |
JKleinRot | 16:c34c5d9dfe1a | 511 | |
JKleinRot | 25:71e52c56be13 | 512 | case BB: { //Derde keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 513 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 19:38c9d177b6ee | 514 | lcd.printf(" BB "); //Tekst op LCD scherm |
JKleinRot | 19:38c9d177b6ee | 515 | pc.printf("BB\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 516 | |
JKleinRot | 25:71e52c56be13 | 517 | EMG.reset(); //Timer resetten |
JKleinRot | 25:71e52c56be13 | 518 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 519 | |
JKleinRot | 26:438a498e5526 | 520 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 521 | 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 | 22:838a17065bc7 | 522 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 523 | |
JKleinRot | 25:71e52c56be13 | 524 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 525 | } |
JKleinRot | 16:c34c5d9dfe1a | 526 | |
JKleinRot | 19:38c9d177b6ee | 527 | while(state == BB) { |
JKleinRot | 17:c694a0e63a89 | 528 | 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 | 529 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 16:c34c5d9dfe1a | 530 | |
JKleinRot | 25:71e52c56be13 | 531 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 16:c34c5d9dfe1a | 532 | |
JKleinRot | 25:71e52c56be13 | 533 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 534 | state = BBB; //Ga door naar state BBB |
JKleinRot | 16:c34c5d9dfe1a | 535 | } |
JKleinRot | 25:71e52c56be13 | 536 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 537 | state = BBT; //Ga door naar state BBT |
JKleinRot | 16:c34c5d9dfe1a | 538 | } |
JKleinRot | 16:c34c5d9dfe1a | 539 | } |
JKleinRot | 17:c694a0e63a89 | 540 | break; //Stop met alle acties in deze case |
JKleinRot | 16:c34c5d9dfe1a | 541 | } |
JKleinRot | 19:38c9d177b6ee | 542 | |
JKleinRot | 25:71e52c56be13 | 543 | case BT: { //Derde keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 544 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 19:38c9d177b6ee | 545 | lcd.printf(" BT "); //Tekst op LCD scherm |
JKleinRot | 19:38c9d177b6ee | 546 | pc.printf("BT\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 547 | |
JKleinRot | 25:71e52c56be13 | 548 | EMG.reset(); //Timer resetten |
JKleinRot | 25:71e52c56be13 | 549 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 550 | |
JKleinRot | 26:438a498e5526 | 551 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 552 | 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 | 22:838a17065bc7 | 553 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 554 | |
JKleinRot | 25:71e52c56be13 | 555 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 556 | } |
JKleinRot | 15:3ebd0e666a9c | 557 | |
JKleinRot | 19:38c9d177b6ee | 558 | while(state == BT) { |
JKleinRot | 17:c694a0e63a89 | 559 | 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 | 560 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 15:3ebd0e666a9c | 561 | |
JKleinRot | 22:838a17065bc7 | 562 | EMG_meten(); |
JKleinRot | 17:c694a0e63a89 | 563 | |
JKleinRot | 25:71e52c56be13 | 564 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 565 | state = BTB; //Ga door naar state BTB |
JKleinRot | 17:c694a0e63a89 | 566 | } |
JKleinRot | 25:71e52c56be13 | 567 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 568 | state = BTT; //Ga door naar state BTT |
JKleinRot | 17:c694a0e63a89 | 569 | } |
JKleinRot | 17:c694a0e63a89 | 570 | } |
JKleinRot | 19:38c9d177b6ee | 571 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 572 | } |
JKleinRot | 19:38c9d177b6ee | 573 | |
JKleinRot | 25:71e52c56be13 | 574 | case TB: { //Derde keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 575 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 19:38c9d177b6ee | 576 | lcd.printf(" TB "); //Tekst op LCD scherm |
JKleinRot | 19:38c9d177b6ee | 577 | pc.printf("TB\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 578 | |
JKleinRot | 25:71e52c56be13 | 579 | EMG.reset(); //Timer resetten |
JKleinRot | 25:71e52c56be13 | 580 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 581 | |
JKleinRot | 26:438a498e5526 | 582 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 583 | 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 | 22:838a17065bc7 | 584 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 585 | |
JKleinRot | 25:71e52c56be13 | 586 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 587 | } |
JKleinRot | 17:c694a0e63a89 | 588 | |
JKleinRot | 19:38c9d177b6ee | 589 | while(state == TB) { |
JKleinRot | 17:c694a0e63a89 | 590 | 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 | 591 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 17:c694a0e63a89 | 592 | |
JKleinRot | 25:71e52c56be13 | 593 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 17:c694a0e63a89 | 594 | |
JKleinRot | 25:71e52c56be13 | 595 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 596 | state = TBB; //Ga door naar state TBB |
JKleinRot | 17:c694a0e63a89 | 597 | } |
JKleinRot | 25:71e52c56be13 | 598 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 599 | state = TBT; //Ga door naar state TBT |
JKleinRot | 17:c694a0e63a89 | 600 | } |
JKleinRot | 17:c694a0e63a89 | 601 | } |
JKleinRot | 19:38c9d177b6ee | 602 | break; //Stop met alle acties in deze case |
JKleinRot | 19:38c9d177b6ee | 603 | } |
JKleinRot | 17:c694a0e63a89 | 604 | |
JKleinRot | 25:71e52c56be13 | 605 | case TT: { //Derde keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 606 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 19:38c9d177b6ee | 607 | lcd.printf(" TT "); //Tekst op LCD scherm |
JKleinRot | 19:38c9d177b6ee | 608 | pc.printf("TT\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 609 | |
JKleinRot | 25:71e52c56be13 | 610 | EMG.reset(); //Timer resetten |
JKleinRot | 25:71e52c56be13 | 611 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 612 | |
JKleinRot | 26:438a498e5526 | 613 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 614 | 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 | 22:838a17065bc7 | 615 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 616 | |
JKleinRot | 25:71e52c56be13 | 617 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 618 | } |
JKleinRot | 17:c694a0e63a89 | 619 | |
JKleinRot | 19:38c9d177b6ee | 620 | while(state == TT) { |
JKleinRot | 17:c694a0e63a89 | 621 | 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 | 622 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 17:c694a0e63a89 | 623 | |
JKleinRot | 25:71e52c56be13 | 624 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 17:c694a0e63a89 | 625 | |
JKleinRot | 25:71e52c56be13 | 626 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 627 | state = TTB; //Ga door naar state TTB |
JKleinRot | 17:c694a0e63a89 | 628 | } |
JKleinRot | 25:71e52c56be13 | 629 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 630 | state = TTT; //Ga door naar state TTT |
JKleinRot | 17:c694a0e63a89 | 631 | } |
JKleinRot | 17:c694a0e63a89 | 632 | } |
JKleinRot | 19:38c9d177b6ee | 633 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 634 | } |
JKleinRot | 19:38c9d177b6ee | 635 | |
JKleinRot | 25:71e52c56be13 | 636 | case BBB: { //Vierde keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 637 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 19:38c9d177b6ee | 638 | lcd.printf(" BBB "); //Tekst op LCD scherm |
JKleinRot | 19:38c9d177b6ee | 639 | pc.printf("BBB\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 640 | |
JKleinRot | 25:71e52c56be13 | 641 | EMG.reset(); //Timer resetten |
JKleinRot | 25:71e52c56be13 | 642 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 643 | |
JKleinRot | 26:438a498e5526 | 644 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 645 | 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 | 22:838a17065bc7 | 646 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 647 | |
JKleinRot | 25:71e52c56be13 | 648 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 649 | } |
JKleinRot | 17:c694a0e63a89 | 650 | |
JKleinRot | 19:38c9d177b6ee | 651 | while(state == BBB) { |
JKleinRot | 17:c694a0e63a89 | 652 | 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 | 653 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 17:c694a0e63a89 | 654 | |
JKleinRot | 25:71e52c56be13 | 655 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 17:c694a0e63a89 | 656 | |
JKleinRot | 25:71e52c56be13 | 657 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 658 | state = BBBB; //Ga door naar state BBBB |
JKleinRot | 17:c694a0e63a89 | 659 | } |
JKleinRot | 25:71e52c56be13 | 660 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 661 | state = BBBT; //Ga door naar state BBBT |
JKleinRot | 17:c694a0e63a89 | 662 | } |
JKleinRot | 17:c694a0e63a89 | 663 | } |
JKleinRot | 19:38c9d177b6ee | 664 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 665 | } |
JKleinRot | 17:c694a0e63a89 | 666 | |
JKleinRot | 19:38c9d177b6ee | 667 | |
JKleinRot | 25:71e52c56be13 | 668 | case BBT: { //Vierde keuze maken in doel |
JKleinRot | 17:c694a0e63a89 | 669 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 19:38c9d177b6ee | 670 | lcd.printf(" BBT "); //Tekst op LCD scherm |
JKleinRot | 19:38c9d177b6ee | 671 | pc.printf("BBT\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 672 | |
JKleinRot | 25:71e52c56be13 | 673 | EMG.reset(); //Timer resetten |
JKleinRot | 25:71e52c56be13 | 674 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 675 | |
JKleinRot | 26:438a498e5526 | 676 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 677 | 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 | 22:838a17065bc7 | 678 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 679 | |
JKleinRot | 25:71e52c56be13 | 680 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 681 | } |
JKleinRot | 17:c694a0e63a89 | 682 | |
JKleinRot | 19:38c9d177b6ee | 683 | while(state == BBT) { |
JKleinRot | 17:c694a0e63a89 | 684 | 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 | 685 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 17:c694a0e63a89 | 686 | |
JKleinRot | 25:71e52c56be13 | 687 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 17:c694a0e63a89 | 688 | |
JKleinRot | 25:71e52c56be13 | 689 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 690 | state = BBTB; //Ga door naar state BBTB |
JKleinRot | 17:c694a0e63a89 | 691 | } |
JKleinRot | 25:71e52c56be13 | 692 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 693 | state = BBTT; //Ga door naar state BBTT |
JKleinRot | 17:c694a0e63a89 | 694 | } |
JKleinRot | 17:c694a0e63a89 | 695 | } |
JKleinRot | 19:38c9d177b6ee | 696 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 697 | } |
JKleinRot | 19:38c9d177b6ee | 698 | |
JKleinRot | 25:71e52c56be13 | 699 | case BTB: { //Vierde keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 700 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 19:38c9d177b6ee | 701 | lcd.printf(" BTB "); //Tekst op LCD scherm |
JKleinRot | 19:38c9d177b6ee | 702 | pc.printf("BTB\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 703 | |
JKleinRot | 25:71e52c56be13 | 704 | EMG.reset(); //Timer resetten |
JKleinRot | 25:71e52c56be13 | 705 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 706 | |
JKleinRot | 26:438a498e5526 | 707 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 708 | 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 | 22:838a17065bc7 | 709 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 710 | |
JKleinRot | 25:71e52c56be13 | 711 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 712 | } |
JKleinRot | 17:c694a0e63a89 | 713 | |
JKleinRot | 19:38c9d177b6ee | 714 | while(state == BTB) { |
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 | 25:71e52c56be13 | 718 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 17:c694a0e63a89 | 719 | |
JKleinRot | 25:71e52c56be13 | 720 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 721 | state = BTBB; //Ga door naar state BTBB |
JKleinRot | 17:c694a0e63a89 | 722 | } |
JKleinRot | 25:71e52c56be13 | 723 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 724 | state = BTBT; //Ga door naar state BTBT |
JKleinRot | 17:c694a0e63a89 | 725 | } |
JKleinRot | 17:c694a0e63a89 | 726 | } |
JKleinRot | 19:38c9d177b6ee | 727 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 728 | } |
JKleinRot | 19:38c9d177b6ee | 729 | |
JKleinRot | 25:71e52c56be13 | 730 | case BTT: { //Vierde keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 731 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 19:38c9d177b6ee | 732 | lcd.printf(" BTT "); //Tekst op LCD scherm |
JKleinRot | 19:38c9d177b6ee | 733 | pc.printf("BTT\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 734 | |
JKleinRot | 25:71e52c56be13 | 735 | EMG.reset(); //Timer resetten |
JKleinRot | 25:71e52c56be13 | 736 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 737 | |
JKleinRot | 26:438a498e5526 | 738 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 739 | 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 | 22:838a17065bc7 | 740 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 741 | |
JKleinRot | 25:71e52c56be13 | 742 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 743 | } |
JKleinRot | 17:c694a0e63a89 | 744 | |
JKleinRot | 19:38c9d177b6ee | 745 | while(state == BTT) { |
JKleinRot | 17:c694a0e63a89 | 746 | 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 | 747 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 17:c694a0e63a89 | 748 | |
JKleinRot | 25:71e52c56be13 | 749 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 17:c694a0e63a89 | 750 | |
JKleinRot | 25:71e52c56be13 | 751 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 752 | state = BTTB; //Ga door naar state BTTB |
JKleinRot | 17:c694a0e63a89 | 753 | } |
JKleinRot | 25:71e52c56be13 | 754 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 755 | state = BTTT; //Ga door naar state BTTT |
JKleinRot | 17:c694a0e63a89 | 756 | } |
JKleinRot | 17:c694a0e63a89 | 757 | } |
JKleinRot | 19:38c9d177b6ee | 758 | break; //Stop met alle acties in deze case |
JKleinRot | 19:38c9d177b6ee | 759 | } |
JKleinRot | 19:38c9d177b6ee | 760 | |
JKleinRot | 25:71e52c56be13 | 761 | case TBB: { //Vierde keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 762 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 19:38c9d177b6ee | 763 | lcd.printf(" TBB "); //Tekst op LCD scherm |
JKleinRot | 19:38c9d177b6ee | 764 | pc.printf("TBB\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 765 | |
JKleinRot | 25:71e52c56be13 | 766 | EMG.reset(); //Timer resetten |
JKleinRot | 25:71e52c56be13 | 767 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 768 | |
JKleinRot | 26:438a498e5526 | 769 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 770 | 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 | 22:838a17065bc7 | 771 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 772 | |
JKleinRot | 25:71e52c56be13 | 773 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 774 | } |
JKleinRot | 17:c694a0e63a89 | 775 | |
JKleinRot | 19:38c9d177b6ee | 776 | while(state == TBB) { |
JKleinRot | 17:c694a0e63a89 | 777 | 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 | 778 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 17:c694a0e63a89 | 779 | |
JKleinRot | 25:71e52c56be13 | 780 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 17:c694a0e63a89 | 781 | |
JKleinRot | 25:71e52c56be13 | 782 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 783 | state = TBBB; //Ga door naar state TBBB |
JKleinRot | 17:c694a0e63a89 | 784 | } |
JKleinRot | 25:71e52c56be13 | 785 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 786 | state = TBBT; //Ga door naar state TBBT |
JKleinRot | 17:c694a0e63a89 | 787 | } |
JKleinRot | 17:c694a0e63a89 | 788 | } |
JKleinRot | 19:38c9d177b6ee | 789 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 790 | } |
JKleinRot | 19:38c9d177b6ee | 791 | |
JKleinRot | 25:71e52c56be13 | 792 | case TBT: { //Vierde keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 793 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 19:38c9d177b6ee | 794 | lcd.printf(" TBT "); //Tekst op LCD scherm |
JKleinRot | 19:38c9d177b6ee | 795 | pc.printf("TBT\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 796 | |
JKleinRot | 25:71e52c56be13 | 797 | EMG.reset(); //Timer resetten |
JKleinRot | 25:71e52c56be13 | 798 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 799 | |
JKleinRot | 26:438a498e5526 | 800 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 801 | 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 | 22:838a17065bc7 | 802 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 803 | |
JKleinRot | 25:71e52c56be13 | 804 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 805 | } |
JKleinRot | 17:c694a0e63a89 | 806 | |
JKleinRot | 19:38c9d177b6ee | 807 | while(state == TBT) { |
JKleinRot | 17:c694a0e63a89 | 808 | 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 | 809 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 17:c694a0e63a89 | 810 | |
JKleinRot | 25:71e52c56be13 | 811 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 17:c694a0e63a89 | 812 | |
JKleinRot | 26:438a498e5526 | 813 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 814 | state = TBTB; //Ga door naar state TBTB |
JKleinRot | 17:c694a0e63a89 | 815 | } |
JKleinRot | 26:438a498e5526 | 816 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 19:38c9d177b6ee | 817 | state = TBTT; //Ga door naar state TBTT |
JKleinRot | 17:c694a0e63a89 | 818 | } |
JKleinRot | 17:c694a0e63a89 | 819 | } |
JKleinRot | 19:38c9d177b6ee | 820 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 821 | } |
JKleinRot | 19:38c9d177b6ee | 822 | |
JKleinRot | 25:71e52c56be13 | 823 | case TTB: { //Vierde keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 824 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 22:838a17065bc7 | 825 | lcd.printf(" TTB "); //Tekst op LCD scherm |
JKleinRot | 22:838a17065bc7 | 826 | pc.printf("TTB\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 827 | |
JKleinRot | 25:71e52c56be13 | 828 | EMG.reset(); //Timer resetten |
JKleinRot | 25:71e52c56be13 | 829 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 830 | |
JKleinRot | 26:438a498e5526 | 831 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 832 | 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 | 22:838a17065bc7 | 833 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 834 | |
JKleinRot | 25:71e52c56be13 | 835 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 836 | } |
JKleinRot | 17:c694a0e63a89 | 837 | |
JKleinRot | 22:838a17065bc7 | 838 | while(state == TTB) { |
JKleinRot | 17:c694a0e63a89 | 839 | 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 | 840 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 17:c694a0e63a89 | 841 | |
JKleinRot | 25:71e52c56be13 | 842 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 17:c694a0e63a89 | 843 | |
JKleinRot | 26:438a498e5526 | 844 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 17:c694a0e63a89 | 845 | state = TTBB; //Ga door naar state TTBB |
JKleinRot | 17:c694a0e63a89 | 846 | } |
JKleinRot | 26:438a498e5526 | 847 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 17:c694a0e63a89 | 848 | state = TTBT; //Ga door naar state TTBT |
JKleinRot | 17:c694a0e63a89 | 849 | } |
JKleinRot | 17:c694a0e63a89 | 850 | } |
JKleinRot | 19:38c9d177b6ee | 851 | break; //Stop met alle acties in deze case |
JKleinRot | 19:38c9d177b6ee | 852 | } |
JKleinRot | 19:38c9d177b6ee | 853 | |
JKleinRot | 25:71e52c56be13 | 854 | case TTT: { //Vierde keuze maken voor doel |
JKleinRot | 17:c694a0e63a89 | 855 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 17:c694a0e63a89 | 856 | lcd.printf(" TTT "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 857 | pc.printf("TTT\n\r"); //Controle naar pc sturen |
JKleinRot | 23:5267c928ae2b | 858 | |
JKleinRot | 25:71e52c56be13 | 859 | EMG.reset(); //Timer resetten |
JKleinRot | 25:71e52c56be13 | 860 | EMG.start(); //Timer aanzetten voor EMG meten |
JKleinRot | 23:5267c928ae2b | 861 | |
JKleinRot | 26:438a498e5526 | 862 | while(EMG.read() <= 2) { //Timer nog geen twee seconden gemeten |
JKleinRot | 22:838a17065bc7 | 863 | 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 | 22:838a17065bc7 | 864 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 23:5267c928ae2b | 865 | |
JKleinRot | 25:71e52c56be13 | 866 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 22:838a17065bc7 | 867 | } |
JKleinRot | 17:c694a0e63a89 | 868 | |
JKleinRot | 19:38c9d177b6ee | 869 | while(state == TTT) { |
JKleinRot | 17:c694a0e63a89 | 870 | 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 | 871 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 17:c694a0e63a89 | 872 | |
JKleinRot | 25:71e52c56be13 | 873 | EMG_meten(); //EMG meten van biceps en triceps |
JKleinRot | 17:c694a0e63a89 | 874 | |
JKleinRot | 26:438a498e5526 | 875 | if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold |
JKleinRot | 17:c694a0e63a89 | 876 | state = TTTB; //Ga door naar state TTTB |
JKleinRot | 17:c694a0e63a89 | 877 | } |
JKleinRot | 26:438a498e5526 | 878 | if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold |
JKleinRot | 17:c694a0e63a89 | 879 | state = TTTT; //Ga door naar state TTTT |
JKleinRot | 17:c694a0e63a89 | 880 | } |
JKleinRot | 17:c694a0e63a89 | 881 | } |
JKleinRot | 19:38c9d177b6ee | 882 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 883 | } |
JKleinRot | 19:38c9d177b6ee | 884 | |
JKleinRot | 25:71e52c56be13 | 885 | case BBBB: { //Motoraansturing voor doel rechtsonder |
JKleinRot | 17:c694a0e63a89 | 886 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 17:c694a0e63a89 | 887 | lcd.printf(" BBBB "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 888 | pc.printf("BBBB\n\r"); //Controle naar pc sturen |
JKleinRot | 17:c694a0e63a89 | 889 | |
JKleinRot | 19:38c9d177b6ee | 890 | while(state == BBBB) { |
JKleinRot | 25:71e52c56be13 | 891 | 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 | 25:71e52c56be13 | 892 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 25:71e52c56be13 | 893 | |
JKleinRot | 32:80fc2de5b511 | 894 | while(puls_motor_arm1.getPosition() > -107) { |
JKleinRot | 32:80fc2de5b511 | 895 | referentie_arm1 = referentie_arm1 - 287.0/200.0; //Referentie arm 1 loopt af in een seconden naar de gewenste waarde |
JKleinRot | 26:438a498e5526 | 896 | pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 897 | pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 898 | pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen |
JKleinRot | 25:71e52c56be13 | 899 | } |
JKleinRot | 29:f95f0cc84349 | 900 | |
JKleinRot | 32:80fc2de5b511 | 901 | if(puls_motor_arm1.getPosition() <= -107) { |
JKleinRot | 32:80fc2de5b511 | 902 | referentie_arm1 = -107; |
JKleinRot | 27:5ac1fc1005d7 | 903 | } |
JKleinRot | 25:71e52c56be13 | 904 | |
JKleinRot | 25:71e52c56be13 | 905 | while(puls_motor_arm2.getPosition() < 211) { |
JKleinRot | 26:438a498e5526 | 906 | referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar de gewenste waarde |
JKleinRot | 26:438a498e5526 | 907 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 908 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 909 | pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 25:71e52c56be13 | 910 | } |
JKleinRot | 29:f95f0cc84349 | 911 | ticker_motor_arm2_pid.detach(); |
JKleinRot | 32:80fc2de5b511 | 912 | pwm_to_motor_arm2 = 0.8; |
JKleinRot | 29:f95f0cc84349 | 913 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 29:f95f0cc84349 | 914 | dir_motor_arm2.write(0); |
JKleinRot | 32:80fc2de5b511 | 915 | while(puls_motor_arm2.getPosition() > -306) { |
JKleinRot | 25:71e52c56be13 | 916 | |
JKleinRot | 26:438a498e5526 | 917 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 28:e2f5cee5e73b | 918 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 919 | pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 26:438a498e5526 | 920 | pc.printf("t is %f\n\r", t); //T naar pc sturen |
JKleinRot | 25:71e52c56be13 | 921 | } |
JKleinRot | 29:f95f0cc84349 | 922 | pwm_to_motor_arm2 = 0.5; |
JKleinRot | 29:f95f0cc84349 | 923 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 29:f95f0cc84349 | 924 | dir_motor_arm2.write(1); |
JKleinRot | 29:f95f0cc84349 | 925 | while(puls_motor_arm2.getPosition() <= 211) { |
JKleinRot | 25:71e52c56be13 | 926 | |
JKleinRot | 29:f95f0cc84349 | 927 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 29:f95f0cc84349 | 928 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 29:f95f0cc84349 | 929 | pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 29:f95f0cc84349 | 930 | } |
JKleinRot | 29:f95f0cc84349 | 931 | ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); |
JKleinRot | 29:f95f0cc84349 | 932 | pc.printf("staat stil"); |
JKleinRot | 25:71e52c56be13 | 933 | |
JKleinRot | 29:f95f0cc84349 | 934 | state = THUISPOSITIE_RECHTS; //Door naar de volgende state |
JKleinRot | 29:f95f0cc84349 | 935 | |
JKleinRot | 19:38c9d177b6ee | 936 | } |
JKleinRot | 29:f95f0cc84349 | 937 | |
JKleinRot | 29:f95f0cc84349 | 938 | |
JKleinRot | 19:38c9d177b6ee | 939 | break; //Stop met alle acties in deze case |
JKleinRot | 19:38c9d177b6ee | 940 | } |
JKleinRot | 19:38c9d177b6ee | 941 | |
JKleinRot | 25:71e52c56be13 | 942 | case BBBT: { //Geen motoraansturing |
JKleinRot | 17:c694a0e63a89 | 943 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 17:c694a0e63a89 | 944 | lcd.printf(" BBBT "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 945 | pc.printf("BBBT\n\r"); //Controle naar pc sturen |
JKleinRot | 26:438a498e5526 | 946 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 947 | lcd.cls(); //LCD scherm leegmaken |
JKleinRot | 17:c694a0e63a89 | 948 | |
JKleinRot | 19:38c9d177b6ee | 949 | while(state == BBBT) { |
JKleinRot | 26:438a498e5526 | 950 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 26:438a498e5526 | 951 | lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm |
JKleinRot | 26:438a498e5526 | 952 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 953 | state = WACHT; //Door naar de volgende state |
JKleinRot | 19:38c9d177b6ee | 954 | } |
JKleinRot | 19:38c9d177b6ee | 955 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 956 | } |
JKleinRot | 19:38c9d177b6ee | 957 | |
JKleinRot | 25:71e52c56be13 | 958 | case BBTB: { //Motoraansturing voor doel rechtsmidden |
JKleinRot | 17:c694a0e63a89 | 959 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 17:c694a0e63a89 | 960 | lcd.printf(" BBTB "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 961 | pc.printf("BBTB\n\r"); //Controle naar pc sturen |
JKleinRot | 17:c694a0e63a89 | 962 | |
JKleinRot | 19:38c9d177b6ee | 963 | while(state == BBTB) { |
JKleinRot | 25:71e52c56be13 | 964 | 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 | 25:71e52c56be13 | 965 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 25:71e52c56be13 | 966 | |
JKleinRot | 32:80fc2de5b511 | 967 | while(puls_motor_arm1.getPosition() > -107) { |
JKleinRot | 32:80fc2de5b511 | 968 | referentie_arm1 = referentie_arm1 - 287.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde |
JKleinRot | 26:438a498e5526 | 969 | pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 970 | pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 971 | pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen |
JKleinRot | 25:71e52c56be13 | 972 | } |
JKleinRot | 29:f95f0cc84349 | 973 | |
JKleinRot | 32:80fc2de5b511 | 974 | if(puls_motor_arm1.getPosition() <= -107) { |
JKleinRot | 32:80fc2de5b511 | 975 | referentie_arm1 = -107; |
JKleinRot | 27:5ac1fc1005d7 | 976 | } |
JKleinRot | 25:71e52c56be13 | 977 | |
JKleinRot | 25:71e52c56be13 | 978 | while(puls_motor_arm2.getPosition() < 211) { |
JKleinRot | 26:438a498e5526 | 979 | referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde |
JKleinRot | 26:438a498e5526 | 980 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 981 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 982 | pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 25:71e52c56be13 | 983 | } |
JKleinRot | 30:f79cf70e2917 | 984 | ticker_motor_arm2_pid.detach(); |
JKleinRot | 32:80fc2de5b511 | 985 | pwm_to_motor_arm2 = 0.9; |
JKleinRot | 30:f79cf70e2917 | 986 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 30:f79cf70e2917 | 987 | dir_motor_arm2.write(0); |
JKleinRot | 32:80fc2de5b511 | 988 | while(puls_motor_arm2.getPosition() > -306) { |
JKleinRot | 25:71e52c56be13 | 989 | |
JKleinRot | 26:438a498e5526 | 990 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 28:e2f5cee5e73b | 991 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 992 | pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 26:438a498e5526 | 993 | pc.printf("t is %f\n\r", t); //T naar pc sturen |
JKleinRot | 25:71e52c56be13 | 994 | } |
JKleinRot | 30:f79cf70e2917 | 995 | pwm_to_motor_arm2 = 0.5; |
JKleinRot | 30:f79cf70e2917 | 996 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 30:f79cf70e2917 | 997 | dir_motor_arm2.write(1); |
JKleinRot | 30:f79cf70e2917 | 998 | while(puls_motor_arm2.getPosition() <= 211) { |
JKleinRot | 25:71e52c56be13 | 999 | |
JKleinRot | 30:f79cf70e2917 | 1000 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1001 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1002 | pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1003 | } |
JKleinRot | 30:f79cf70e2917 | 1004 | ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); |
JKleinRot | 30:f79cf70e2917 | 1005 | state = THUISPOSITIE_RECHTS; //Door naar de volgende state |
JKleinRot | 25:71e52c56be13 | 1006 | |
JKleinRot | 30:f79cf70e2917 | 1007 | |
JKleinRot | 19:38c9d177b6ee | 1008 | } |
JKleinRot | 19:38c9d177b6ee | 1009 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 1010 | } |
JKleinRot | 19:38c9d177b6ee | 1011 | |
JKleinRot | 25:71e52c56be13 | 1012 | case BBTT: { //Motoraansturing voor doel rechtsboven |
JKleinRot | 17:c694a0e63a89 | 1013 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 17:c694a0e63a89 | 1014 | lcd.printf(" BBTT "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 1015 | pc.printf("BBTT\n\r"); //Controle naar pc sturen |
JKleinRot | 17:c694a0e63a89 | 1016 | |
JKleinRot | 19:38c9d177b6ee | 1017 | while(state == BBTT) { |
JKleinRot | 25:71e52c56be13 | 1018 | 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 | 25:71e52c56be13 | 1019 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 25:71e52c56be13 | 1020 | |
JKleinRot | 32:80fc2de5b511 | 1021 | while(puls_motor_arm1.getPosition() > -107) { |
JKleinRot | 32:80fc2de5b511 | 1022 | referentie_arm1 = referentie_arm1 - 287.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde |
JKleinRot | 26:438a498e5526 | 1023 | pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1024 | pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1025 | pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1026 | } |
JKleinRot | 29:f95f0cc84349 | 1027 | |
JKleinRot | 32:80fc2de5b511 | 1028 | if(puls_motor_arm1.getPosition() <= -107) { |
JKleinRot | 32:80fc2de5b511 | 1029 | referentie_arm1 = -107; |
JKleinRot | 27:5ac1fc1005d7 | 1030 | } |
JKleinRot | 25:71e52c56be13 | 1031 | |
JKleinRot | 25:71e52c56be13 | 1032 | while(puls_motor_arm2.getPosition() < 211) { |
JKleinRot | 26:438a498e5526 | 1033 | referentie_arm2 = referentie_arm2 + 88.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde |
JKleinRot | 26:438a498e5526 | 1034 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1035 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1036 | pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1037 | } |
JKleinRot | 30:f79cf70e2917 | 1038 | ticker_motor_arm2_pid.detach(); |
JKleinRot | 30:f79cf70e2917 | 1039 | pwm_to_motor_arm2 = 1; |
JKleinRot | 30:f79cf70e2917 | 1040 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 30:f79cf70e2917 | 1041 | dir_motor_arm2.write(0); |
JKleinRot | 32:80fc2de5b511 | 1042 | while(puls_motor_arm2.getPosition() > -306) { |
JKleinRot | 25:71e52c56be13 | 1043 | |
JKleinRot | 26:438a498e5526 | 1044 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 28:e2f5cee5e73b | 1045 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1046 | pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 26:438a498e5526 | 1047 | pc.printf("t is %f\n\r", t); //T naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1048 | } |
JKleinRot | 30:f79cf70e2917 | 1049 | pwm_to_motor_arm2 = 0.5; |
JKleinRot | 30:f79cf70e2917 | 1050 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 30:f79cf70e2917 | 1051 | dir_motor_arm2.write(1); |
JKleinRot | 30:f79cf70e2917 | 1052 | while(puls_motor_arm2.getPosition() <= 211) { |
JKleinRot | 25:71e52c56be13 | 1053 | |
JKleinRot | 30:f79cf70e2917 | 1054 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1055 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1056 | pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1057 | } |
JKleinRot | 30:f79cf70e2917 | 1058 | ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); |
JKleinRot | 30:f79cf70e2917 | 1059 | pc.printf("staat stil\n\r"); //Staat stil naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1060 | state = THUISPOSITIE_RECHTS; //Door naar de volgende state |
JKleinRot | 30:f79cf70e2917 | 1061 | } |
JKleinRot | 25:71e52c56be13 | 1062 | |
JKleinRot | 19:38c9d177b6ee | 1063 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 1064 | } |
JKleinRot | 19:38c9d177b6ee | 1065 | |
JKleinRot | 25:71e52c56be13 | 1066 | case BTBB: { //Geen motoraansturing |
JKleinRot | 17:c694a0e63a89 | 1067 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 17:c694a0e63a89 | 1068 | lcd.printf(" BTBB "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 1069 | pc.printf("BTBB\n\r"); //Controle naar pc sturen |
JKleinRot | 26:438a498e5526 | 1070 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 1071 | lcd.cls(); //LCD scherm leegmaken |
JKleinRot | 17:c694a0e63a89 | 1072 | |
JKleinRot | 19:38c9d177b6ee | 1073 | while(state == BTBB) { |
JKleinRot | 26:438a498e5526 | 1074 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 26:438a498e5526 | 1075 | lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm |
JKleinRot | 26:438a498e5526 | 1076 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 1077 | state = WACHT; //Door naar de volgende state |
JKleinRot | 19:38c9d177b6ee | 1078 | } |
JKleinRot | 19:38c9d177b6ee | 1079 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 1080 | } |
JKleinRot | 19:38c9d177b6ee | 1081 | |
JKleinRot | 25:71e52c56be13 | 1082 | case BTBT: { //Geen motoraansturing |
JKleinRot | 17:c694a0e63a89 | 1083 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 17:c694a0e63a89 | 1084 | lcd.printf(" BTBT "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 1085 | pc.printf("BTBT\n\r"); //Controle naar pc sturen |
JKleinRot | 26:438a498e5526 | 1086 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 1087 | lcd.cls(); //LCD scherm leegmaken |
JKleinRot | 17:c694a0e63a89 | 1088 | |
JKleinRot | 19:38c9d177b6ee | 1089 | while(state == BTBT) { |
JKleinRot | 26:438a498e5526 | 1090 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 26:438a498e5526 | 1091 | lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm |
JKleinRot | 26:438a498e5526 | 1092 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 1093 | state = WACHT; //Door naar de volgende state |
JKleinRot | 19:38c9d177b6ee | 1094 | } |
JKleinRot | 19:38c9d177b6ee | 1095 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 1096 | } |
JKleinRot | 19:38c9d177b6ee | 1097 | |
JKleinRot | 25:71e52c56be13 | 1098 | case BTTB: { //Geen motoraansturing |
JKleinRot | 17:c694a0e63a89 | 1099 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 17:c694a0e63a89 | 1100 | lcd.printf(" BTTB "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 1101 | pc.printf("BTTB\n\r"); //Controle naar pc sturen |
JKleinRot | 26:438a498e5526 | 1102 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 1103 | lcd.cls(); //LCD scherm leegmaken |
JKleinRot | 17:c694a0e63a89 | 1104 | |
JKleinRot | 19:38c9d177b6ee | 1105 | while(state == BTTB) { |
JKleinRot | 26:438a498e5526 | 1106 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 26:438a498e5526 | 1107 | lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm |
JKleinRot | 26:438a498e5526 | 1108 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 1109 | state = WACHT; //Door naar de volgende state |
JKleinRot | 19:38c9d177b6ee | 1110 | } |
JKleinRot | 19:38c9d177b6ee | 1111 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 1112 | } |
JKleinRot | 19:38c9d177b6ee | 1113 | |
JKleinRot | 25:71e52c56be13 | 1114 | case BTTT: { //Geen motoraansturing |
JKleinRot | 17:c694a0e63a89 | 1115 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 17:c694a0e63a89 | 1116 | lcd.printf(" BTTT "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 1117 | pc.printf("BTTT\n\r"); //Controle naar pc sturen |
JKleinRot | 26:438a498e5526 | 1118 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 1119 | lcd.cls(); //LCD scherm leegmaken |
JKleinRot | 17:c694a0e63a89 | 1120 | |
JKleinRot | 19:38c9d177b6ee | 1121 | while(state == BTTT) { |
JKleinRot | 26:438a498e5526 | 1122 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 26:438a498e5526 | 1123 | lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm |
JKleinRot | 26:438a498e5526 | 1124 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 1125 | state = WACHT; //Door naar de volgende state |
JKleinRot | 19:38c9d177b6ee | 1126 | } |
JKleinRot | 19:38c9d177b6ee | 1127 | break; //Stop met alle acties in deze case |
JKleinRot | 17:c694a0e63a89 | 1128 | } |
JKleinRot | 19:38c9d177b6ee | 1129 | |
JKleinRot | 25:71e52c56be13 | 1130 | case TBBB: { //Motoraansturing voor doel middenonder |
JKleinRot | 17:c694a0e63a89 | 1131 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 17:c694a0e63a89 | 1132 | lcd.printf(" TBBB "); //Tekst op LCD scherm |
JKleinRot | 17:c694a0e63a89 | 1133 | pc.printf("TBBB\n\r"); //Controle naar pc sturen |
JKleinRot | 17:c694a0e63a89 | 1134 | |
JKleinRot | 19:38c9d177b6ee | 1135 | while(state == TBBB) { |
JKleinRot | 19:38c9d177b6ee | 1136 | 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 | 1137 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 19:38c9d177b6ee | 1138 | |
JKleinRot | 32:80fc2de5b511 | 1139 | while(puls_motor_arm1.getPosition() > 25) { |
JKleinRot | 32:80fc2de5b511 | 1140 | referentie_arm1 = referentie_arm1 - 155.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde |
JKleinRot | 26:438a498e5526 | 1141 | pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1142 | pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1143 | pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen |
JKleinRot | 23:5267c928ae2b | 1144 | } |
JKleinRot | 29:f95f0cc84349 | 1145 | |
JKleinRot | 32:80fc2de5b511 | 1146 | if(puls_motor_arm1.getPosition() <= 25) { |
JKleinRot | 32:80fc2de5b511 | 1147 | referentie_arm1 = 25; |
JKleinRot | 27:5ac1fc1005d7 | 1148 | } |
JKleinRot | 19:38c9d177b6ee | 1149 | |
JKleinRot | 23:5267c928ae2b | 1150 | while(puls_motor_arm2.getPosition() < 167) { |
JKleinRot | 26:438a498e5526 | 1151 | referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste waarde |
JKleinRot | 26:438a498e5526 | 1152 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1153 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1154 | pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 23:5267c928ae2b | 1155 | } |
JKleinRot | 30:f79cf70e2917 | 1156 | ticker_motor_arm2_pid.detach(); |
JKleinRot | 32:80fc2de5b511 | 1157 | pwm_to_motor_arm2 = 0.8; |
JKleinRot | 30:f79cf70e2917 | 1158 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 30:f79cf70e2917 | 1159 | dir_motor_arm2.write(0); |
JKleinRot | 23:5267c928ae2b | 1160 | |
JKleinRot | 32:80fc2de5b511 | 1161 | while(puls_motor_arm2.getPosition() > -370) { |
JKleinRot | 23:5267c928ae2b | 1162 | |
JKleinRot | 26:438a498e5526 | 1163 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 28:e2f5cee5e73b | 1164 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1165 | pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 26:438a498e5526 | 1166 | pc.printf("t is %f\n\r", t); //T naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1167 | } |
JKleinRot | 30:f79cf70e2917 | 1168 | pwm_to_motor_arm2 = 0.5; |
JKleinRot | 30:f79cf70e2917 | 1169 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 30:f79cf70e2917 | 1170 | dir_motor_arm2.write(1); |
JKleinRot | 23:5267c928ae2b | 1171 | |
JKleinRot | 30:f79cf70e2917 | 1172 | while(puls_motor_arm2.getPosition() <= 167) { |
JKleinRot | 30:f79cf70e2917 | 1173 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1174 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1175 | pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1176 | } |
JKleinRot | 30:f79cf70e2917 | 1177 | ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); |
JKleinRot | 30:f79cf70e2917 | 1178 | pc.printf("staat stil\n\r"); //Staat stil naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1179 | state = THUISPOSITIE_MIDDEN; //Door naar de volgende state |
JKleinRot | 23:5267c928ae2b | 1180 | |
JKleinRot | 19:38c9d177b6ee | 1181 | } |
JKleinRot | 24:a1fdc830f96c | 1182 | break; //Stop met alle acties in deze case |
JKleinRot | 24:a1fdc830f96c | 1183 | } |
JKleinRot | 23:5267c928ae2b | 1184 | |
JKleinRot | 25:71e52c56be13 | 1185 | case TBBT: { //Geen motoraansturing |
JKleinRot | 25:71e52c56be13 | 1186 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 25:71e52c56be13 | 1187 | lcd.printf(" TBBT "); //Tekst op LCD scherm |
JKleinRot | 25:71e52c56be13 | 1188 | pc.printf("TBBT\n\r"); //Controle naar pc sturen |
JKleinRot | 26:438a498e5526 | 1189 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 1190 | lcd.cls(); //LCD scherm leegmaken |
JKleinRot | 25:71e52c56be13 | 1191 | |
JKleinRot | 25:71e52c56be13 | 1192 | while(state == TBBT) { |
JKleinRot | 26:438a498e5526 | 1193 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 26:438a498e5526 | 1194 | lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm |
JKleinRot | 26:438a498e5526 | 1195 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 1196 | state = WACHT; //Door naar de volgende state |
JKleinRot | 25:71e52c56be13 | 1197 | } |
JKleinRot | 25:71e52c56be13 | 1198 | break; //Stop met alle acties in deze case |
JKleinRot | 25:71e52c56be13 | 1199 | } |
JKleinRot | 25:71e52c56be13 | 1200 | |
JKleinRot | 25:71e52c56be13 | 1201 | case TBTB: { //Motoraansturing voor doel midden |
JKleinRot | 25:71e52c56be13 | 1202 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 25:71e52c56be13 | 1203 | lcd.printf(" TBTB "); //Tekst op LCD scherm |
JKleinRot | 25:71e52c56be13 | 1204 | pc.printf("TBTB\n\r"); //Controle naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1205 | |
JKleinRot | 25:71e52c56be13 | 1206 | while(state == TBTB) { |
JKleinRot | 25:71e52c56be13 | 1207 | 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 | 25:71e52c56be13 | 1208 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 25:71e52c56be13 | 1209 | |
JKleinRot | 32:80fc2de5b511 | 1210 | while(puls_motor_arm1.getPosition() > 25) { |
JKleinRot | 32:80fc2de5b511 | 1211 | referentie_arm1 = referentie_arm1 - 155.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste waarde |
JKleinRot | 26:438a498e5526 | 1212 | pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1213 | pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1214 | pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1215 | } |
JKleinRot | 29:f95f0cc84349 | 1216 | |
JKleinRot | 32:80fc2de5b511 | 1217 | if(puls_motor_arm1.getPosition() <= 25) { |
JKleinRot | 32:80fc2de5b511 | 1218 | referentie_arm1 = 25; |
JKleinRot | 27:5ac1fc1005d7 | 1219 | } |
JKleinRot | 25:71e52c56be13 | 1220 | |
JKleinRot | 25:71e52c56be13 | 1221 | while(puls_motor_arm2.getPosition() < 167) { |
JKleinRot | 26:438a498e5526 | 1222 | referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie |
JKleinRot | 26:438a498e5526 | 1223 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1224 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1225 | pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1226 | } |
JKleinRot | 30:f79cf70e2917 | 1227 | ticker_motor_arm2_pid.detach(); |
JKleinRot | 32:80fc2de5b511 | 1228 | pwm_to_motor_arm2 = 0.9; |
JKleinRot | 30:f79cf70e2917 | 1229 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 30:f79cf70e2917 | 1230 | dir_motor_arm2.write(0); |
JKleinRot | 25:71e52c56be13 | 1231 | |
JKleinRot | 32:80fc2de5b511 | 1232 | while(puls_motor_arm2.getPosition() > -370) { |
JKleinRot | 25:71e52c56be13 | 1233 | |
JKleinRot | 26:438a498e5526 | 1234 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 28:e2f5cee5e73b | 1235 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1236 | pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 26:438a498e5526 | 1237 | pc.printf("t is %f\n\r", t); //T naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1238 | } |
JKleinRot | 30:f79cf70e2917 | 1239 | pwm_to_motor_arm2 = 0.5; |
JKleinRot | 30:f79cf70e2917 | 1240 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 30:f79cf70e2917 | 1241 | dir_motor_arm2.write(1); |
JKleinRot | 25:71e52c56be13 | 1242 | |
JKleinRot | 30:f79cf70e2917 | 1243 | while(puls_motor_arm2.getPosition() <= 167) { |
JKleinRot | 30:f79cf70e2917 | 1244 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1245 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1246 | pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1247 | } |
JKleinRot | 30:f79cf70e2917 | 1248 | ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); |
JKleinRot | 30:f79cf70e2917 | 1249 | pc.printf("staat stil\n\r"); //Staat stil naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1250 | state = THUISPOSITIE_MIDDEN; //Door naar de volgende state |
JKleinRot | 25:71e52c56be13 | 1251 | |
JKleinRot | 30:f79cf70e2917 | 1252 | |
JKleinRot | 25:71e52c56be13 | 1253 | } |
JKleinRot | 25:71e52c56be13 | 1254 | break; //Stop met alle acties in deze case |
JKleinRot | 25:71e52c56be13 | 1255 | } |
JKleinRot | 25:71e52c56be13 | 1256 | |
JKleinRot | 25:71e52c56be13 | 1257 | case TBTT: { //Motoraansturing voor doel middenboven |
JKleinRot | 25:71e52c56be13 | 1258 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 25:71e52c56be13 | 1259 | lcd.printf(" TBTT "); //Tekst op LCD scherm |
JKleinRot | 25:71e52c56be13 | 1260 | pc.printf("TBTT\n\r"); //Controle naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1261 | |
JKleinRot | 25:71e52c56be13 | 1262 | while(state == TBTT) { |
JKleinRot | 25:71e52c56be13 | 1263 | 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 | 25:71e52c56be13 | 1264 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 25:71e52c56be13 | 1265 | |
JKleinRot | 32:80fc2de5b511 | 1266 | while(puls_motor_arm1.getPosition() > 25) { |
JKleinRot | 32:80fc2de5b511 | 1267 | referentie_arm1 = referentie_arm1 - 155.0/200.0; //Referentie arm 1 loopt af in een seconde naar gewenste positie |
JKleinRot | 26:438a498e5526 | 1268 | pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1269 | pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1270 | pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1); //PWM naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1271 | } |
JKleinRot | 29:f95f0cc84349 | 1272 | |
JKleinRot | 32:80fc2de5b511 | 1273 | if(puls_motor_arm1.getPosition() <= 25) { |
JKleinRot | 32:80fc2de5b511 | 1274 | referentie_arm1 = 25; |
JKleinRot | 27:5ac1fc1005d7 | 1275 | } |
JKleinRot | 25:71e52c56be13 | 1276 | |
JKleinRot | 25:71e52c56be13 | 1277 | while(puls_motor_arm2.getPosition() < 167) { |
JKleinRot | 26:438a498e5526 | 1278 | referentie_arm2 = referentie_arm2 + 44.0/200.0; //Referentie arm 2 loopt op in een seconde naar gewenste positie |
JKleinRot | 26:438a498e5526 | 1279 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1280 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1281 | pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1282 | } |
JKleinRot | 30:f79cf70e2917 | 1283 | ticker_motor_arm2_pid.detach(); |
JKleinRot | 30:f79cf70e2917 | 1284 | pwm_to_motor_arm2 = 1; |
JKleinRot | 30:f79cf70e2917 | 1285 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 30:f79cf70e2917 | 1286 | dir_motor_arm2.write(0); |
JKleinRot | 25:71e52c56be13 | 1287 | |
JKleinRot | 32:80fc2de5b511 | 1288 | while(puls_motor_arm2.getPosition() > -370) { |
JKleinRot | 25:71e52c56be13 | 1289 | |
JKleinRot | 26:438a498e5526 | 1290 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 28:e2f5cee5e73b | 1291 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1292 | pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 26:438a498e5526 | 1293 | pc.printf("t is %f\n\r", t); //T naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1294 | } |
JKleinRot | 30:f79cf70e2917 | 1295 | pwm_to_motor_arm2 = 0.5; |
JKleinRot | 30:f79cf70e2917 | 1296 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 30:f79cf70e2917 | 1297 | dir_motor_arm2.write(1); |
JKleinRot | 25:71e52c56be13 | 1298 | |
JKleinRot | 30:f79cf70e2917 | 1299 | while(puls_motor_arm2.getPosition() <= 167) { |
JKleinRot | 30:f79cf70e2917 | 1300 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1301 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1302 | pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1303 | } |
JKleinRot | 30:f79cf70e2917 | 1304 | ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); |
JKleinRot | 30:f79cf70e2917 | 1305 | pc.printf("staat stil\n\r"); //Staat stil naar pc sturen |
JKleinRot | 30:f79cf70e2917 | 1306 | state = THUISPOSITIE_MIDDEN; //Door naar de volgende state |
JKleinRot | 25:71e52c56be13 | 1307 | |
JKleinRot | 30:f79cf70e2917 | 1308 | |
JKleinRot | 25:71e52c56be13 | 1309 | } |
JKleinRot | 25:71e52c56be13 | 1310 | break; //Stop met alle acties in deze case |
JKleinRot | 25:71e52c56be13 | 1311 | } |
JKleinRot | 25:71e52c56be13 | 1312 | |
JKleinRot | 25:71e52c56be13 | 1313 | case TTBB: { //Motoraansturing voor doel linksonder |
JKleinRot | 24:a1fdc830f96c | 1314 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 24:a1fdc830f96c | 1315 | lcd.printf(" TTBB "); //Tekst op LCD scherm |
JKleinRot | 24:a1fdc830f96c | 1316 | pc.printf("TTBB\n\r"); //Controle naar pc sturen |
JKleinRot | 24:a1fdc830f96c | 1317 | |
JKleinRot | 24:a1fdc830f96c | 1318 | while(state == TTBB) { |
JKleinRot | 24:a1fdc830f96c | 1319 | 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 | 24:a1fdc830f96c | 1320 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 24:a1fdc830f96c | 1321 | |
JKleinRot | 31:36fe36657f8d | 1322 | ticker_motor_arm2_pid.detach(); |
JKleinRot | 32:80fc2de5b511 | 1323 | pwm_to_motor_arm2 = 0.8; |
JKleinRot | 31:36fe36657f8d | 1324 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 31:36fe36657f8d | 1325 | dir_motor_arm2.write(0); |
JKleinRot | 24:a1fdc830f96c | 1326 | |
JKleinRot | 32:80fc2de5b511 | 1327 | while(puls_motor_arm2.getPosition() > -414) { |
JKleinRot | 24:a1fdc830f96c | 1328 | |
JKleinRot | 31:36fe36657f8d | 1329 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1330 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1331 | pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1332 | pc.printf("t is %f\n\r", t); //T naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1333 | } |
JKleinRot | 31:36fe36657f8d | 1334 | pwm_to_motor_arm2 = 0.5; |
JKleinRot | 31:36fe36657f8d | 1335 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 31:36fe36657f8d | 1336 | dir_motor_arm2.write(1); |
JKleinRot | 24:a1fdc830f96c | 1337 | |
JKleinRot | 32:80fc2de5b511 | 1338 | while(puls_motor_arm2.getPosition()<= 123) { |
JKleinRot | 31:36fe36657f8d | 1339 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1340 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1341 | pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1342 | } |
JKleinRot | 31:36fe36657f8d | 1343 | ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); |
JKleinRot | 31:36fe36657f8d | 1344 | pc.printf("staat stil\n\r"); //Staat stil naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1345 | state = WACHT; //Door naar de volgende state |
JKleinRot | 24:a1fdc830f96c | 1346 | |
JKleinRot | 31:36fe36657f8d | 1347 | |
JKleinRot | 24:a1fdc830f96c | 1348 | } |
JKleinRot | 24:a1fdc830f96c | 1349 | break; //Stop met alle acties in deze case |
JKleinRot | 24:a1fdc830f96c | 1350 | } |
JKleinRot | 23:5267c928ae2b | 1351 | |
JKleinRot | 25:71e52c56be13 | 1352 | case TTBT: { //Geen motoraansturing |
JKleinRot | 24:a1fdc830f96c | 1353 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 24:a1fdc830f96c | 1354 | lcd.printf(" TTBT "); //Tekst op LCD scherm |
JKleinRot | 24:a1fdc830f96c | 1355 | pc.printf("TTBT\n\r"); //Controle naar pc sturen |
JKleinRot | 26:438a498e5526 | 1356 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 1357 | lcd.cls(); //LCD scherm leegmaken |
JKleinRot | 24:a1fdc830f96c | 1358 | |
JKleinRot | 24:a1fdc830f96c | 1359 | while(state == TTBT) { |
JKleinRot | 26:438a498e5526 | 1360 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 26:438a498e5526 | 1361 | lcd.printf(" GEEN DOEL "); //Tekst op LCD scherm |
JKleinRot | 26:438a498e5526 | 1362 | wait(1); //Een seconde wachten |
JKleinRot | 26:438a498e5526 | 1363 | state = WACHT; //Door naar de volgende state |
JKleinRot | 24:a1fdc830f96c | 1364 | } |
JKleinRot | 24:a1fdc830f96c | 1365 | break; //Stop met alle acties in deze case |
JKleinRot | 24:a1fdc830f96c | 1366 | } |
JKleinRot | 24:a1fdc830f96c | 1367 | |
JKleinRot | 25:71e52c56be13 | 1368 | case TTTB: { //Motoraansturing voor doel linksmidden |
JKleinRot | 24:a1fdc830f96c | 1369 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 24:a1fdc830f96c | 1370 | lcd.printf(" TTTB "); //Tekst op LCD scherm |
JKleinRot | 24:a1fdc830f96c | 1371 | pc.printf("TTTB\n\r"); //Controle naar pc sturen |
JKleinRot | 24:a1fdc830f96c | 1372 | |
JKleinRot | 24:a1fdc830f96c | 1373 | while(state == TTTB) { |
JKleinRot | 24:a1fdc830f96c | 1374 | |
JKleinRot | 24:a1fdc830f96c | 1375 | 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 | 24:a1fdc830f96c | 1376 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 24:a1fdc830f96c | 1377 | |
JKleinRot | 31:36fe36657f8d | 1378 | ticker_motor_arm2_pid.detach(); |
JKleinRot | 31:36fe36657f8d | 1379 | pwm_to_motor_arm2 = 1; |
JKleinRot | 31:36fe36657f8d | 1380 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 31:36fe36657f8d | 1381 | dir_motor_arm2.write(0); |
JKleinRot | 24:a1fdc830f96c | 1382 | |
JKleinRot | 32:80fc2de5b511 | 1383 | while(puls_motor_arm2.getPosition() > -414) { |
JKleinRot | 24:a1fdc830f96c | 1384 | |
JKleinRot | 31:36fe36657f8d | 1385 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1386 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1387 | pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1388 | pc.printf("t is %f\n\r", t); //T naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1389 | } |
JKleinRot | 31:36fe36657f8d | 1390 | pwm_to_motor_arm2 = 0.5; |
JKleinRot | 31:36fe36657f8d | 1391 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 31:36fe36657f8d | 1392 | dir_motor_arm2.write(1); |
JKleinRot | 24:a1fdc830f96c | 1393 | |
JKleinRot | 32:80fc2de5b511 | 1394 | while(puls_motor_arm2.getPosition()<= 123) { |
JKleinRot | 31:36fe36657f8d | 1395 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1396 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1397 | pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1398 | } |
JKleinRot | 31:36fe36657f8d | 1399 | ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); |
JKleinRot | 31:36fe36657f8d | 1400 | pc.printf("staat stil\n\r"); //Staat stil naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1401 | state = WACHT; //Door naar de volgende state |
JKleinRot | 24:a1fdc830f96c | 1402 | |
JKleinRot | 31:36fe36657f8d | 1403 | |
JKleinRot | 23:5267c928ae2b | 1404 | } |
JKleinRot | 19:38c9d177b6ee | 1405 | |
JKleinRot | 24:a1fdc830f96c | 1406 | break; //Stop met alle acties in deze case |
JKleinRot | 24:a1fdc830f96c | 1407 | } |
JKleinRot | 24:a1fdc830f96c | 1408 | |
JKleinRot | 25:71e52c56be13 | 1409 | case TTTT: { //Motoraansturing voor doel linksboven |
JKleinRot | 24:a1fdc830f96c | 1410 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 24:a1fdc830f96c | 1411 | lcd.printf(" TTTT "); //Tekst op LCD scherm |
JKleinRot | 24:a1fdc830f96c | 1412 | pc.printf("TTBB\n\r"); //Controle naar pc sturen |
JKleinRot | 24:a1fdc830f96c | 1413 | |
JKleinRot | 24:a1fdc830f96c | 1414 | while(state == TTTT) { |
JKleinRot | 24:a1fdc830f96c | 1415 | 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 | 24:a1fdc830f96c | 1416 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 24:a1fdc830f96c | 1417 | |
JKleinRot | 31:36fe36657f8d | 1418 | ticker_motor_arm2_pid.detach(); |
JKleinRot | 31:36fe36657f8d | 1419 | pwm_to_motor_arm2 = 1; |
JKleinRot | 31:36fe36657f8d | 1420 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 31:36fe36657f8d | 1421 | dir_motor_arm2.write(0); |
JKleinRot | 24:a1fdc830f96c | 1422 | |
JKleinRot | 32:80fc2de5b511 | 1423 | while(puls_motor_arm2.getPosition() > -414) { |
JKleinRot | 24:a1fdc830f96c | 1424 | |
JKleinRot | 31:36fe36657f8d | 1425 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1426 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1427 | pc.printf("pwm motor 2 %f", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1428 | pc.printf("t is %f\n\r", t); //T naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1429 | } |
JKleinRot | 31:36fe36657f8d | 1430 | pwm_to_motor_arm2 = 0.5; |
JKleinRot | 31:36fe36657f8d | 1431 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 31:36fe36657f8d | 1432 | dir_motor_arm2.write(1); |
JKleinRot | 24:a1fdc830f96c | 1433 | |
JKleinRot | 32:80fc2de5b511 | 1434 | while(puls_motor_arm2.getPosition()<= 123) { |
JKleinRot | 31:36fe36657f8d | 1435 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1436 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1437 | pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 24:a1fdc830f96c | 1438 | } |
JKleinRot | 31:36fe36657f8d | 1439 | ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR); |
JKleinRot | 31:36fe36657f8d | 1440 | pc.printf("staat stil\n\r"); //Staat stil naar pc sturen |
JKleinRot | 31:36fe36657f8d | 1441 | state = WACHT; //Door naar de volgende state |
JKleinRot | 24:a1fdc830f96c | 1442 | } |
JKleinRot | 27:5ac1fc1005d7 | 1443 | break; |
JKleinRot | 24:a1fdc830f96c | 1444 | } |
JKleinRot | 24:a1fdc830f96c | 1445 | |
JKleinRot | 26:438a498e5526 | 1446 | case THUISPOSITIE_MIDDEN: { //Terug naar gekalibreerde positie |
JKleinRot | 24:a1fdc830f96c | 1447 | while(puls_motor_arm2.getPosition() > 123) { |
JKleinRot | 25:71e52c56be13 | 1448 | 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 | 25:71e52c56be13 | 1449 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 25:71e52c56be13 | 1450 | |
JKleinRot | 26:438a498e5526 | 1451 | referentie_arm2 = referentie_arm2 - 44.0/200.0; //Referentie arm 2 loopt af in een seconde naar gewenste waarde |
JKleinRot | 26:438a498e5526 | 1452 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 28:e2f5cee5e73b | 1453 | pc.printf("get position 2 %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1454 | pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 24:a1fdc830f96c | 1455 | } |
JKleinRot | 24:a1fdc830f96c | 1456 | |
JKleinRot | 32:80fc2de5b511 | 1457 | while(puls_motor_arm1.getPosition() < 203) { |
JKleinRot | 25:71e52c56be13 | 1458 | 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 | 25:71e52c56be13 | 1459 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 25:71e52c56be13 | 1460 | |
JKleinRot | 32:80fc2de5b511 | 1461 | referentie_arm1 = referentie_arm1 + 155.0/200.0; //Referentie arm 1 loopt op in een seconde naar gewenste waarde |
JKleinRot | 26:438a498e5526 | 1462 | pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1463 | pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1464 | pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1); //PWM naar pc sturen |
JKleinRot | 24:a1fdc830f96c | 1465 | } |
JKleinRot | 24:a1fdc830f96c | 1466 | |
JKleinRot | 26:438a498e5526 | 1467 | state = WACHT; //Door naar de volgende state |
JKleinRot | 24:a1fdc830f96c | 1468 | break; |
JKleinRot | 24:a1fdc830f96c | 1469 | } |
JKleinRot | 24:a1fdc830f96c | 1470 | |
JKleinRot | 26:438a498e5526 | 1471 | case THUISPOSITIE_RECHTS: { //Terug naar gekalibreerde positie |
JKleinRot | 25:71e52c56be13 | 1472 | while(puls_motor_arm2.getPosition() > 123) { |
JKleinRot | 25:71e52c56be13 | 1473 | 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 | 25:71e52c56be13 | 1474 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 25:71e52c56be13 | 1475 | |
JKleinRot | 26:438a498e5526 | 1476 | referentie_arm2 = referentie_arm2 - 88.0/200.0; //Referentie arm 2 loopt af in een seconde naar gewenste waarde |
JKleinRot | 26:438a498e5526 | 1477 | pc.printf("referentie arm 2 %f ", referentie_arm2); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1478 | pc.printf("get position %d ", puls_motor_arm2.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1479 | pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2); //PWM naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1480 | } |
JKleinRot | 25:71e52c56be13 | 1481 | |
JKleinRot | 32:80fc2de5b511 | 1482 | while(puls_motor_arm1.getPosition() < 203) { |
JKleinRot | 25:71e52c56be13 | 1483 | 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 | 25:71e52c56be13 | 1484 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 25:71e52c56be13 | 1485 | |
JKleinRot | 32:80fc2de5b511 | 1486 | referentie_arm1 = referentie_arm1 + 287.0/200.0; //Referentie arm 1 loopt op in een seconde naar gewenste waarde |
JKleinRot | 26:438a498e5526 | 1487 | pc.printf("referentie arm 1 %f ", referentie_arm1); //Referentie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1488 | pc.printf("get position 1 %d ", puls_motor_arm1.getPosition()); //Positie naar pc sturen |
JKleinRot | 26:438a498e5526 | 1489 | pc.printf("pwm motor 1 %f\n\r ", pwm_to_motor_arm1); //PWM naar pc sturen |
JKleinRot | 25:71e52c56be13 | 1490 | } |
JKleinRot | 27:5ac1fc1005d7 | 1491 | state = WACHT; |
JKleinRot | 27:5ac1fc1005d7 | 1492 | break; |
JKleinRot | 25:71e52c56be13 | 1493 | } |
JKleinRot | 25:71e52c56be13 | 1494 | |
JKleinRot | 26:438a498e5526 | 1495 | case WACHT: { //Even wachten en weer terug naar start |
JKleinRot | 26:438a498e5526 | 1496 | lcd.locate(0,0); //Zet tekst op eerste regel |
JKleinRot | 26:438a498e5526 | 1497 | lcd.printf(" WACHT "); //Tekst op LCD scherm |
JKleinRot | 28:e2f5cee5e73b | 1498 | wait(5); //Vijf seconden wachten |
JKleinRot | 26:438a498e5526 | 1499 | state = START; //Terug naar state START |
JKleinRot | 27:5ac1fc1005d7 | 1500 | break; |
JKleinRot | 26:438a498e5526 | 1501 | } |
JKleinRot | 23:5267c928ae2b | 1502 | |
JKleinRot | 26:438a498e5526 | 1503 | default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case |
JKleinRot | 26:438a498e5526 | 1504 | state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan |
JKleinRot | 23:5267c928ae2b | 1505 | } |
JKleinRot | 9:454e7da8ab65 | 1506 | } |
JKleinRot | 20:1cb0cf0d49ac | 1507 | } |
JKleinRot | 24:a1fdc830f96c | 1508 | } |
JKleinRot | 26:438a498e5526 | 1509 | |
JKleinRot | 27:5ac1fc1005d7 | 1510 |