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@15:3ebd0e666a9c, 2014-10-28 (annotated)

Committer:
JKleinRot
Date:
Tue Oct 28 11:11:13 2014 +0000
Revision:
15:3ebd0e666a9c
Parent:
14:e1816efa712d
Child:
16:c34c5d9dfe1a
2014-10-28 Timer werkt, printf ook. While loop voor de timer ipv if loop, anders niet terug in de loop.

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 15:3ebd0e666a9c 9 #define KP_arm1 0.01 //Factor proprotionele regelaar arm 1
JKleinRot 13:54ee98850a15 10 #define KI_arm1 0 //Factor integraal regelaar arm 1
JKleinRot 15:3ebd0e666a9c 11 #define KD_arm1 0 //Factor afgeleide regelaar arm 1
JKleinRot 15:3ebd0e666a9c 12 #define KP_arm2 0.01 //Factor proprotionele regelaar arm 2
JKleinRot 13:54ee98850a15 13 #define KI_arm2 0 //Factor integraal regelaar arm 2
JKleinRot 15:3ebd0e666a9c 14 #define KD_arm2 0 //Factor afgeleide regelaar arm 2
JKleinRot 11:e9b906b5f572 15 #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
JKleinRot 11:e9b906b5f572 16 #define PULS_ARM1_HOME_KALIBRATIE 180 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
JKleinRot 11:e9b906b5f572 17 #define PULS_ARM2_HOME_KALIBRATIE 393 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
JKleinRot 0:859c89785d3f 18
JKleinRot 13:54ee98850a15 19 //High Pass filter Filtercoëfficiënten
JKleinRot 15:3ebd0e666a9c 20 #define A1 1
JKleinRot 15:3ebd0e666a9c 21 #define A2 -1.5610
JKleinRot 15:3ebd0e666a9c 22 #define A3 0.6414
JKleinRot 15:3ebd0e666a9c 23 #define B1 0.0201
JKleinRot 15:3ebd0e666a9c 24 #define B2 0.0402
JKleinRot 15:3ebd0e666a9c 25 #define B3 0.0201
JKleinRot 13:54ee98850a15 26
JKleinRot 13:54ee98850a15 27 //Notch filter Filtercoëfficiënten
JKleinRot 13:54ee98850a15 28 #define C1 1
JKleinRot 13:54ee98850a15 29 #define C2 -1.1873E-16
JKleinRot 13:54ee98850a15 30 #define C3 0.9391
JKleinRot 13:54ee98850a15 31 #define D1 0.9695
JKleinRot 13:54ee98850a15 32 #define D2 -1.1873E-16
JKleinRot 13:54ee98850a15 33 #define D3 0.9695
JKleinRot 13:54ee98850a15 34
JKleinRot 13:54ee98850a15 35 //Low pass filter Filtercoëfficiënten
JKleinRot 13:54ee98850a15 36 #define E1 1
JKleinRot 15:3ebd0e666a9c 37 #define E2 -1.9645
JKleinRot 15:3ebd0e666a9c 38 #define E3 0.9651
JKleinRot 15:3ebd0e666a9c 39 #define F1 1.5515E-4
JKleinRot 15:3ebd0e666a9c 40 #define F2 3.1030E-4
JKleinRot 15:3ebd0e666a9c 41 #define F3 1.5515E-4
JKleinRot 15:3ebd0e666a9c 42
JKleinRot 13:54ee98850a15 43
JKleinRot 0:859c89785d3f 44 //Pinverdeling en naamgeving variabelen
JKleinRot 0:859c89785d3f 45 TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm
JKleinRot 0:859c89785d3f 46 MODSERIAL pc(USBTX, USBRX); //PC
JKleinRot 0:859c89785d3f 47
JKleinRot 1:e5e1eb9d0025 48 PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
JKleinRot 9:454e7da8ab65 49 DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1
JKleinRot 12:996f9f8e3acc 50 Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit)
JKleinRot 11:e9b906b5f572 51 PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2
JKleinRot 9:454e7da8ab65 52 DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2
JKleinRot 12:996f9f8e3acc 53 Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit)
JKleinRot 6:3b6fad529520 54 AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps
JKleinRot 15:3ebd0e666a9c 55 AnalogIn EMG_tri(PTB2);
JKleinRot 12:996f9f8e3acc 56 //Blauw op 3,3 V en groen op GND
JKleinRot 1:e5e1eb9d0025 57
JKleinRot 6:3b6fad529520 58 Ticker ticker_regelaar; //Ticker voor regelaar motor
JKleinRot 6:3b6fad529520 59 Ticker ticker_EMG; //Ticker voor EMG meten
JKleinRot 14:e1816efa712d 60 Timer biceps_kalibratie;
JKleinRot 15:3ebd0e666a9c 61 Timer triceps_kalibratie;
JKleinRot 1:e5e1eb9d0025 62
JKleinRot 9:454e7da8ab65 63 //States definiëren
JKleinRot 15:3ebd0e666a9c 64 enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, BEGIN_METEN, B, T}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
JKleinRot 9:454e7da8ab65 65 uint8_t state = RUST; //State is rust aan het begin
JKleinRot 9:454e7da8ab65 66
JKleinRot 6:3b6fad529520 67 //Gedefinieerde datatypen en naamgeving en beginwaarden
JKleinRot 6:3b6fad529520 68 char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm
JKleinRot 6:3b6fad529520 69 char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm
JKleinRot 0:859c89785d3f 70
JKleinRot 11:e9b906b5f572 71 float pwm_to_motor_arm1; //PWM output naar motor arm 1
JKleinRot 11:e9b906b5f572 72 float pwm_to_motor_arm2; //PWM output naar motor arm 2
JKleinRot 13:54ee98850a15 73 float error_arm1_kalibratie; //Error in pulsen arm 1
JKleinRot 13:54ee98850a15 74 float vorige_error_arm1 = 0; //Derivative actie van regelaar arm 1
JKleinRot 13:54ee98850a15 75 float integraal_arm1 = 0; //Integraal actie van regelaar arm 1
JKleinRot 13:54ee98850a15 76 float afgeleide_arm1; //Afgeleide actie van regelaar arm 1
JKleinRot 13:54ee98850a15 77 float error_arm2_kalibratie; //Error in pulsen arm 2
JKleinRot 13:54ee98850a15 78 float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2
JKleinRot 13:54ee98850a15 79 float integraal_arm2 = 0; //Integraal actie van regelaar arm 2
JKleinRot 13:54ee98850a15 80 float afgeleide_arm2; //Afgeleide actie van regelaar arm 2
JKleinRot 13:54ee98850a15 81 float xb; //Gemeten EMG waarde biceps
JKleinRot 15:3ebd0e666a9c 82 float xt;
JKleinRot 15:3ebd0e666a9c 83
JKleinRot 15:3ebd0e666a9c 84 arm_biquad_casd_df1_inst_f32 highpass_biceps;
JKleinRot 15:3ebd0e666a9c 85 float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3};
JKleinRot 15:3ebd0e666a9c 86 float highpass_biceps_states[4];
JKleinRot 13:54ee98850a15 87
JKleinRot 15:3ebd0e666a9c 88 arm_biquad_casd_df1_inst_f32 notch_biceps;
JKleinRot 15:3ebd0e666a9c 89 float notch_biceps_const[] = {D1, D2, D3, -C2, -C3};
JKleinRot 15:3ebd0e666a9c 90 float notch_biceps_states[4];
JKleinRot 15:3ebd0e666a9c 91
JKleinRot 15:3ebd0e666a9c 92 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
JKleinRot 15:3ebd0e666a9c 93 float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3};
JKleinRot 15:3ebd0e666a9c 94 float lowpass_biceps_states[4];
JKleinRot 15:3ebd0e666a9c 95
JKleinRot 15:3ebd0e666a9c 96 float xbf;
JKleinRot 15:3ebd0e666a9c 97 float xbfmax = 0;
JKleinRot 13:54ee98850a15 98
JKleinRot 15:3ebd0e666a9c 99 arm_biquad_casd_df1_inst_f32 highpass_triceps;
JKleinRot 15:3ebd0e666a9c 100 float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3};
JKleinRot 15:3ebd0e666a9c 101 float highpass_triceps_states[4];
JKleinRot 13:54ee98850a15 102
JKleinRot 15:3ebd0e666a9c 103 arm_biquad_casd_df1_inst_f32 notch_triceps;
JKleinRot 15:3ebd0e666a9c 104 float notch_triceps_const[] = {D1, D2, D3, -C2, -C3};
JKleinRot 15:3ebd0e666a9c 105 float notch_triceps_states[4];
JKleinRot 13:54ee98850a15 106
JKleinRot 15:3ebd0e666a9c 107 arm_biquad_casd_df1_inst_f32 lowpass_triceps;
JKleinRot 15:3ebd0e666a9c 108 float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3};
JKleinRot 15:3ebd0e666a9c 109 float lowpass_triceps_states[4];
JKleinRot 13:54ee98850a15 110
JKleinRot 15:3ebd0e666a9c 111 float xtf;
JKleinRot 15:3ebd0e666a9c 112 float xtfmax = 0;
JKleinRot 15:3ebd0e666a9c 113
JKleinRot 15:3ebd0e666a9c 114 float xbt;
JKleinRot 15:3ebd0e666a9c 115 float xtt;
JKleinRot 7:dd3cba61b34b 116
JKleinRot 6:3b6fad529520 117 volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 118 void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
JKleinRot 10:52b22bff409a 119 {
JKleinRot 10:52b22bff409a 120 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 121 }
JKleinRot 1:e5e1eb9d0025 122
JKleinRot 6:3b6fad529520 123 volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 124 void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
JKleinRot 10:52b22bff409a 125 {
JKleinRot 10:52b22bff409a 126 regelaar_EMG_flag = true;
JKleinRot 4:69540b9c0646 127 }
JKleinRot 4:69540b9c0646 128
JKleinRot 10:52b22bff409a 129 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 130 {
JKleinRot 10:52b22bff409a 131 if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum
JKleinRot 2:0cb899f2800a 132 *in = min;
JKleinRot 2:0cb899f2800a 133 }
JKleinRot 10:52b22bff409a 134 if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum
JKleinRot 2:0cb899f2800a 135 *in = max;
JKleinRot 10:52b22bff409a 136 } else { //In alle andere gevallen is de waarde de waarde zelf
JKleinRot 2:0cb899f2800a 137 *in = *in;
JKleinRot 2:0cb899f2800a 138 }
JKleinRot 2:0cb899f2800a 139 }
JKleinRot 1:e5e1eb9d0025 140
JKleinRot 10:52b22bff409a 141 void arm1_naar_thuispositie()
JKleinRot 10:52b22bff409a 142 {
JKleinRot 13:54ee98850a15 143 error_arm1_kalibratie = (PULS_ARM1_HOME_KALIBRATIE - puls_motor_arm1.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
JKleinRot 13:54ee98850a15 144 integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
JKleinRot 13:54ee98850a15 145 afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
JKleinRot 13:54ee98850a15 146 pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1; //Output naar motor na PID regelaar
JKleinRot 7:dd3cba61b34b 147 keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 10:52b22bff409a 148
JKleinRot 11:e9b906b5f572 149 if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 10:52b22bff409a 150 dir_motor_arm1.write(1);
JKleinRot 11:e9b906b5f572 151 } else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 152 dir_motor_arm1.write(0);
JKleinRot 7:dd3cba61b34b 153 }
JKleinRot 11:e9b906b5f572 154 pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 10:52b22bff409a 155 pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition());
JKleinRot 9:454e7da8ab65 156 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1);
JKleinRot 10:52b22bff409a 157
JKleinRot 11:e9b906b5f572 158 if (pwm_to_motor_arm1 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
JKleinRot 11:e9b906b5f572 159 state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 13:54ee98850a15 160 pc.printf("KALIBRATIE_ARM1 afgerond\n"); //Tekst voor controle Arm 1 naar thuispositie
JKleinRot 10:52b22bff409a 161 }
JKleinRot 10:52b22bff409a 162 }
JKleinRot 7:dd3cba61b34b 163
JKleinRot 10:52b22bff409a 164 void arm2_naar_thuispositie()
JKleinRot 10:52b22bff409a 165 {
JKleinRot 13:54ee98850a15 166 error_arm2_kalibratie = (PULS_ARM2_HOME_KALIBRATIE - puls_motor_arm2.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
JKleinRot 13:54ee98850a15 167 integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
JKleinRot 13:54ee98850a15 168 afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
JKleinRot 13:54ee98850a15 169 pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2; //Output naar motor na PID regelaar
JKleinRot 7:dd3cba61b34b 170 keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 10:52b22bff409a 171
JKleinRot 11:e9b906b5f572 172 if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1
JKleinRot 7:dd3cba61b34b 173 dir_motor_arm2.write(1);
JKleinRot 11:e9b906b5f572 174 } else { //Anders richting nul
JKleinRot 7:dd3cba61b34b 175 dir_motor_arm2.write(0);
JKleinRot 7:dd3cba61b34b 176 }
JKleinRot 11:e9b906b5f572 177 pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 13:54ee98850a15 178 pc.printf("pulsmotorgetposition %d ", puls_motor_arm2.getPosition());
JKleinRot 13:54ee98850a15 179 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2);
JKleinRot 10:52b22bff409a 180
JKleinRot 11:e9b906b5f572 181 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 14:e1816efa712d 182 state = EMG_KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 14:e1816efa712d 183 pc.printf("KALIBRATIE_ARM2 afgerond\n\r"); //Tekst voor controle Arm 2 naar thuispositie
JKleinRot 7:dd3cba61b34b 184 }
JKleinRot 7:dd3cba61b34b 185 }
JKleinRot 0:859c89785d3f 186
JKleinRot 13:54ee98850a15 187 void filter_biceps()
JKleinRot 13:54ee98850a15 188 {
JKleinRot 15:3ebd0e666a9c 189 arm_biquad_cascade_df1_f32(&highpass_biceps, &xb, &xbf, 1);
JKleinRot 15:3ebd0e666a9c 190
JKleinRot 15:3ebd0e666a9c 191 arm_biquad_cascade_df1_f32(&notch_biceps, &xbf, &xbf, 1);
JKleinRot 15:3ebd0e666a9c 192
JKleinRot 15:3ebd0e666a9c 193 xbf = fabs(xbf);
JKleinRot 15:3ebd0e666a9c 194
JKleinRot 15:3ebd0e666a9c 195 arm_biquad_cascade_df1_f32(&lowpass_biceps, &xbf, &xbf, 1);
JKleinRot 13:54ee98850a15 196
JKleinRot 15:3ebd0e666a9c 197 pc.printf("xbf is %f\n\r", xbf);
JKleinRot 15:3ebd0e666a9c 198
JKleinRot 15:3ebd0e666a9c 199
JKleinRot 13:54ee98850a15 200 }
JKleinRot 13:54ee98850a15 201
JKleinRot 15:3ebd0e666a9c 202 void filter_triceps()
JKleinRot 15:3ebd0e666a9c 203 {
JKleinRot 15:3ebd0e666a9c 204 arm_biquad_cascade_df1_f32(&highpass_triceps, &xt, &xtf, 1);
JKleinRot 15:3ebd0e666a9c 205
JKleinRot 15:3ebd0e666a9c 206 arm_biquad_cascade_df1_f32(&notch_triceps, &xtf, &xtf, 1);
JKleinRot 15:3ebd0e666a9c 207
JKleinRot 15:3ebd0e666a9c 208 xtf = fabs(xtf);
JKleinRot 15:3ebd0e666a9c 209
JKleinRot 15:3ebd0e666a9c 210 arm_biquad_cascade_df1_f32(&lowpass_triceps, &xtf, &xtf, 1);
JKleinRot 15:3ebd0e666a9c 211
JKleinRot 15:3ebd0e666a9c 212 pc.printf("xtf is %f\n\r", xtf);
JKleinRot 15:3ebd0e666a9c 213
JKleinRot 15:3ebd0e666a9c 214 }
JKleinRot 0:859c89785d3f 215
JKleinRot 10:52b22bff409a 216 int main()
JKleinRot 10:52b22bff409a 217 {
JKleinRot 11:e9b906b5f572 218 while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
JKleinRot 10:52b22bff409a 219 //PC baud rate instellen
JKleinRot 10:52b22bff409a 220 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 10:52b22bff409a 221
JKleinRot 10:52b22bff409a 222 switch(state) { //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case
JKleinRot 10:52b22bff409a 223
JKleinRot 11:e9b906b5f572 224 case RUST: { //Aanzetten
JKleinRot 10:52b22bff409a 225 lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
JKleinRot 10:52b22bff409a 226 lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
JKleinRot 10:52b22bff409a 227 wait(2); //Twee seconden wachten
JKleinRot 14:e1816efa712d 228 pc.printf("RUST afgerond\n\r"); //Tekst voor controle Aanzetten
JKleinRot 10:52b22bff409a 229 state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
JKleinRot 10:52b22bff409a 230 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 231 }
JKleinRot 10:52b22bff409a 232
JKleinRot 10:52b22bff409a 233 case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie
JKleinRot 14:e1816efa712d 234 pc.printf("KALIBRATIE_ARM1\n\r");
JKleinRot 10:52b22bff409a 235 wait(1); //Een seconde wachten
JKleinRot 15:3ebd0e666a9c 236
JKleinRot 10:52b22bff409a 237 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 238
JKleinRot 10:52b22bff409a 239 while(state == KALIBRATIE_ARM1) {
JKleinRot 10:52b22bff409a 240 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 241 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 242
JKleinRot 10:52b22bff409a 243 arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 244 }
JKleinRot 10:52b22bff409a 245 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 246 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 247 }
JKleinRot 10:52b22bff409a 248
JKleinRot 11:e9b906b5f572 249 case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
JKleinRot 14:e1816efa712d 250 pc.printf("KALIBRATIE_ARM1\n\r");
JKleinRot 10:52b22bff409a 251 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 252
JKleinRot 10:52b22bff409a 253 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 254
JKleinRot 10:52b22bff409a 255 while(state == KALIBRATIE_ARM2) {
JKleinRot 10:52b22bff409a 256 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 257 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 258
JKleinRot 10:52b22bff409a 259 arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 260 }
JKleinRot 11:e9b906b5f572 261 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 262 ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
JKleinRot 10:52b22bff409a 263 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 264 }
JKleinRot 15:3ebd0e666a9c 265
JKleinRot 15:3ebd0e666a9c 266 case EMG_KALIBRATIE_BICEPS: {
JKleinRot 14:e1816efa712d 267 pc.printf("EMG__KALIBRATIE_BICEPS\n\r");
JKleinRot 15:3ebd0e666a9c 268
JKleinRot 14:e1816efa712d 269 lcd_r1 = " SPAN IN 5 SEC. ";
JKleinRot 14:e1816efa712d 270 lcd_r2 = " 2 X BICEPS AAN ";
JKleinRot 15:3ebd0e666a9c 271 pc.printf("span biceps aan\n\r");
JKleinRot 15:3ebd0e666a9c 272 pc.printf("EMG biceps %f\n\r",xb);
JKleinRot 15:3ebd0e666a9c 273
JKleinRot 15:3ebd0e666a9c 274 arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1, highpass_biceps_const, highpass_biceps_states);
JKleinRot 15:3ebd0e666a9c 275 arm_biquad_cascade_df1_init_f32(&notch_biceps, 1, notch_biceps_const, notch_biceps_states);
JKleinRot 15:3ebd0e666a9c 276 arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1, lowpass_biceps_const, lowpass_biceps_states);
JKleinRot 15:3ebd0e666a9c 277
JKleinRot 14:e1816efa712d 278 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);
JKleinRot 14:e1816efa712d 279 biceps_kalibratie.start();
JKleinRot 15:3ebd0e666a9c 280
JKleinRot 15:3ebd0e666a9c 281 while(biceps_kalibratie.read() <= 5) {
JKleinRot 13:54ee98850a15 282
JKleinRot 15:3ebd0e666a9c 283 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 284 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 285
JKleinRot 15:3ebd0e666a9c 286 xb = EMG_bi.read(); //EMG meten van biceps
JKleinRot 15:3ebd0e666a9c 287
JKleinRot 15:3ebd0e666a9c 288 filter_biceps();
JKleinRot 15:3ebd0e666a9c 289
JKleinRot 15:3ebd0e666a9c 290 if(int(biceps_kalibratie.read()) == 1) {
JKleinRot 14:e1816efa712d 291 lcd_r1 = " 4 ";
JKleinRot 15:3ebd0e666a9c 292 pc.printf("4");
JKleinRot 14:e1816efa712d 293 }
JKleinRot 15:3ebd0e666a9c 294 if(biceps_kalibratie.read() == 2) {
JKleinRot 14:e1816efa712d 295 lcd_r1 = " 3 ";
JKleinRot 15:3ebd0e666a9c 296 pc.printf("3");
JKleinRot 14:e1816efa712d 297 }
JKleinRot 15:3ebd0e666a9c 298 if(biceps_kalibratie.read() == 3) {
JKleinRot 14:e1816efa712d 299 lcd_r1 = " 2 ";
JKleinRot 15:3ebd0e666a9c 300 pc.printf("2");
JKleinRot 14:e1816efa712d 301 }
JKleinRot 15:3ebd0e666a9c 302 if(biceps_kalibratie.read() == 4) {
JKleinRot 15:3ebd0e666a9c 303 lcd_r1 = " 1 ";
JKleinRot 15:3ebd0e666a9c 304 pc.printf("1");
JKleinRot 15:3ebd0e666a9c 305 }
JKleinRot 15:3ebd0e666a9c 306
JKleinRot 15:3ebd0e666a9c 307 }
JKleinRot 15:3ebd0e666a9c 308 if(xbf >= xbfmax) {
JKleinRot 15:3ebd0e666a9c 309 xbfmax = xbf;
JKleinRot 15:3ebd0e666a9c 310 }
JKleinRot 15:3ebd0e666a9c 311 xbt = xbfmax * 0.8;
JKleinRot 15:3ebd0e666a9c 312 pc.printf("threshold is %f\n\r", xbt);
JKleinRot 15:3ebd0e666a9c 313 state = EMG_KALIBRATIE_TRICEPS;
JKleinRot 15:3ebd0e666a9c 314 break;
JKleinRot 15:3ebd0e666a9c 315 }
JKleinRot 15:3ebd0e666a9c 316
JKleinRot 15:3ebd0e666a9c 317 case EMG_KALIBRATIE_TRICEPS: {
JKleinRot 15:3ebd0e666a9c 318 pc.printf("EMG__KALIBRATIE_TRICEPS\n\r");
JKleinRot 15:3ebd0e666a9c 319
JKleinRot 15:3ebd0e666a9c 320 lcd_r1 = " SPAN IN 5 SEC. ";
JKleinRot 15:3ebd0e666a9c 321 lcd_r2 = " 2 X TRICEPS AAN";
JKleinRot 15:3ebd0e666a9c 322 pc.printf("span triceps aan\n\r");
JKleinRot 15:3ebd0e666a9c 323 pc.printf("EMG biceps %f\n\r",xt);
JKleinRot 15:3ebd0e666a9c 324
JKleinRot 15:3ebd0e666a9c 325 arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1, highpass_triceps_const, highpass_triceps_states);
JKleinRot 15:3ebd0e666a9c 326 arm_biquad_cascade_df1_init_f32(&notch_triceps, 1, notch_triceps_const, notch_triceps_states);
JKleinRot 15:3ebd0e666a9c 327 arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 1, lowpass_triceps_const, lowpass_triceps_states);
JKleinRot 15:3ebd0e666a9c 328
JKleinRot 15:3ebd0e666a9c 329 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);
JKleinRot 15:3ebd0e666a9c 330 triceps_kalibratie.start();
JKleinRot 15:3ebd0e666a9c 331
JKleinRot 15:3ebd0e666a9c 332 while(triceps_kalibratie.read() <= 5) {
JKleinRot 15:3ebd0e666a9c 333
JKleinRot 15:3ebd0e666a9c 334 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 335 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 336
JKleinRot 15:3ebd0e666a9c 337 xt = EMG_tri.read(); //EMG meten van biceps
JKleinRot 15:3ebd0e666a9c 338
JKleinRot 15:3ebd0e666a9c 339 filter_triceps();
JKleinRot 15:3ebd0e666a9c 340
JKleinRot 15:3ebd0e666a9c 341 if(triceps_kalibratie.read() == 1) {
JKleinRot 15:3ebd0e666a9c 342 lcd_r1 = " 4 ";
JKleinRot 15:3ebd0e666a9c 343 pc.printf("4");
JKleinRot 15:3ebd0e666a9c 344 }
JKleinRot 15:3ebd0e666a9c 345 if(triceps_kalibratie.read() == 2) {
JKleinRot 15:3ebd0e666a9c 346 lcd_r1 = " 3 ";
JKleinRot 15:3ebd0e666a9c 347 pc.printf("3");
JKleinRot 15:3ebd0e666a9c 348 }
JKleinRot 15:3ebd0e666a9c 349 if(triceps_kalibratie.read() == 3) {
JKleinRot 15:3ebd0e666a9c 350 lcd_r1 = " 2 ";
JKleinRot 15:3ebd0e666a9c 351 pc.printf("2");
JKleinRot 15:3ebd0e666a9c 352 }
JKleinRot 15:3ebd0e666a9c 353 if(triceps_kalibratie.read() == 4) {
JKleinRot 14:e1816efa712d 354 lcd_r1 = " 1 ";
JKleinRot 14:e1816efa712d 355 }
JKleinRot 15:3ebd0e666a9c 356
JKleinRot 15:3ebd0e666a9c 357 }
JKleinRot 15:3ebd0e666a9c 358 if(xtf >= xtfmax) {
JKleinRot 15:3ebd0e666a9c 359 xtfmax = xtf;
JKleinRot 10:52b22bff409a 360 }
JKleinRot 15:3ebd0e666a9c 361 xtt = xtfmax * 0.8;
JKleinRot 15:3ebd0e666a9c 362 pc.printf("threshold is %f\n\r", xtt);
JKleinRot 15:3ebd0e666a9c 363 state = BEGIN_METEN;
JKleinRot 15:3ebd0e666a9c 364 break;
JKleinRot 10:52b22bff409a 365 }
JKleinRot 15:3ebd0e666a9c 366
JKleinRot 15:3ebd0e666a9c 367 case BEGIN_METEN: {
JKleinRot 15:3ebd0e666a9c 368 lcd_r1 = " BEGIN ";
JKleinRot 15:3ebd0e666a9c 369 pc.printf("KEUZE1\n\r");
JKleinRot 15:3ebd0e666a9c 370
JKleinRot 15:3ebd0e666a9c 371 while(state == BEGIN_METEN) {
JKleinRot 15:3ebd0e666a9c 372 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 15:3ebd0e666a9c 373 regelaar_EMG_flag = false;
JKleinRot 15:3ebd0e666a9c 374
JKleinRot 15:3ebd0e666a9c 375 xb = EMG_bi.read();
JKleinRot 15:3ebd0e666a9c 376 filter_biceps();
JKleinRot 15:3ebd0e666a9c 377 xt = EMG_tri.read();
JKleinRot 15:3ebd0e666a9c 378 filter_triceps();
JKleinRot 15:3ebd0e666a9c 379
JKleinRot 15:3ebd0e666a9c 380 if(xb >= xbt) {
JKleinRot 15:3ebd0e666a9c 381 state = B;
JKleinRot 15:3ebd0e666a9c 382 }
JKleinRot 15:3ebd0e666a9c 383 if(xt >= xtt) {
JKleinRot 15:3ebd0e666a9c 384 state = T;
JKleinRot 15:3ebd0e666a9c 385 }
JKleinRot 15:3ebd0e666a9c 386 }
JKleinRot 15:3ebd0e666a9c 387
JKleinRot 15:3ebd0e666a9c 388
JKleinRot 15:3ebd0e666a9c 389
JKleinRot 15:3ebd0e666a9c 390
JKleinRot 15:3ebd0e666a9c 391 default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
JKleinRot 15:3ebd0e666a9c 392 state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
JKleinRot 15:3ebd0e666a9c 393 }
JKleinRot 10:52b22bff409a 394 }
JKleinRot 15:3ebd0e666a9c 395 pc.printf("state: %u\n\r",state);
JKleinRot 9:454e7da8ab65 396 }
JKleinRot 3:adc3052039e7 397 }
JKleinRot 0:859c89785d3f 398 }