Janine Klein Rot / Mbed 2 deprecated PIPO-2

2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range

Dependencies:   Encoder MODSERIAL TextLCD mbed mbed-dsp

main.cpp@22:838a17065bc7, 2014-10-31 (annotated)

Committer:
JKleinRot
Date:
Fri Oct 31 14:08:57 2014 +0000
Revision:
22:838a17065bc7
Parent:
21:f4e9f6c28de1
Child:
23:5267c928ae2b
2014-10-31 Keuze in welk doel werkt. Extra drie seconden meten voor bekijken boven threshold. EMG meten als functie waar niks uit komt

Who changed what in which revision?

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