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@24:a1fdc830f96c, 2014-11-02 (annotated)

Committer:
JKleinRot
Date:
Sun Nov 02 19:42:31 2014 +0000
Revision:
24:a1fdc830f96c
Parent:
23:5267c928ae2b
Child:
25:71e52c56be13
2014-11-01 Laatste versie van de dag, mogelijke oplossing stoppen na slaan;

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 23:5267c928ae2b 12 #define KP_arm2 0.01 //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 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 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 23:5267c928ae2b 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, WACHT, THUISPOSITIE_MIDDEN}; //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 23:5267c928ae2b 86 float t = 0;
JKleinRot 23:5267c928ae2b 87
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 20:1cb0cf0d49ac 145 void motor_arm1_pid()
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 21:f4e9f6c28de1 151 vorige_error_arm1 = error_arm1_kalibratie;
JKleinRot 21:f4e9f6c28de1 152 keep_in_range(&pwm_to_motor_arm1, -1, 1);
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 20:1cb0cf0d49ac 163 void motor_arm2_pid()
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 21:f4e9f6c28de1 169 vorige_error_arm2 = error_arm2_kalibratie;
JKleinRot 20:1cb0cf0d49ac 170 keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
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(&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 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(&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 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 22:838a17065bc7 207 void EMG_meten()
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 20:1cb0cf0d49ac 218 ticker_motor_arm1_pid.attach(motor_arm1_pid,SAMPLETIME_REGELAAR);
JKleinRot 20:1cb0cf0d49ac 219 ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
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 16:c34c5d9dfe1a 235 state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
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 20:1cb0cf0d49ac 249 referentie_arm1 = referentie_arm1 + 180.0/200.0;
JKleinRot 21:f4e9f6c28de1 250
JKleinRot 20:1cb0cf0d49ac 251 pc.printf("pulsmotorgetposition1 %d ", puls_motor_arm1.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
JKleinRot 20:1cb0cf0d49ac 252 pc.printf("pwmmotor1 %f ", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen
JKleinRot 20:1cb0cf0d49ac 253 pc.printf("referentie1 %f\n\r", referentie_arm1);
JKleinRot 21:f4e9f6c28de1 254
JKleinRot 21:f4e9f6c28de1 255 if (referentie_arm1 >= 180) {
JKleinRot 21:f4e9f6c28de1 256 referentie_arm1 = 180;
JKleinRot 21:f4e9f6c28de1 257 state = KALIBRATIE_ARM2;
JKleinRot 20:1cb0cf0d49ac 258 }
JKleinRot 21:f4e9f6c28de1 259
JKleinRot 10:52b22bff409a 260 }
JKleinRot 10:52b22bff409a 261 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 262 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 263 }
JKleinRot 10:52b22bff409a 264
JKleinRot 19:38c9d177b6ee 265 case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
JKleinRot 19:38c9d177b6ee 266 pc.printf("KALIBRATIE_ARM2\n\r"); //Begin KALIBRATIE_ARM2 naar pc sturen
JKleinRot 16:c34c5d9dfe1a 267 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 268
JKleinRot 10:52b22bff409a 269 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 270
JKleinRot 19:38c9d177b6ee 271 while(state == KALIBRATIE_ARM2) {
JKleinRot 16:c34c5d9dfe1a 272 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 273 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 274
JKleinRot 23:5267c928ae2b 275 referentie_arm2 = referentie_arm2 + 123.0/200.0;
JKleinRot 21:f4e9f6c28de1 276
JKleinRot 20:1cb0cf0d49ac 277 pc.printf("pulsmotorgetposition2 %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
JKleinRot 20:1cb0cf0d49ac 278 pc.printf("pwmmotor2 %f ", pwm_to_motor_arm2); //Huidige PWM waarde naar motor naar pc sturen
JKleinRot 20:1cb0cf0d49ac 279 pc.printf("referentie2 %f\n\r", referentie_arm2);
JKleinRot 21:f4e9f6c28de1 280
JKleinRot 23:5267c928ae2b 281 if(referentie_arm2 >= 123) {
JKleinRot 23:5267c928ae2b 282 referentie_arm2 = 123;
JKleinRot 20:1cb0cf0d49ac 283 state = EMG_KALIBRATIE_BICEPS;
JKleinRot 20:1cb0cf0d49ac 284 }
JKleinRot 21:f4e9f6c28de1 285
JKleinRot 10:52b22bff409a 286 }
JKleinRot 11:e9b906b5f572 287 wait(1); //Een seconde wachten
JKleinRot 23:5267c928ae2b 288 //ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
JKleinRot 16:c34c5d9dfe1a 289 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 290 }
JKleinRot 15:3ebd0e666a9c 291
JKleinRot 19:38c9d177b6ee 292 case EMG_KALIBRATIE_BICEPS: { //Kalibratie biceps voor bepalen threshold
JKleinRot 16:c34c5d9dfe1a 293 pc.printf("EMG__KALIBRATIE_BICEPS\n\r"); //Begin EMG_KALIBRATIE_BICEPS naar pc sturen
JKleinRot 19:38c9d177b6ee 294
JKleinRot 16:c34c5d9dfe1a 295 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 16:c34c5d9dfe1a 296 lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm
JKleinRot 16:c34c5d9dfe1a 297 lcd.locate(0,1); //Zet tekst op tweede regel
JKleinRot 16:c34c5d9dfe1a 298 lcd.printf(" 2 X BICEPS AAN "); //Tekst op LCD scherm
JKleinRot 21:f4e9f6c28de1 299 wait(1);
JKleinRot 21:f4e9f6c28de1 300 lcd.cls();
JKleinRot 15:3ebd0e666a9c 301
JKleinRot 17:c694a0e63a89 302 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 303 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 304 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 305
JKleinRot 16:c34c5d9dfe1a 306 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 16:c34c5d9dfe1a 307 biceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt
JKleinRot 15:3ebd0e666a9c 308
JKleinRot 19:38c9d177b6ee 309 while(biceps_kalibratie.read() <= 5) { //Zolang de timer nog geen 5 seconden heeft gemeten
JKleinRot 19:38c9d177b6ee 310
JKleinRot 15:3ebd0e666a9c 311 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 312 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 313
JKleinRot 15:3ebd0e666a9c 314 xb = EMG_bi.read(); //EMG meten van biceps
JKleinRot 15:3ebd0e666a9c 315
JKleinRot 17:c694a0e63a89 316 filter_biceps(); //Voer acties uit om EMG signaal van de biceps te filteren
JKleinRot 15:3ebd0e666a9c 317
JKleinRot 19:38c9d177b6ee 318 if(int(biceps_kalibratie.read()) == 0) { //Wanneer de timer nog geen een seconde heeft gemeten
JKleinRot 17:c694a0e63a89 319 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 320 lcd.printf(" 5 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 321 pc.printf("4"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 322 }
JKleinRot 19:38c9d177b6ee 323 if(int(biceps_kalibratie.read()) == 1) { //Wanneer de timer nog geen twee seconden heeft gemeten
JKleinRot 17:c694a0e63a89 324 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 325 lcd.printf(" 4 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 326 pc.printf("3"); //Controle naar pc sturen
JKleinRot 14:e1816efa712d 327 }
JKleinRot 19:38c9d177b6ee 328 if(int(biceps_kalibratie.read()) == 2) { //Wanneer de timer nog geen drie seconden heeft gemeten
JKleinRot 17:c694a0e63a89 329 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 330 lcd.printf(" 3 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 331 pc.printf("2"); //Controle naar pc sturen
JKleinRot 14:e1816efa712d 332 }
JKleinRot 19:38c9d177b6ee 333 if(int(biceps_kalibratie.read()) == 3) { //Wanneer de timer nog geen vier seconden heeft gemeten
JKleinRot 17:c694a0e63a89 334 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 335 lcd.printf(" 2 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 336 pc.printf("1"); //Controle naar pc sturen
JKleinRot 14:e1816efa712d 337 }
JKleinRot 19:38c9d177b6ee 338 if(int(biceps_kalibratie.read()) == 4) { //Wanneer de timer nog geen vijf seconden heeft gemeten
JKleinRot 17:c694a0e63a89 339 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 340 lcd.printf(" 1 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 341 pc.printf("1"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 342 }
JKleinRot 15:3ebd0e666a9c 343
JKleinRot 21:f4e9f6c28de1 344 if(xbf >= xbfmax) { //Als de gefilterde EMG waarde groter is dan xbfmax
JKleinRot 21:f4e9f6c28de1 345 xbfmax = xbf; //Dan wordt deze gefilterde EMG waarde de nieuwe xbfmax
JKleinRot 21:f4e9f6c28de1 346 }
JKleinRot 15:3ebd0e666a9c 347 }
JKleinRot 19:38c9d177b6ee 348
JKleinRot 21:f4e9f6c28de1 349 xbt = xbfmax * 0.95; //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten
JKleinRot 21:f4e9f6c28de1 350 pc.printf("maximale biceps %f", xbfmax);
JKleinRot 17:c694a0e63a89 351 pc.printf("threshold is %f\n\r", xbt); //Thresholdwaarde naar pc sturen
JKleinRot 17:c694a0e63a89 352 state = EMG_KALIBRATIE_TRICEPS; //Gaat door naar kalibratie van de EMG van de triceps
JKleinRot 17:c694a0e63a89 353 break; //Stopt alle acties in deze case
JKleinRot 15:3ebd0e666a9c 354 }
JKleinRot 15:3ebd0e666a9c 355
JKleinRot 19:38c9d177b6ee 356 case EMG_KALIBRATIE_TRICEPS: {
JKleinRot 17:c694a0e63a89 357 pc.printf("EMG__KALIBRATIE_TRICEPS\n\r"); //Begin EMG_KALIBRATIE_TRICEPS naar pc sturen
JKleinRot 15:3ebd0e666a9c 358
JKleinRot 17:c694a0e63a89 359 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 360 lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 361 lcd.locate(0,1); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 362 lcd.printf(" 2 X TRICEPS AAN"); //Tekst op LCD scherm
JKleinRot 21:f4e9f6c28de1 363 wait(1);
JKleinRot 21:f4e9f6c28de1 364 lcd.cls();
JKleinRot 15:3ebd0e666a9c 365
JKleinRot 19:38c9d177b6ee 366 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 367 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 368 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 369
JKleinRot 17:c694a0e63a89 370 //ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Deze ticker loopt nog
JKleinRot 17:c694a0e63a89 371 triceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt
JKleinRot 15:3ebd0e666a9c 372
JKleinRot 19:38c9d177b6ee 373 while(triceps_kalibratie.read() <= 5) { //Zolang de timer nog geen 5 seconden heeft gemeten
JKleinRot 15:3ebd0e666a9c 374 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 375 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 376
JKleinRot 17:c694a0e63a89 377 xt = EMG_tri.read(); //EMG meten van ticeps
JKleinRot 15:3ebd0e666a9c 378
JKleinRot 17:c694a0e63a89 379 filter_triceps(); //Voer acties uit om EMG signaal van de triceps te meten
JKleinRot 15:3ebd0e666a9c 380
JKleinRot 19:38c9d177b6ee 381 if(int(triceps_kalibratie.read()) == 0) { //Wanneer de timer nog geen twee seconden heeft gemeten
JKleinRot 17:c694a0e63a89 382 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 383 lcd.printf(" 5 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 384 pc.printf("4"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 385 }
JKleinRot 19:38c9d177b6ee 386 if(int(triceps_kalibratie.read()) == 1) { //Wanneer de timer nog geen twee seconden heeft gemeten
JKleinRot 17:c694a0e63a89 387 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 388 lcd.printf(" 4 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 389 pc.printf("3"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 390 }
JKleinRot 19:38c9d177b6ee 391 if(int(triceps_kalibratie.read()) == 2) { //Wanneer de timer nog geen drie seconden heeft gemeten
JKleinRot 17:c694a0e63a89 392 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 393 lcd.printf(" 3 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 394 pc.printf("2"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 395 }
JKleinRot 19:38c9d177b6ee 396 if(int(triceps_kalibratie.read()) == 3) { //Wanneer de timer nog geen vier seconden heeft gemeten
JKleinRot 17:c694a0e63a89 397 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 398 lcd.printf(" 2 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 399 pc.printf("1"); //Controle naar pc sturen
JKleinRot 15:3ebd0e666a9c 400 }
JKleinRot 19:38c9d177b6ee 401 if(int(triceps_kalibratie.read()) == 4) { //Wanneer de timer nog geen vijf seconden heeft gemeten
JKleinRot 17:c694a0e63a89 402 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 17:c694a0e63a89 403 lcd.printf(" 1 "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 404 pc.printf("1"); //Controle naar pc sturen
JKleinRot 14:e1816efa712d 405 }
JKleinRot 15:3ebd0e666a9c 406
JKleinRot 21:f4e9f6c28de1 407
JKleinRot 21:f4e9f6c28de1 408 if(xtf >= xtfmax) { //Als de gefilterde EMG waarde groter is dan xtfmax
JKleinRot 21:f4e9f6c28de1 409 xtfmax = xtf; //Dan wordt deze gefilterde EMG waarde de nieuwe xtfmax
JKleinRot 21:f4e9f6c28de1 410 }
JKleinRot 10:52b22bff409a 411 }
JKleinRot 19:38c9d177b6ee 412
JKleinRot 21:f4e9f6c28de1 413 xtt = xtfmax * 0.90; //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten
JKleinRot 21:f4e9f6c28de1 414 pc.printf("maximale triceps %f", xtfmax);
JKleinRot 17:c694a0e63a89 415 pc.printf("threshold is %f\n\r", xtt); //Thresholdwaarde naar pc sturen
JKleinRot 17:c694a0e63a89 416 state = START; //Gaat door naar het slaan, kalibratie nu afgerond
JKleinRot 17:c694a0e63a89 417 break; //Stopt alle acties in deze case
JKleinRot 10:52b22bff409a 418 }
JKleinRot 15:3ebd0e666a9c 419
JKleinRot 17:c694a0e63a89 420 case START: {
JKleinRot 17:c694a0e63a89 421 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 422 lcd.printf(" START "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 423
JKleinRot 17:c694a0e63a89 424 pc.printf("START\n\r"); //Controle naar pc sturen
JKleinRot 19:38c9d177b6ee 425
JKleinRot 19:38c9d177b6ee 426 while(state == START) {
JKleinRot 17:c694a0e63a89 427 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 428 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 429
JKleinRot 17:c694a0e63a89 430 xb = EMG_bi.read(); //EMG signaal biceps uitlezen
JKleinRot 17:c694a0e63a89 431 filter_biceps(); //EMG signaal biceps filteren
JKleinRot 17:c694a0e63a89 432 xt = EMG_tri.read(); //EMG signaal triceps uitlezen
JKleinRot 17:c694a0e63a89 433 filter_triceps(); //EMG signaal triceps filteren
JKleinRot 15:3ebd0e666a9c 434
JKleinRot 21:f4e9f6c28de1 435 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 436 state = B; //Ga door naar state B
JKleinRot 15:3ebd0e666a9c 437 }
JKleinRot 21:f4e9f6c28de1 438 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 439 state = T; //Ga door naar state T
JKleinRot 15:3ebd0e666a9c 440 }
JKleinRot 15:3ebd0e666a9c 441 }
JKleinRot 17:c694a0e63a89 442 break; //Stopt met de acties in deze case
JKleinRot 16:c34c5d9dfe1a 443 }
JKleinRot 19:38c9d177b6ee 444
JKleinRot 19:38c9d177b6ee 445 case B: {
JKleinRot 17:c694a0e63a89 446 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 447 lcd.printf(" B "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 448 pc.printf("B\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 449
JKleinRot 22:838a17065bc7 450 EMG.start();
JKleinRot 23:5267c928ae2b 451
JKleinRot 23:5267c928ae2b 452 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 453 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 454 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 455
JKleinRot 22:838a17065bc7 456 EMG_meten();
JKleinRot 22:838a17065bc7 457 }
JKleinRot 23:5267c928ae2b 458
JKleinRot 19:38c9d177b6ee 459 while(state == B) {
JKleinRot 19:38c9d177b6ee 460 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 461 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 462
JKleinRot 22:838a17065bc7 463 EMG_meten();
JKleinRot 23:5267c928ae2b 464
JKleinRot 21:f4e9f6c28de1 465 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 466 state = BB; //Ga door naar state BB
JKleinRot 19:38c9d177b6ee 467 }
JKleinRot 21:f4e9f6c28de1 468 if(xtf>= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 469 state = BT; //Ga door naar state BT
JKleinRot 19:38c9d177b6ee 470 }
JKleinRot 19:38c9d177b6ee 471 }
JKleinRot 19:38c9d177b6ee 472 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 473 }
JKleinRot 19:38c9d177b6ee 474
JKleinRot 19:38c9d177b6ee 475 case T: {
JKleinRot 19:38c9d177b6ee 476 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 477 lcd.printf(" T "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 478 pc.printf("T\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 479
JKleinRot 22:838a17065bc7 480 EMG.start();
JKleinRot 23:5267c928ae2b 481
JKleinRot 23:5267c928ae2b 482 while(EMG.read() <= 3) {
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 23:5267c928ae2b 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 23:5267c928ae2b 509
JKleinRot 22:838a17065bc7 510 EMG.reset();
JKleinRot 22:838a17065bc7 511 EMG.start();
JKleinRot 23:5267c928ae2b 512
JKleinRot 23:5267c928ae2b 513 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 514 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 515 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 516
JKleinRot 22:838a17065bc7 517 EMG_meten();
JKleinRot 22:838a17065bc7 518 }
JKleinRot 16:c34c5d9dfe1a 519
JKleinRot 19:38c9d177b6ee 520 while(state == BB) {
JKleinRot 17:c694a0e63a89 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 17:c694a0e63a89 522 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 16:c34c5d9dfe1a 523
JKleinRot 22:838a17065bc7 524 EMG_meten();
JKleinRot 16:c34c5d9dfe1a 525
JKleinRot 21:f4e9f6c28de1 526 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 527 state = BBB; //Ga door naar state BBB
JKleinRot 16:c34c5d9dfe1a 528 }
JKleinRot 21:f4e9f6c28de1 529 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 530 state = BBT; //Ga door naar state BBT
JKleinRot 16:c34c5d9dfe1a 531 }
JKleinRot 16:c34c5d9dfe1a 532 }
JKleinRot 17:c694a0e63a89 533 break; //Stop met alle acties in deze case
JKleinRot 16:c34c5d9dfe1a 534 }
JKleinRot 19:38c9d177b6ee 535
JKleinRot 19:38c9d177b6ee 536 case BT: {
JKleinRot 17:c694a0e63a89 537 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 538 lcd.printf(" BT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 539 pc.printf("BT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 540
JKleinRot 22:838a17065bc7 541 EMG.reset();
JKleinRot 22:838a17065bc7 542 EMG.start();
JKleinRot 23:5267c928ae2b 543
JKleinRot 23:5267c928ae2b 544 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 545 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 546 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 547
JKleinRot 22:838a17065bc7 548 EMG_meten();
JKleinRot 22:838a17065bc7 549 }
JKleinRot 15:3ebd0e666a9c 550
JKleinRot 19:38c9d177b6ee 551 while(state == BT) {
JKleinRot 17:c694a0e63a89 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 17:c694a0e63a89 553 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 554
JKleinRot 22:838a17065bc7 555 EMG_meten();
JKleinRot 17:c694a0e63a89 556
JKleinRot 21:f4e9f6c28de1 557 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 558 state = BTB; //Ga door naar state BTB
JKleinRot 17:c694a0e63a89 559 }
JKleinRot 21:f4e9f6c28de1 560 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 561 state = BTT; //Ga door naar state BTT
JKleinRot 17:c694a0e63a89 562 }
JKleinRot 17:c694a0e63a89 563 }
JKleinRot 19:38c9d177b6ee 564 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 565 }
JKleinRot 19:38c9d177b6ee 566
JKleinRot 19:38c9d177b6ee 567 case TB: {
JKleinRot 17:c694a0e63a89 568 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 569 lcd.printf(" TB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 570 pc.printf("TB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 571
JKleinRot 22:838a17065bc7 572 EMG.reset();
JKleinRot 22:838a17065bc7 573 EMG.start();
JKleinRot 23:5267c928ae2b 574
JKleinRot 23:5267c928ae2b 575 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 576 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 577 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 578
JKleinRot 22:838a17065bc7 579 EMG_meten();
JKleinRot 22:838a17065bc7 580 }
JKleinRot 17:c694a0e63a89 581
JKleinRot 19:38c9d177b6ee 582 while(state == TB) {
JKleinRot 17:c694a0e63a89 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 17:c694a0e63a89 584 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 585
JKleinRot 22:838a17065bc7 586 EMG_meten();
JKleinRot 17:c694a0e63a89 587
JKleinRot 21:f4e9f6c28de1 588 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 589 state = TBB; //Ga door naar state TBB
JKleinRot 17:c694a0e63a89 590 }
JKleinRot 21:f4e9f6c28de1 591 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 592 state = TBT; //Ga door naar state TBT
JKleinRot 17:c694a0e63a89 593 }
JKleinRot 17:c694a0e63a89 594 }
JKleinRot 19:38c9d177b6ee 595 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 596 }
JKleinRot 17:c694a0e63a89 597
JKleinRot 19:38c9d177b6ee 598 case TT: {
JKleinRot 17:c694a0e63a89 599 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 600 lcd.printf(" TT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 601 pc.printf("TT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 602
JKleinRot 22:838a17065bc7 603 EMG.reset();
JKleinRot 22:838a17065bc7 604 EMG.start();
JKleinRot 23:5267c928ae2b 605
JKleinRot 23:5267c928ae2b 606 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 607 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 608 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 609
JKleinRot 22:838a17065bc7 610 EMG_meten();
JKleinRot 22:838a17065bc7 611 }
JKleinRot 17:c694a0e63a89 612
JKleinRot 19:38c9d177b6ee 613 while(state == TT) {
JKleinRot 17:c694a0e63a89 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 17:c694a0e63a89 615 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 616
JKleinRot 22:838a17065bc7 617 EMG_meten();
JKleinRot 17:c694a0e63a89 618
JKleinRot 21:f4e9f6c28de1 619 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 620 state = TTB; //Ga door naar state TTB
JKleinRot 17:c694a0e63a89 621 }
JKleinRot 21:f4e9f6c28de1 622 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 623 state = TTT; //Ga door naar state TTT
JKleinRot 17:c694a0e63a89 624 }
JKleinRot 17:c694a0e63a89 625 }
JKleinRot 19:38c9d177b6ee 626 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 627 }
JKleinRot 19:38c9d177b6ee 628
JKleinRot 19:38c9d177b6ee 629 case BBB: {
JKleinRot 17:c694a0e63a89 630 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 631 lcd.printf(" BBB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 632 pc.printf("BBB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 633
JKleinRot 22:838a17065bc7 634 EMG.reset();
JKleinRot 22:838a17065bc7 635 EMG.start();
JKleinRot 23:5267c928ae2b 636
JKleinRot 23:5267c928ae2b 637 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 638 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 639 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 640
JKleinRot 22:838a17065bc7 641 EMG_meten();
JKleinRot 22:838a17065bc7 642 }
JKleinRot 17:c694a0e63a89 643
JKleinRot 19:38c9d177b6ee 644 while(state == BBB) {
JKleinRot 17:c694a0e63a89 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 17:c694a0e63a89 646 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 647
JKleinRot 22:838a17065bc7 648 EMG_meten();
JKleinRot 17:c694a0e63a89 649
JKleinRot 21:f4e9f6c28de1 650 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 651 state = BBBB; //Ga door naar state BBBB
JKleinRot 17:c694a0e63a89 652 }
JKleinRot 21:f4e9f6c28de1 653 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 654 state = BBBT; //Ga door naar state BBBT
JKleinRot 17:c694a0e63a89 655 }
JKleinRot 17:c694a0e63a89 656 }
JKleinRot 19:38c9d177b6ee 657 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 658 }
JKleinRot 17:c694a0e63a89 659
JKleinRot 19:38c9d177b6ee 660
JKleinRot 19:38c9d177b6ee 661 case BBT: {
JKleinRot 17:c694a0e63a89 662 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 663 lcd.printf(" BBT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 664 pc.printf("BBT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 665
JKleinRot 22:838a17065bc7 666 EMG.reset();
JKleinRot 22:838a17065bc7 667 EMG.start();
JKleinRot 23:5267c928ae2b 668
JKleinRot 23:5267c928ae2b 669 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 670 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 671 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 672
JKleinRot 22:838a17065bc7 673 EMG_meten();
JKleinRot 22:838a17065bc7 674 }
JKleinRot 17:c694a0e63a89 675
JKleinRot 19:38c9d177b6ee 676 while(state == BBT) {
JKleinRot 17:c694a0e63a89 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 17:c694a0e63a89 678 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 679
JKleinRot 22:838a17065bc7 680 EMG_meten();
JKleinRot 17:c694a0e63a89 681
JKleinRot 21:f4e9f6c28de1 682 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 683 state = BBTB; //Ga door naar state BBTB
JKleinRot 17:c694a0e63a89 684 }
JKleinRot 21:f4e9f6c28de1 685 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 686 state = BBTT; //Ga door naar state BBTT
JKleinRot 17:c694a0e63a89 687 }
JKleinRot 17:c694a0e63a89 688 }
JKleinRot 19:38c9d177b6ee 689 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 690 }
JKleinRot 19:38c9d177b6ee 691
JKleinRot 19:38c9d177b6ee 692 case BTB: {
JKleinRot 17:c694a0e63a89 693 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 694 lcd.printf(" BTB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 695 pc.printf("BTB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 696
JKleinRot 22:838a17065bc7 697 EMG.reset();
JKleinRot 22:838a17065bc7 698 EMG.start();
JKleinRot 23:5267c928ae2b 699
JKleinRot 23:5267c928ae2b 700 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 701 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 702 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 703
JKleinRot 22:838a17065bc7 704 EMG_meten();
JKleinRot 22:838a17065bc7 705 }
JKleinRot 17:c694a0e63a89 706
JKleinRot 19:38c9d177b6ee 707 while(state == BTB) {
JKleinRot 17:c694a0e63a89 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 17:c694a0e63a89 709 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 710
JKleinRot 22:838a17065bc7 711 EMG_meten();
JKleinRot 17:c694a0e63a89 712
JKleinRot 21:f4e9f6c28de1 713 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 714 state = BTBB; //Ga door naar state BTBB
JKleinRot 17:c694a0e63a89 715 }
JKleinRot 21:f4e9f6c28de1 716 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 717 state = BTBT; //Ga door naar state BTBT
JKleinRot 17:c694a0e63a89 718 }
JKleinRot 17:c694a0e63a89 719 }
JKleinRot 19:38c9d177b6ee 720 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 721 }
JKleinRot 19:38c9d177b6ee 722
JKleinRot 19:38c9d177b6ee 723 case BTT: {
JKleinRot 17:c694a0e63a89 724 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 725 lcd.printf(" BTT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 726 pc.printf("BTT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 727
JKleinRot 22:838a17065bc7 728 EMG.reset();
JKleinRot 22:838a17065bc7 729 EMG.start();
JKleinRot 23:5267c928ae2b 730
JKleinRot 23:5267c928ae2b 731 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 732 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 733 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 734
JKleinRot 22:838a17065bc7 735 EMG_meten();
JKleinRot 22:838a17065bc7 736 }
JKleinRot 17:c694a0e63a89 737
JKleinRot 19:38c9d177b6ee 738 while(state == BTT) {
JKleinRot 17:c694a0e63a89 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 17:c694a0e63a89 740 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 741
JKleinRot 22:838a17065bc7 742 EMG_meten();
JKleinRot 17:c694a0e63a89 743
JKleinRot 21:f4e9f6c28de1 744 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 745 state = BTTB; //Ga door naar state BTTB
JKleinRot 17:c694a0e63a89 746 }
JKleinRot 21:f4e9f6c28de1 747 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 748 state = BTTT; //Ga door naar state BTTT
JKleinRot 17:c694a0e63a89 749 }
JKleinRot 17:c694a0e63a89 750 }
JKleinRot 19:38c9d177b6ee 751 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 752 }
JKleinRot 19:38c9d177b6ee 753
JKleinRot 19:38c9d177b6ee 754 case TBB: {
JKleinRot 17:c694a0e63a89 755 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 756 lcd.printf(" TBB "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 757 pc.printf("TBB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 758
JKleinRot 22:838a17065bc7 759 EMG.reset();
JKleinRot 22:838a17065bc7 760 EMG.start();
JKleinRot 23:5267c928ae2b 761
JKleinRot 23:5267c928ae2b 762 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 763 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 764 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 765
JKleinRot 22:838a17065bc7 766 EMG_meten();
JKleinRot 22:838a17065bc7 767 }
JKleinRot 17:c694a0e63a89 768
JKleinRot 19:38c9d177b6ee 769 while(state == TBB) {
JKleinRot 17:c694a0e63a89 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 17:c694a0e63a89 771 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 772
JKleinRot 22:838a17065bc7 773 EMG_meten();
JKleinRot 17:c694a0e63a89 774
JKleinRot 21:f4e9f6c28de1 775 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 776 state = TBBB; //Ga door naar state TBBB
JKleinRot 17:c694a0e63a89 777 }
JKleinRot 21:f4e9f6c28de1 778 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 779 state = TBBT; //Ga door naar state TBBT
JKleinRot 17:c694a0e63a89 780 }
JKleinRot 17:c694a0e63a89 781 }
JKleinRot 19:38c9d177b6ee 782 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 783 }
JKleinRot 19:38c9d177b6ee 784
JKleinRot 19:38c9d177b6ee 785 case TBT: {
JKleinRot 17:c694a0e63a89 786 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 19:38c9d177b6ee 787 lcd.printf(" TBT "); //Tekst op LCD scherm
JKleinRot 19:38c9d177b6ee 788 pc.printf("TBT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 789
JKleinRot 22:838a17065bc7 790 EMG.reset();
JKleinRot 22:838a17065bc7 791 EMG.start();
JKleinRot 23:5267c928ae2b 792
JKleinRot 23:5267c928ae2b 793 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 794 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 795 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 796
JKleinRot 22:838a17065bc7 797 EMG_meten();
JKleinRot 22:838a17065bc7 798 }
JKleinRot 17:c694a0e63a89 799
JKleinRot 19:38c9d177b6ee 800 while(state == TBT) {
JKleinRot 17:c694a0e63a89 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 17:c694a0e63a89 802 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 803
JKleinRot 22:838a17065bc7 804 EMG_meten();
JKleinRot 17:c694a0e63a89 805
JKleinRot 21:f4e9f6c28de1 806 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 807 state = TBTB; //Ga door naar state TBTB
JKleinRot 17:c694a0e63a89 808 }
JKleinRot 21:f4e9f6c28de1 809 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 19:38c9d177b6ee 810 state = TBTT; //Ga door naar state TBTT
JKleinRot 17:c694a0e63a89 811 }
JKleinRot 17:c694a0e63a89 812 }
JKleinRot 19:38c9d177b6ee 813 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 814 }
JKleinRot 19:38c9d177b6ee 815
JKleinRot 19:38c9d177b6ee 816 case TTB: {
JKleinRot 17:c694a0e63a89 817 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 22:838a17065bc7 818 lcd.printf(" TTB "); //Tekst op LCD scherm
JKleinRot 22:838a17065bc7 819 pc.printf("TTB\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 820
JKleinRot 22:838a17065bc7 821 EMG.reset();
JKleinRot 22:838a17065bc7 822 EMG.start();
JKleinRot 23:5267c928ae2b 823
JKleinRot 23:5267c928ae2b 824 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 825 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 826 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 827
JKleinRot 22:838a17065bc7 828 EMG_meten();
JKleinRot 22:838a17065bc7 829 }
JKleinRot 17:c694a0e63a89 830
JKleinRot 22:838a17065bc7 831 while(state == TTB) {
JKleinRot 17:c694a0e63a89 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 17:c694a0e63a89 833 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 834
JKleinRot 22:838a17065bc7 835 EMG_meten();
JKleinRot 17:c694a0e63a89 836
JKleinRot 21:f4e9f6c28de1 837 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 838 state = TTBB; //Ga door naar state TTBB
JKleinRot 17:c694a0e63a89 839 }
JKleinRot 21:f4e9f6c28de1 840 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 841 state = TTBT; //Ga door naar state TTBT
JKleinRot 17:c694a0e63a89 842 }
JKleinRot 17:c694a0e63a89 843 }
JKleinRot 19:38c9d177b6ee 844 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 845 }
JKleinRot 19:38c9d177b6ee 846
JKleinRot 19:38c9d177b6ee 847 case TTT: {
JKleinRot 17:c694a0e63a89 848 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 849 lcd.printf(" TTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 850 pc.printf("TTT\n\r"); //Controle naar pc sturen
JKleinRot 23:5267c928ae2b 851
JKleinRot 22:838a17065bc7 852 EMG.reset();
JKleinRot 22:838a17065bc7 853 EMG.start();
JKleinRot 23:5267c928ae2b 854
JKleinRot 23:5267c928ae2b 855 while(EMG.read() <= 3) {
JKleinRot 22:838a17065bc7 856 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 857 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 23:5267c928ae2b 858
JKleinRot 22:838a17065bc7 859 EMG_meten();
JKleinRot 22:838a17065bc7 860 }
JKleinRot 17:c694a0e63a89 861
JKleinRot 19:38c9d177b6ee 862 while(state == TTT) {
JKleinRot 17:c694a0e63a89 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 17:c694a0e63a89 864 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 17:c694a0e63a89 865
JKleinRot 22:838a17065bc7 866 EMG_meten();
JKleinRot 17:c694a0e63a89 867
JKleinRot 21:f4e9f6c28de1 868 if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 869 state = TTTB; //Ga door naar state TTTB
JKleinRot 17:c694a0e63a89 870 }
JKleinRot 21:f4e9f6c28de1 871 if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
JKleinRot 17:c694a0e63a89 872 state = TTTT; //Ga door naar state TTTT
JKleinRot 17:c694a0e63a89 873 }
JKleinRot 17:c694a0e63a89 874 }
JKleinRot 19:38c9d177b6ee 875 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 876 }
JKleinRot 19:38c9d177b6ee 877
JKleinRot 19:38c9d177b6ee 878 case BBBB: {
JKleinRot 17:c694a0e63a89 879 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 880 lcd.printf(" BBBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 881 pc.printf("BBBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 882
JKleinRot 19:38c9d177b6ee 883 while(state == BBBB) {
JKleinRot 17:c694a0e63a89 884 //Motoractie
JKleinRot 19:38c9d177b6ee 885 }
JKleinRot 19:38c9d177b6ee 886 break; //Stop met alle acties in deze case
JKleinRot 19:38c9d177b6ee 887 }
JKleinRot 19:38c9d177b6ee 888
JKleinRot 19:38c9d177b6ee 889 case BBBT: {
JKleinRot 17:c694a0e63a89 890 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 891 lcd.printf(" BBBT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 892 pc.printf("BBBT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 893
JKleinRot 19:38c9d177b6ee 894 while(state == BBBT) {
JKleinRot 17:c694a0e63a89 895 //Motoractie
JKleinRot 19:38c9d177b6ee 896 }
JKleinRot 19:38c9d177b6ee 897 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 898 }
JKleinRot 19:38c9d177b6ee 899
JKleinRot 19:38c9d177b6ee 900 case BBTB: {
JKleinRot 17:c694a0e63a89 901 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 902 lcd.printf(" BBTB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 903 pc.printf("BBTB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 904
JKleinRot 19:38c9d177b6ee 905 while(state == BBTB) {
JKleinRot 17:c694a0e63a89 906 //Motoractie
JKleinRot 19:38c9d177b6ee 907 }
JKleinRot 19:38c9d177b6ee 908 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 909 }
JKleinRot 19:38c9d177b6ee 910
JKleinRot 19:38c9d177b6ee 911 case BBTT: {
JKleinRot 17:c694a0e63a89 912 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 913 lcd.printf(" BBTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 914 pc.printf("BBTT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 915
JKleinRot 19:38c9d177b6ee 916 while(state == BBTT) {
JKleinRot 17:c694a0e63a89 917 //Motoractie
JKleinRot 19:38c9d177b6ee 918 }
JKleinRot 19:38c9d177b6ee 919 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 920 }
JKleinRot 19:38c9d177b6ee 921
JKleinRot 19:38c9d177b6ee 922 case BTBB: {
JKleinRot 17:c694a0e63a89 923 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 924 lcd.printf(" BTBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 925 pc.printf("BTBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 926
JKleinRot 19:38c9d177b6ee 927 while(state == BTBB) {
JKleinRot 17:c694a0e63a89 928 //Motoractie
JKleinRot 19:38c9d177b6ee 929 }
JKleinRot 19:38c9d177b6ee 930 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 931 }
JKleinRot 19:38c9d177b6ee 932
JKleinRot 19:38c9d177b6ee 933 case BTBT: {
JKleinRot 17:c694a0e63a89 934 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 935 lcd.printf(" BTBT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 936 pc.printf("BTBT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 937
JKleinRot 19:38c9d177b6ee 938 while(state == BTBT) {
JKleinRot 17:c694a0e63a89 939 //Motoractie
JKleinRot 19:38c9d177b6ee 940 }
JKleinRot 19:38c9d177b6ee 941 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 942 }
JKleinRot 19:38c9d177b6ee 943
JKleinRot 19:38c9d177b6ee 944 case BTTB: {
JKleinRot 17:c694a0e63a89 945 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 946 lcd.printf(" BTTB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 947 pc.printf("BTTB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 948
JKleinRot 19:38c9d177b6ee 949 while(state == BTTB) {
JKleinRot 17:c694a0e63a89 950 //Motoractie
JKleinRot 19:38c9d177b6ee 951 }
JKleinRot 19:38c9d177b6ee 952 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 953 }
JKleinRot 19:38c9d177b6ee 954
JKleinRot 19:38c9d177b6ee 955 case BTTT: {
JKleinRot 17:c694a0e63a89 956 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 957 lcd.printf(" BTTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 958 pc.printf("BTTT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 959
JKleinRot 19:38c9d177b6ee 960 while(state == BTTT) {
JKleinRot 17:c694a0e63a89 961 //Motoractie
JKleinRot 19:38c9d177b6ee 962 }
JKleinRot 19:38c9d177b6ee 963 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 964 }
JKleinRot 19:38c9d177b6ee 965
JKleinRot 19:38c9d177b6ee 966 case TBBB: {
JKleinRot 17:c694a0e63a89 967 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 968 lcd.printf(" TBBB "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 969 pc.printf("TBBB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 970
JKleinRot 19:38c9d177b6ee 971 while(state == TBBB) {
JKleinRot 17:c694a0e63a89 972 //Motoractie
JKleinRot 19:38c9d177b6ee 973 }
JKleinRot 19:38c9d177b6ee 974 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 975 }
JKleinRot 19:38c9d177b6ee 976
JKleinRot 19:38c9d177b6ee 977 case TBBT: {
JKleinRot 17:c694a0e63a89 978 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 979 lcd.printf(" TBBT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 980 pc.printf("TBBT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 981
JKleinRot 19:38c9d177b6ee 982 while(state == TBBT) {
JKleinRot 17:c694a0e63a89 983 //Motoractie
JKleinRot 19:38c9d177b6ee 984 }
JKleinRot 19:38c9d177b6ee 985 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 986 }
JKleinRot 19:38c9d177b6ee 987
JKleinRot 19:38c9d177b6ee 988 case TBTB: {
JKleinRot 17:c694a0e63a89 989 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 22:838a17065bc7 990 lcd.printf(" TBTB "); //Tekst op LCD scherm
JKleinRot 22:838a17065bc7 991 pc.printf("TBTB\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 992
JKleinRot 22:838a17065bc7 993 while(state == TBTB) {
JKleinRot 17:c694a0e63a89 994 //Motoractie
JKleinRot 19:38c9d177b6ee 995 }
JKleinRot 19:38c9d177b6ee 996 break; //Stop met alle acties in deze case
JKleinRot 17:c694a0e63a89 997 }
JKleinRot 19:38c9d177b6ee 998
JKleinRot 23:5267c928ae2b 999 case TBTT: { //Motoraansturing voor richten op doel middenboven
JKleinRot 17:c694a0e63a89 1000 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 17:c694a0e63a89 1001 lcd.printf(" TBTT "); //Tekst op LCD scherm
JKleinRot 17:c694a0e63a89 1002 pc.printf("TBTT\n\r"); //Controle naar pc sturen
JKleinRot 17:c694a0e63a89 1003
JKleinRot 19:38c9d177b6ee 1004 while(state == TBTT) {
JKleinRot 17:c694a0e63a89 1005 //Motoractie
JKleinRot 19:38c9d177b6ee 1006 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 1007 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 19:38c9d177b6ee 1008
JKleinRot 19:38c9d177b6ee 1009 //Positie arm 1 is goed
JKleinRot 19:38c9d177b6ee 1010 //Snelheid arm 2
JKleinRot 19:38c9d177b6ee 1011
JKleinRot 23:5267c928ae2b 1012 while(puls_motor_arm1.getPosition() > 48) {
JKleinRot 23:5267c928ae2b 1013 referentie_arm1 = referentie_arm1 - 132.0/200.0;
JKleinRot 23:5267c928ae2b 1014 pc.printf("referentie arm 1 %f ", referentie_arm1);
JKleinRot 23:5267c928ae2b 1015 pc.printf("get position 1 %d ", puls_motor_arm1.getPosition());
JKleinRot 23:5267c928ae2b 1016 pc.printf("pwm motor 1 %f\n\r", pwm_to_motor_arm1);
JKleinRot 23:5267c928ae2b 1017 }
JKleinRot 23:5267c928ae2b 1018 pc.printf("arm 1 op positie");
JKleinRot 19:38c9d177b6ee 1019
JKleinRot 23:5267c928ae2b 1020 while(puls_motor_arm2.getPosition() < 167) {
JKleinRot 23:5267c928ae2b 1021 referentie_arm2 = referentie_arm2 + 44.0/200.0;
JKleinRot 23:5267c928ae2b 1022 pc.printf("referentie arm 2 %f ", referentie_arm2);
JKleinRot 23:5267c928ae2b 1023 pc.printf("get position 2 %d ", puls_motor_arm2.getPosition());
JKleinRot 23:5267c928ae2b 1024 pc.printf("pwm motor 2 %f\n\r", pwm_to_motor_arm2);
JKleinRot 23:5267c928ae2b 1025 }
JKleinRot 23:5267c928ae2b 1026 pc.printf("arm2 op positie");
JKleinRot 23:5267c928ae2b 1027
JKleinRot 23:5267c928ae2b 1028
JKleinRot 23:5267c928ae2b 1029 if(referentie_arm2 >= 167) {
JKleinRot 23:5267c928ae2b 1030 while(puls_motor_arm2.getPosition() > -8) {
JKleinRot 23:5267c928ae2b 1031
JKleinRot 23:5267c928ae2b 1032
JKleinRot 23:5267c928ae2b 1033
JKleinRot 23:5267c928ae2b 1034 referentie_arm2 = -0.5 * 200000 * t * t;
JKleinRot 23:5267c928ae2b 1035 t = t + 0.005;
JKleinRot 23:5267c928ae2b 1036
JKleinRot 23:5267c928ae2b 1037 pc.printf("referentie arm 2 %f ", referentie_arm2);
JKleinRot 23:5267c928ae2b 1038 pc.printf("get position %d ", puls_motor_arm2.getPosition());
JKleinRot 23:5267c928ae2b 1039 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
JKleinRot 23:5267c928ae2b 1040 pc.printf("t is %f\n\r", t);
JKleinRot 23:5267c928ae2b 1041 }
JKleinRot 23:5267c928ae2b 1042
JKleinRot 23:5267c928ae2b 1043
JKleinRot 23:5267c928ae2b 1044 if(puls_motor_arm2.getPosition() <= -8) {
JKleinRot 23:5267c928ae2b 1045 wait(1);
JKleinRot 24:a1fdc830f96c 1046 while(puls_motor_arm2.getPosition() < 167) {
JKleinRot 23:5267c928ae2b 1047 referentie_arm2 = referentie_arm2 + 167.0/200.0;
JKleinRot 23:5267c928ae2b 1048
JKleinRot 23:5267c928ae2b 1049
JKleinRot 23:5267c928ae2b 1050 pc.printf("referentie arm 2 %f ", referentie_arm2);
JKleinRot 23:5267c928ae2b 1051 pc.printf("get position %d ", puls_motor_arm2.getPosition());
JKleinRot 23:5267c928ae2b 1052 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
JKleinRot 23:5267c928ae2b 1053 }
JKleinRot 23:5267c928ae2b 1054
JKleinRot 24:a1fdc830f96c 1055 if(puls_motor_arm2.getPosition() >= 167) {
JKleinRot 23:5267c928ae2b 1056 pc.printf("staat stil\n\r");
JKleinRot 23:5267c928ae2b 1057 state = WACHT;
JKleinRot 23:5267c928ae2b 1058 }
JKleinRot 21:f4e9f6c28de1 1059 }
JKleinRot 19:38c9d177b6ee 1060 }
JKleinRot 19:38c9d177b6ee 1061 }
JKleinRot 24:a1fdc830f96c 1062 break; //Stop met alle acties in deze case
JKleinRot 24:a1fdc830f96c 1063 }
JKleinRot 23:5267c928ae2b 1064
JKleinRot 24:a1fdc830f96c 1065 case TTBB: { //Motoraansturing voor richten op doel linksonder
JKleinRot 24:a1fdc830f96c 1066 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 24:a1fdc830f96c 1067 lcd.printf(" TTBB "); //Tekst op LCD scherm
JKleinRot 24:a1fdc830f96c 1068 pc.printf("TTBB\n\r"); //Controle naar pc sturen
JKleinRot 24:a1fdc830f96c 1069
JKleinRot 24:a1fdc830f96c 1070 while(state == TTBB) {
JKleinRot 24:a1fdc830f96c 1071 //Motoractie
JKleinRot 24:a1fdc830f96c 1072 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 1073 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 24:a1fdc830f96c 1074
JKleinRot 24:a1fdc830f96c 1075 //Positie arm 1 is goed
JKleinRot 24:a1fdc830f96c 1076 //Snelheid arm 2
JKleinRot 24:a1fdc830f96c 1077
JKleinRot 24:a1fdc830f96c 1078 referentie_arm2 = 0.5 * 100000 * t * t;
JKleinRot 24:a1fdc830f96c 1079 t = t + 0.005;
JKleinRot 24:a1fdc830f96c 1080
JKleinRot 24:a1fdc830f96c 1081 pc.printf("referentie arm 2 %f ", referentie_arm2);
JKleinRot 24:a1fdc830f96c 1082 pc.printf("get position %d ", puls_motor_arm2.getPosition());
JKleinRot 24:a1fdc830f96c 1083 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
JKleinRot 24:a1fdc830f96c 1084 pc.printf("t is %f\n\r", t);
JKleinRot 24:a1fdc830f96c 1085
JKleinRot 24:a1fdc830f96c 1086 if(puls_motor_arm2.getPosition() >= 175) {
JKleinRot 24:a1fdc830f96c 1087 wait(1);
JKleinRot 24:a1fdc830f96c 1088 while(puls_motor_arm2.getPosition() > 0) {
JKleinRot 24:a1fdc830f96c 1089 referentie_arm2 = referentie_arm2 - 175.0/200.0;
JKleinRot 24:a1fdc830f96c 1090
JKleinRot 24:a1fdc830f96c 1091
JKleinRot 24:a1fdc830f96c 1092 pc.printf("referentie arm 2 %f ", referentie_arm2);
JKleinRot 24:a1fdc830f96c 1093 pc.printf("get position %d ", puls_motor_arm2.getPosition());
JKleinRot 24:a1fdc830f96c 1094 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
JKleinRot 24:a1fdc830f96c 1095 }
JKleinRot 24:a1fdc830f96c 1096
JKleinRot 24:a1fdc830f96c 1097
JKleinRot 24:a1fdc830f96c 1098 if(puls_motor_arm2.getPosition() <= 0) {
JKleinRot 24:a1fdc830f96c 1099 pc.printf("staat stil\n\r");
JKleinRot 24:a1fdc830f96c 1100 state = WACHT;
JKleinRot 24:a1fdc830f96c 1101 }
JKleinRot 24:a1fdc830f96c 1102
JKleinRot 23:5267c928ae2b 1103 }
JKleinRot 24:a1fdc830f96c 1104 }
JKleinRot 24:a1fdc830f96c 1105 break; //Stop met alle acties in deze case
JKleinRot 24:a1fdc830f96c 1106 }
JKleinRot 23:5267c928ae2b 1107
JKleinRot 24:a1fdc830f96c 1108 case TTBT: { //Motoraansturing voor terug naar kalibratiepositie
JKleinRot 24:a1fdc830f96c 1109 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 24:a1fdc830f96c 1110 lcd.printf(" TTBT "); //Tekst op LCD scherm
JKleinRot 24:a1fdc830f96c 1111 pc.printf("TTBT\n\r"); //Controle naar pc sturen
JKleinRot 24:a1fdc830f96c 1112
JKleinRot 24:a1fdc830f96c 1113 while(state == TTBT) {
JKleinRot 24:a1fdc830f96c 1114 //Motoractie
JKleinRot 24:a1fdc830f96c 1115 lcd.printf(" GEEN DOEL ");
JKleinRot 24:a1fdc830f96c 1116 wait(1);
JKleinRot 24:a1fdc830f96c 1117 state = WACHT;
JKleinRot 24:a1fdc830f96c 1118 }
JKleinRot 24:a1fdc830f96c 1119 break; //Stop met alle acties in deze case
JKleinRot 24:a1fdc830f96c 1120 }
JKleinRot 24:a1fdc830f96c 1121
JKleinRot 24:a1fdc830f96c 1122 case TTTB: { //Motoraansturing voor richten op doel linksmidden
JKleinRot 24:a1fdc830f96c 1123 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 24:a1fdc830f96c 1124 lcd.printf(" TTTB "); //Tekst op LCD scherm
JKleinRot 24:a1fdc830f96c 1125 pc.printf("TTTB\n\r"); //Controle naar pc sturen
JKleinRot 24:a1fdc830f96c 1126
JKleinRot 24:a1fdc830f96c 1127 while(state == TTTB) {
JKleinRot 24:a1fdc830f96c 1128 //Motoractie
JKleinRot 24:a1fdc830f96c 1129
JKleinRot 24:a1fdc830f96c 1130 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 1131 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 24:a1fdc830f96c 1132
JKleinRot 24:a1fdc830f96c 1133 //Positie arm 1 is goed
JKleinRot 24:a1fdc830f96c 1134 //Snelheid arm 2
JKleinRot 24:a1fdc830f96c 1135
JKleinRot 24:a1fdc830f96c 1136 referentie_arm2 = 0.5 * 150000 * t * t;
JKleinRot 24:a1fdc830f96c 1137 t = t + 0.005;
JKleinRot 24:a1fdc830f96c 1138
JKleinRot 24:a1fdc830f96c 1139 pc.printf("referentie arm 2 %f ", referentie_arm2);
JKleinRot 24:a1fdc830f96c 1140 pc.printf("get position %d ", puls_motor_arm2.getPosition());
JKleinRot 24:a1fdc830f96c 1141 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
JKleinRot 24:a1fdc830f96c 1142 pc.printf("t is %f\n\r", t);
JKleinRot 24:a1fdc830f96c 1143
JKleinRot 24:a1fdc830f96c 1144 if(puls_motor_arm2.getPosition() >= 175) {
JKleinRot 24:a1fdc830f96c 1145 wait(1);
JKleinRot 24:a1fdc830f96c 1146 while(puls_motor_arm2.getPosition() > 0) {
JKleinRot 24:a1fdc830f96c 1147 referentie_arm2 = referentie_arm2 - 175.0/200.0;
JKleinRot 24:a1fdc830f96c 1148
JKleinRot 24:a1fdc830f96c 1149
JKleinRot 24:a1fdc830f96c 1150 pc.printf("referentie arm 2 %f ", referentie_arm2);
JKleinRot 24:a1fdc830f96c 1151 pc.printf("get position %d ", puls_motor_arm2.getPosition());
JKleinRot 24:a1fdc830f96c 1152 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
JKleinRot 24:a1fdc830f96c 1153 }
JKleinRot 24:a1fdc830f96c 1154
JKleinRot 24:a1fdc830f96c 1155
JKleinRot 24:a1fdc830f96c 1156 if(puls_motor_arm2.getPosition() <= 0) {
JKleinRot 24:a1fdc830f96c 1157 pc.printf("staat stil\n\r");
JKleinRot 24:a1fdc830f96c 1158 state = WACHT;
JKleinRot 24:a1fdc830f96c 1159 }
JKleinRot 24:a1fdc830f96c 1160
JKleinRot 23:5267c928ae2b 1161 }
JKleinRot 23:5267c928ae2b 1162 }
JKleinRot 19:38c9d177b6ee 1163
JKleinRot 24:a1fdc830f96c 1164 break; //Stop met alle acties in deze case
JKleinRot 24:a1fdc830f96c 1165 }
JKleinRot 24:a1fdc830f96c 1166
JKleinRot 24:a1fdc830f96c 1167
JKleinRot 24:a1fdc830f96c 1168 case TTTT: { //Motoraansturing voor richten op doel linksboven
JKleinRot 24:a1fdc830f96c 1169 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 24:a1fdc830f96c 1170 lcd.printf(" TTTT "); //Tekst op LCD scherm
JKleinRot 24:a1fdc830f96c 1171 pc.printf("TTBB\n\r"); //Controle naar pc sturen
JKleinRot 24:a1fdc830f96c 1172
JKleinRot 24:a1fdc830f96c 1173 while(state == TTTT) {
JKleinRot 24:a1fdc830f96c 1174 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 1175 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 24:a1fdc830f96c 1176
JKleinRot 24:a1fdc830f96c 1177 //Positie arm 1 is goed
JKleinRot 24:a1fdc830f96c 1178 //Snelheid arm 2
JKleinRot 24:a1fdc830f96c 1179
JKleinRot 24:a1fdc830f96c 1180 referentie_arm2 = 0.5 * 200000 * t * t;
JKleinRot 24:a1fdc830f96c 1181 t = t + 0.005;
JKleinRot 24:a1fdc830f96c 1182
JKleinRot 24:a1fdc830f96c 1183 pc.printf("referentie arm 2 %f ", referentie_arm2);
JKleinRot 24:a1fdc830f96c 1184 pc.printf("get position %d ", puls_motor_arm2.getPosition());
JKleinRot 24:a1fdc830f96c 1185 pc.printf("pwm motor 2 %f", pwm_to_motor_arm2);
JKleinRot 24:a1fdc830f96c 1186 pc.printf("t is %f\n\r", t);
JKleinRot 24:a1fdc830f96c 1187
JKleinRot 24:a1fdc830f96c 1188 if(puls_motor_arm2.getPosition() >= 175) {
JKleinRot 24:a1fdc830f96c 1189 wait(1);
JKleinRot 24:a1fdc830f96c 1190 while(puls_motor_arm2.getPosition() > 0) {
JKleinRot 24:a1fdc830f96c 1191 referentie_arm2 = referentie_arm2 - 175.0/200.0;
JKleinRot 24:a1fdc830f96c 1192
JKleinRot 24:a1fdc830f96c 1193
JKleinRot 24:a1fdc830f96c 1194 pc.printf("referentie arm 2 %f ", referentie_arm2);
JKleinRot 24:a1fdc830f96c 1195 pc.printf("get position %d ", puls_motor_arm2.getPosition());
JKleinRot 24:a1fdc830f96c 1196 pc.printf("pwm motor 2 %f\n\r ", pwm_to_motor_arm2);
JKleinRot 24:a1fdc830f96c 1197 }
JKleinRot 24:a1fdc830f96c 1198
JKleinRot 24:a1fdc830f96c 1199
JKleinRot 24:a1fdc830f96c 1200 if(puls_motor_arm2.getPosition() <= 0) {
JKleinRot 24:a1fdc830f96c 1201 pc.printf("staat stil\n\r");
JKleinRot 24:a1fdc830f96c 1202 state = WACHT;
JKleinRot 24:a1fdc830f96c 1203 }
JKleinRot 24:a1fdc830f96c 1204
JKleinRot 24:a1fdc830f96c 1205 }
JKleinRot 24:a1fdc830f96c 1206 }
JKleinRot 24:a1fdc830f96c 1207 }
JKleinRot 24:a1fdc830f96c 1208
JKleinRot 24:a1fdc830f96c 1209 case THUISPOSITIE_MIDDEN: {
JKleinRot 24:a1fdc830f96c 1210 while(puls_motor_arm2.getPosition() > 123) {
JKleinRot 24:a1fdc830f96c 1211 referentie_arm2 = referentie_arm2 - 44.0/200.0;
JKleinRot 24:a1fdc830f96c 1212 }
JKleinRot 24:a1fdc830f96c 1213
JKleinRot 24:a1fdc830f96c 1214 while(puls_motor_arm1.getPosition() < 180) {
JKleinRot 24:a1fdc830f96c 1215 referentie_arm1 = referentie_arm1 + 132.0/200.0;
JKleinRot 24:a1fdc830f96c 1216 }
JKleinRot 24:a1fdc830f96c 1217
JKleinRot 24:a1fdc830f96c 1218 state = WACHT;
JKleinRot 24:a1fdc830f96c 1219 break;
JKleinRot 24:a1fdc830f96c 1220 }
JKleinRot 24:a1fdc830f96c 1221
JKleinRot 24:a1fdc830f96c 1222 case WACHT: {
JKleinRot 24:a1fdc830f96c 1223 lcd.printf(" WACHT ");
JKleinRot 24:a1fdc830f96c 1224 wait(5);
JKleinRot 24:a1fdc830f96c 1225 state = START;
JKleinRot 23:5267c928ae2b 1226
JKleinRot 23:5267c928ae2b 1227
JKleinRot 23:5267c928ae2b 1228
JKleinRot 23:5267c928ae2b 1229
JKleinRot 24:a1fdc830f96c 1230 default: {
JKleinRot 24:a1fdc830f96c 1231 //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
JKleinRot 24:a1fdc830f96c 1232 state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
JKleinRot 24:a1fdc830f96c 1233 }
JKleinRot 23:5267c928ae2b 1234
JKleinRot 24:a1fdc830f96c 1235 pc.printf("state: %u\n\r",state);
JKleinRot 23:5267c928ae2b 1236 }
JKleinRot 24:a1fdc830f96c 1237 //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 9:454e7da8ab65 1238 }
JKleinRot 20:1cb0cf0d49ac 1239 }
JKleinRot 24:a1fdc830f96c 1240 }