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@20:1cb0cf0d49ac, 2014-10-30 (annotated)

Committer:
JKleinRot
Date:
Thu Oct 30 19:29:31 2014 +0000
Revision:
20:1cb0cf0d49ac
Parent:
19:38c9d177b6ee
Child:
21:f4e9f6c28de1
2014-10-30 Regelaar beide motoren als eerste in mainscript met ticker. Motor 1 met P regelaar rustig naar de goede positie. Referentie loopt op

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