![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
main.cpp@31:36fe36657f8d, 2014-11-04 (annotated)
- Committer:
- JKleinRot
- Date:
- Tue Nov 04 00:33:35 2014 +0000
- Revision:
- 31:36fe36657f8d
- Parent:
- 30:f79cf70e2917
- Child:
- 32:80fc2de5b511
Snelle snelheid
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 | 25:71e52c56be13 | 249 | referentie_arm1 = referentie_arm1 + 180.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 | 25:71e52c56be13 | 255 | if (puls_motor_arm1.getPosition() >= 180) { //Als het gewenste aantal pulsen behaald is |
JKleinRot | 25:71e52c56be13 | 256 | referentie_arm1 = 180; //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 | 25:71e52c56be13 | 894 | while(puls_motor_arm1.getPosition() > -84) { |
JKleinRot | 26:438a498e5526 | 895 | referentie_arm1 = referentie_arm1 - 264.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 | 29:f95f0cc84349 | 901 | if(puls_motor_arm1.getPosition() <= -84) { |
JKleinRot | 27:5ac1fc1005d7 | 902 | referentie_arm1 = -84; |
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 | 29:f95f0cc84349 | 912 | pwm_to_motor_arm2 = 1; |
JKleinRot | 29:f95f0cc84349 | 913 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 29:f95f0cc84349 | 914 | dir_motor_arm2.write(0); |
JKleinRot | 25:71e52c56be13 | 915 | while(puls_motor_arm2.getPosition() > 36) { |
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 | 25:71e52c56be13 | 967 | while(puls_motor_arm1.getPosition() > -84) { |
JKleinRot | 26:438a498e5526 | 968 | referentie_arm1 = referentie_arm1 - 264.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 | 29:f95f0cc84349 | 974 | if(puls_motor_arm1.getPosition() <= -84) { |
JKleinRot | 27:5ac1fc1005d7 | 975 | referentie_arm1 = -84; |
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 | 30:f79cf70e2917 | 985 | pwm_to_motor_arm2 = 1; |
JKleinRot | 30:f79cf70e2917 | 986 | pwm_motor_arm2.write(pwm_to_motor_arm2); |
JKleinRot | 30:f79cf70e2917 | 987 | dir_motor_arm2.write(0); |
JKleinRot | 25:71e52c56be13 | 988 | while(puls_motor_arm2.getPosition() > 36) { |
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 | 25:71e52c56be13 | 1021 | while(puls_motor_arm1.getPosition() > -84) { |
JKleinRot | 27:5ac1fc1005d7 | 1022 | referentie_arm1 = referentie_arm1 - 259.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 | 29:f95f0cc84349 | 1028 | if(puls_motor_arm1.getPosition() <= -84) { |
JKleinRot | 27:5ac1fc1005d7 | 1029 | referentie_arm1 = -84; |
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 | 25:71e52c56be13 | 1042 | while(puls_motor_arm2.getPosition() > 36) { |
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 | 23:5267c928ae2b | 1139 | while(puls_motor_arm1.getPosition() > 48) { |
JKleinRot | 26:438a498e5526 | 1140 | referentie_arm1 = referentie_arm1 - 132.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 | 29:f95f0cc84349 | 1146 | if(puls_motor_arm1.getPosition() <= 48) { |
JKleinRot | 27:5ac1fc1005d7 | 1147 | referentie_arm1 = 48; |
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 | 30:f79cf70e2917 | 1157 | pwm_to_motor_arm2 = 1; |
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 | 25:71e52c56be13 | 1161 | while(puls_motor_arm2.getPosition() > -8) { |
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 | 25:71e52c56be13 | 1210 | while(puls_motor_arm1.getPosition() > 48) { |
JKleinRot | 26:438a498e5526 | 1211 | referentie_arm1 = referentie_arm1 - 132.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 | 29:f95f0cc84349 | 1217 | if(puls_motor_arm1.getPosition() <= 48) { |
JKleinRot | 27:5ac1fc1005d7 | 1218 | referentie_arm1 = 48; |
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 | 30:f79cf70e2917 | 1228 | pwm_to_motor_arm2 = 1; |
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 | 25:71e52c56be13 | 1232 | while(puls_motor_arm2.getPosition() > -8) { |
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 | 25:71e52c56be13 | 1266 | while(puls_motor_arm1.getPosition() > 48) { |
JKleinRot | 26:438a498e5526 | 1267 | referentie_arm1 = referentie_arm1 - 132.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 | 29:f95f0cc84349 | 1273 | if(puls_motor_arm1.getPosition() <= 48) { |
JKleinRot | 27:5ac1fc1005d7 | 1274 | referentie_arm1 = 48; |
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 | 25:71e52c56be13 | 1288 | while(puls_motor_arm2.getPosition() > -8) { |
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 | 31:36fe36657f8d | 1323 | pwm_to_motor_arm2 = 1; |
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 | 31:36fe36657f8d | 1327 | while(puls_motor_arm2.getPosition() < -175) { |
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 | 31:36fe36657f8d | 1338 | while(puls_motor_arm2.getPosition() >= 0) { |
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 | 31:36fe36657f8d | 1383 | while(puls_motor_arm2.getPosition() < -175) { |
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 | 31:36fe36657f8d | 1394 | while(puls_motor_arm2.getPosition() >= 0) { |
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 | 31:36fe36657f8d | 1423 | while(puls_motor_arm2.getPosition() < -175) { |
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 | 31:36fe36657f8d | 1434 | while(puls_motor_arm2.getPosition() >= 0) { |
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 | 24:a1fdc830f96c | 1457 | while(puls_motor_arm1.getPosition() < 180) { |
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 | 26:438a498e5526 | 1461 | referentie_arm1 = referentie_arm1 + 132.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 | 27:5ac1fc1005d7 | 1482 | while(puls_motor_arm1.getPosition() < 175) { |
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 | 27:5ac1fc1005d7 | 1486 | referentie_arm1 = referentie_arm1 + 259.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 |