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@16:c34c5d9dfe1a, 2014-10-28 (annotated)

Committer:
JKleinRot
Date:
Tue Oct 28 18:19:08 2014 +0000
Revision:
16:c34c5d9dfe1a
Parent:
15:3ebd0e666a9c
Child:
17:c694a0e63a89
2014-10-28 LCD scherm pinnen aangepast en de printf voor lcd gemaakt. Comments tot halverwege af

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 16:c34c5d9dfe1a 9 #define KP_arm1 0.01 //Factor proprotionele regelaar arm 1
JKleinRot 16:c34c5d9dfe1a 10 #define KI_arm1 0 //Factor integraal regelaar arm 1
JKleinRot 16:c34c5d9dfe1a 11 #define KD_arm1 0 //Factor afgeleide regelaar arm 1
JKleinRot 16:c34c5d9dfe1a 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 16:c34c5d9dfe1a 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 16:c34c5d9dfe1a 19 //Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk
JKleinRot 16:c34c5d9dfe1a 20 //High Pass filter
JKleinRot 15:3ebd0e666a9c 21 #define A1 1
JKleinRot 15:3ebd0e666a9c 22 #define A2 -1.5610
JKleinRot 15:3ebd0e666a9c 23 #define A3 0.6414
JKleinRot 15:3ebd0e666a9c 24 #define B1 0.0201
JKleinRot 15:3ebd0e666a9c 25 #define B2 0.0402
JKleinRot 15:3ebd0e666a9c 26 #define B3 0.0201
JKleinRot 13:54ee98850a15 27
JKleinRot 16:c34c5d9dfe1a 28 //Notch filter
JKleinRot 13:54ee98850a15 29 #define C1 1
JKleinRot 13:54ee98850a15 30 #define C2 -1.1873E-16
JKleinRot 13:54ee98850a15 31 #define C3 0.9391
JKleinRot 13:54ee98850a15 32 #define D1 0.9695
JKleinRot 13:54ee98850a15 33 #define D2 -1.1873E-16
JKleinRot 13:54ee98850a15 34 #define D3 0.9695
JKleinRot 13:54ee98850a15 35
JKleinRot 16:c34c5d9dfe1a 36 //Low pass filter
JKleinRot 13:54ee98850a15 37 #define E1 1
JKleinRot 15:3ebd0e666a9c 38 #define E2 -1.9645
JKleinRot 15:3ebd0e666a9c 39 #define E3 0.9651
JKleinRot 15:3ebd0e666a9c 40 #define F1 1.5515E-4
JKleinRot 15:3ebd0e666a9c 41 #define F2 3.1030E-4
JKleinRot 15:3ebd0e666a9c 42 #define F3 1.5515E-4
JKleinRot 15:3ebd0e666a9c 43
JKleinRot 0:859c89785d3f 44 //Pinverdeling en naamgeving variabelen
JKleinRot 16:c34c5d9dfe1a 45 TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); //LCD scherm
JKleinRot 16:c34c5d9dfe1a 46 MODSERIAL pc(USBTX, USBRX); //PC
JKleinRot 0:859c89785d3f 47
JKleinRot 16:c34c5d9dfe1a 48 PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
JKleinRot 16:c34c5d9dfe1a 49 DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1
JKleinRot 16:c34c5d9dfe1a 50 Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit)
JKleinRot 16:c34c5d9dfe1a 51 PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2
JKleinRot 16:c34c5d9dfe1a 52 DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2
JKleinRot 16:c34c5d9dfe1a 53 Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit)
JKleinRot 16:c34c5d9dfe1a 54 AnalogIn EMG_bi(PTB1); //Uitlezen EMG signaal biceps
JKleinRot 16:c34c5d9dfe1a 55 AnalogIn EMG_tri(PTB2); //Uitlezen EMG signaal triceps
JKleinRot 16:c34c5d9dfe1a 56 //Blauw op 3,3 V en groen op GND, voeding beide encoders
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 16:c34c5d9dfe1a 60 Timer biceps_kalibratie; //Timer voor kalibratiemeting EMG biceps
JKleinRot 16:c34c5d9dfe1a 61 Timer triceps_kalibratie; //Timer voor kalibratiemeting EMG triceps
JKleinRot 1:e5e1eb9d0025 62
JKleinRot 9:454e7da8ab65 63 //States definiëren
JKleinRot 16:c34c5d9dfe1a 64 enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, BEGIN_METEN, B, T, BB, BT, TB, TT}; //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 11:e9b906b5f572 68 float pwm_to_motor_arm1; //PWM output naar motor arm 1
JKleinRot 11:e9b906b5f572 69 float pwm_to_motor_arm2; //PWM output naar motor arm 2
JKleinRot 13:54ee98850a15 70 float error_arm1_kalibratie; //Error in pulsen arm 1
JKleinRot 13:54ee98850a15 71 float vorige_error_arm1 = 0; //Derivative actie van regelaar arm 1
JKleinRot 13:54ee98850a15 72 float integraal_arm1 = 0; //Integraal actie van regelaar arm 1
JKleinRot 13:54ee98850a15 73 float afgeleide_arm1; //Afgeleide actie van regelaar arm 1
JKleinRot 13:54ee98850a15 74 float error_arm2_kalibratie; //Error in pulsen arm 2
JKleinRot 13:54ee98850a15 75 float vorige_error_arm2 = 0; //Derivative actie van regelaar arm 2
JKleinRot 13:54ee98850a15 76 float integraal_arm2 = 0; //Integraal actie van regelaar arm 2
JKleinRot 13:54ee98850a15 77 float afgeleide_arm2; //Afgeleide actie van regelaar arm 2
JKleinRot 13:54ee98850a15 78 float xb; //Gemeten EMG waarde biceps
JKleinRot 16:c34c5d9dfe1a 79 float xt; //Gemeten EMG waarde triceps
JKleinRot 15:3ebd0e666a9c 80
JKleinRot 16:c34c5d9dfe1a 81 arm_biquad_casd_df1_inst_f32 highpass_biceps; //Highpass_biceps IIR filter in direct form 1
JKleinRot 16:c34c5d9dfe1a 82 float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter
JKleinRot 16:c34c5d9dfe1a 83 float highpass_biceps_states[4]; //Aantal states van het filter, het aantal y(n-x) en x(n-x)
JKleinRot 13:54ee98850a15 84
JKleinRot 16:c34c5d9dfe1a 85 arm_biquad_casd_df1_inst_f32 notch_biceps; //Notch_biceps IIR filter in direct form 1
JKleinRot 16:c34c5d9dfe1a 86 float notch_biceps_const[] = {D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter
JKleinRot 16:c34c5d9dfe1a 87 float notch_biceps_states[4]; //Aantal states van het filter
JKleinRot 15:3ebd0e666a9c 88
JKleinRot 16:c34c5d9dfe1a 89 arm_biquad_casd_df1_inst_f32 lowpass_biceps; //Lowpass_biceps IIR filter in direct form 1
JKleinRot 16:c34c5d9dfe1a 90 float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter
JKleinRot 16:c34c5d9dfe1a 91 float lowpass_biceps_states[4]; //Aantal states van het filter
JKleinRot 15:3ebd0e666a9c 92
JKleinRot 16:c34c5d9dfe1a 93 float xbf; //Gefilterde EMG waarde biceps
JKleinRot 16:c34c5d9dfe1a 94 float xbfmax = 0; //Maximale gefilterde EMG waarde biceps
JKleinRot 13:54ee98850a15 95
JKleinRot 16:c34c5d9dfe1a 96 arm_biquad_casd_df1_inst_f32 highpass_triceps; //Highpass_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
JKleinRot 16:c34c5d9dfe1a 97 float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter
JKleinRot 16:c34c5d9dfe1a 98 float highpass_triceps_states[4]; //Aantal states van het filter
JKleinRot 13:54ee98850a15 99
JKleinRot 16:c34c5d9dfe1a 100 arm_biquad_casd_df1_inst_f32 notch_triceps; //Notch_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
JKleinRot 16:c34c5d9dfe1a 101 float notch_triceps_const[] = {D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter
JKleinRot 16:c34c5d9dfe1a 102 float notch_triceps_states[4]; //Aantal states van het filter
JKleinRot 13:54ee98850a15 103
JKleinRot 16:c34c5d9dfe1a 104 arm_biquad_casd_df1_inst_f32 lowpass_triceps; //Lowpass_biceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
JKleinRot 16:c34c5d9dfe1a 105 float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter
JKleinRot 16:c34c5d9dfe1a 106 float lowpass_triceps_states[4]; //Aantal states van het filter
JKleinRot 13:54ee98850a15 107
JKleinRot 16:c34c5d9dfe1a 108 float xtf; //Gefilterde EMG waarde triceps
JKleinRot 16:c34c5d9dfe1a 109 float xtfmax = 0; //Maximale gefilterde EMG waarde triceps
JKleinRot 15:3ebd0e666a9c 110
JKleinRot 16:c34c5d9dfe1a 111 float xbt; //Thresholdwaarde EMG biceps
JKleinRot 16:c34c5d9dfe1a 112 float xtt; //Thresholdwaarde EMG triceps
JKleinRot 7:dd3cba61b34b 113
JKleinRot 6:3b6fad529520 114 volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 115 void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
JKleinRot 10:52b22bff409a 116 {
JKleinRot 10:52b22bff409a 117 regelaar_ticker_flag = true;
JKleinRot 1:e5e1eb9d0025 118 }
JKleinRot 1:e5e1eb9d0025 119
JKleinRot 6:3b6fad529520 120 volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma
JKleinRot 10:52b22bff409a 121 void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true
JKleinRot 10:52b22bff409a 122 {
JKleinRot 10:52b22bff409a 123 regelaar_EMG_flag = true;
JKleinRot 4:69540b9c0646 124 }
JKleinRot 4:69540b9c0646 125
JKleinRot 10:52b22bff409a 126 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 127 {
JKleinRot 16:c34c5d9dfe1a 128 if (*in < min) //Als de waarde kleiner is als het minimum wordt de waarde het minimum
JKleinRot 16:c34c5d9dfe1a 129 {
JKleinRot 2:0cb899f2800a 130 *in = min;
JKleinRot 2:0cb899f2800a 131 }
JKleinRot 16:c34c5d9dfe1a 132 if (*in > max) //Als de waarde groter is dan het maximum is de waarde het maximum
JKleinRot 16:c34c5d9dfe1a 133 {
JKleinRot 2:0cb899f2800a 134 *in = max;
JKleinRot 16:c34c5d9dfe1a 135 }
JKleinRot 16:c34c5d9dfe1a 136 else //In alle andere gevallen is de waarde de waarde zelf
JKleinRot 16:c34c5d9dfe1a 137 {
JKleinRot 2:0cb899f2800a 138 *in = *in;
JKleinRot 2:0cb899f2800a 139 }
JKleinRot 2:0cb899f2800a 140 }
JKleinRot 1:e5e1eb9d0025 141
JKleinRot 16:c34c5d9dfe1a 142 void arm1_naar_thuispositie() //Brengt arm 1 naar de beginpositie
JKleinRot 10:52b22bff409a 143 {
JKleinRot 16:c34c5d9dfe1a 144 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 16:c34c5d9dfe1a 145 integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
JKleinRot 16:c34c5d9dfe1a 146 afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
JKleinRot 16:c34c5d9dfe1a 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 16:c34c5d9dfe1a 148 keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 10:52b22bff409a 149
JKleinRot 16:c34c5d9dfe1a 150 if (pwm_to_motor_arm1 > 0) //Als PWM is positief, dan richting 1
JKleinRot 16:c34c5d9dfe1a 151 {
JKleinRot 10:52b22bff409a 152 dir_motor_arm1.write(1);
JKleinRot 16:c34c5d9dfe1a 153 }
JKleinRot 16:c34c5d9dfe1a 154 else //Anders richting nul
JKleinRot 16:c34c5d9dfe1a 155 {
JKleinRot 7:dd3cba61b34b 156 dir_motor_arm1.write(0);
JKleinRot 7:dd3cba61b34b 157 }
JKleinRot 11:e9b906b5f572 158 pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 16:c34c5d9dfe1a 159 pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
JKleinRot 16:c34c5d9dfe1a 160 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen
JKleinRot 10:52b22bff409a 161
JKleinRot 16:c34c5d9dfe1a 162 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 16:c34c5d9dfe1a 163 {
JKleinRot 11:e9b906b5f572 164 state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 16:c34c5d9dfe1a 165 pc.printf("KALIBRATIE_ARM1 afgerond\n"); //Tekst voor controle Arm 1 naar thuispositie
JKleinRot 10:52b22bff409a 166 }
JKleinRot 10:52b22bff409a 167 }
JKleinRot 7:dd3cba61b34b 168
JKleinRot 16:c34c5d9dfe1a 169 void arm2_naar_thuispositie() //Brengt arm 2 naar beginpositie
JKleinRot 10:52b22bff409a 170 {
JKleinRot 16:c34c5d9dfe1a 171 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 16:c34c5d9dfe1a 172 integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
JKleinRot 16:c34c5d9dfe1a 173 afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
JKleinRot 16:c34c5d9dfe1a 174 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 175 keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
JKleinRot 10:52b22bff409a 176
JKleinRot 16:c34c5d9dfe1a 177 if (pwm_to_motor_arm2 > 0) //Als PWM is positief, dan richting 1
JKleinRot 16:c34c5d9dfe1a 178 {
JKleinRot 7:dd3cba61b34b 179 dir_motor_arm2.write(1);
JKleinRot 16:c34c5d9dfe1a 180 }
JKleinRot 16:c34c5d9dfe1a 181 else //Anders richting nul
JKleinRot 16:c34c5d9dfe1a 182 {
JKleinRot 7:dd3cba61b34b 183 dir_motor_arm2.write(0);
JKleinRot 7:dd3cba61b34b 184 }
JKleinRot 11:e9b906b5f572 185 pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
JKleinRot 16:c34c5d9dfe1a 186 pc.printf("pulsmotorgetposition %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
JKleinRot 16:c34c5d9dfe1a 187 pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2); //Huidige PWM waarde naar pc sturen
JKleinRot 10:52b22bff409a 188
JKleinRot 16:c34c5d9dfe1a 189 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 190 {
JKleinRot 16:c34c5d9dfe1a 191 state = EMG_KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
JKleinRot 16:c34c5d9dfe1a 192 pc.printf("KALIBRATIE_ARM2 afgerond\n\r"); //Tekst voor controle Arm 2 naar thuispositie
JKleinRot 7:dd3cba61b34b 193 }
JKleinRot 7:dd3cba61b34b 194 }
JKleinRot 0:859c89785d3f 195
JKleinRot 16:c34c5d9dfe1a 196 void filter_biceps() //Filtert het EMG signaal van de biceps
JKleinRot 13:54ee98850a15 197 {
JKleinRot 16:c34c5d9dfe1a 198 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 15:3ebd0e666a9c 199
JKleinRot 16:c34c5d9dfe1a 200 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 201
JKleinRot 16:c34c5d9dfe1a 202 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 203
JKleinRot 16:c34c5d9dfe1a 204 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 16:c34c5d9dfe1a 205
JKleinRot 16:c34c5d9dfe1a 206 pc.printf("xbf is %f\n\r", xbf); //De gefilterde EMG waarde van de biceps naar pc sturen
JKleinRot 16:c34c5d9dfe1a 207 }
JKleinRot 15:3ebd0e666a9c 208
JKleinRot 16:c34c5d9dfe1a 209 void filter_triceps() //Filtert het EMG signaal van de triceps
JKleinRot 16:c34c5d9dfe1a 210 {
JKleinRot 16:c34c5d9dfe1a 211 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 212
JKleinRot 16:c34c5d9dfe1a 213 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 214
JKleinRot 16:c34c5d9dfe1a 215 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 216
JKleinRot 16:c34c5d9dfe1a 217 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 218
JKleinRot 16:c34c5d9dfe1a 219 pc.printf("xtf is %f\n\r", xtf); //De gefilterde EMG waarde van de triceps naar pc sturen
JKleinRot 15:3ebd0e666a9c 220
JKleinRot 13:54ee98850a15 221 }
JKleinRot 13:54ee98850a15 222
JKleinRot 16:c34c5d9dfe1a 223 int main() //Main script
JKleinRot 15:3ebd0e666a9c 224 {
JKleinRot 16:c34c5d9dfe1a 225 while(1) //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
JKleinRot 16:c34c5d9dfe1a 226 {
JKleinRot 10:52b22bff409a 227 pc.baud(38400); //PC baud rate is 38400 bits/seconde
JKleinRot 10:52b22bff409a 228
JKleinRot 16:c34c5d9dfe1a 229 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 16:c34c5d9dfe1a 230 {
JKleinRot 10:52b22bff409a 231
JKleinRot 16:c34c5d9dfe1a 232 case RUST: //Aanzetten
JKleinRot 16:c34c5d9dfe1a 233 {
JKleinRot 16:c34c5d9dfe1a 234 pc.printf("RUST\n\r"); //Begin RUST naar pc sturen
JKleinRot 16:c34c5d9dfe1a 235 lcd.locate(0,0); //Zet de tekst op de eerste regel
JKleinRot 16:c34c5d9dfe1a 236 lcd.printf(" BMT K9 GR. 4 "); //Tekst op LCD scherm
JKleinRot 16:c34c5d9dfe1a 237 lcd.locate(0,1); //Zet de tekst op de tweede regel
JKleinRot 16:c34c5d9dfe1a 238 lcd.printf("HOI! IK BEN PIPO"); //Tekst op LCD scherm
JKleinRot 16:c34c5d9dfe1a 239 wait(2); //Twee seconden wachten
JKleinRot 16:c34c5d9dfe1a 240 lcd.cls(); //Maak LCD scherm leeg
JKleinRot 14:e1816efa712d 241 pc.printf("RUST afgerond\n\r"); //Tekst voor controle Aanzetten
JKleinRot 16:c34c5d9dfe1a 242 state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
JKleinRot 16:c34c5d9dfe1a 243 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 244 }
JKleinRot 10:52b22bff409a 245
JKleinRot 16:c34c5d9dfe1a 246 case KALIBRATIE_ARM1: //Arm 1 naar thuispositie
JKleinRot 16:c34c5d9dfe1a 247 {
JKleinRot 16:c34c5d9dfe1a 248 pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM1 naar pc sturen
JKleinRot 16:c34c5d9dfe1a 249 wait(1); //Een seconde wachten
JKleinRot 15:3ebd0e666a9c 250
JKleinRot 10:52b22bff409a 251 ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 252
JKleinRot 16:c34c5d9dfe1a 253 while(state == KALIBRATIE_ARM1)
JKleinRot 16:c34c5d9dfe1a 254 {
JKleinRot 16:c34c5d9dfe1a 255 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 256 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 257
JKleinRot 10:52b22bff409a 258 arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 259 }
JKleinRot 10:52b22bff409a 260 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 261 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 262 }
JKleinRot 10:52b22bff409a 263
JKleinRot 16:c34c5d9dfe1a 264 case KALIBRATIE_ARM2: //Arm 2 naar thuispositie
JKleinRot 16:c34c5d9dfe1a 265 {
JKleinRot 16:c34c5d9dfe1a 266 pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM2 naar pc sturen
JKleinRot 16:c34c5d9dfe1a 267 wait(1); //Een seconde wachten
JKleinRot 10:52b22bff409a 268
JKleinRot 10:52b22bff409a 269 //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 10:52b22bff409a 270
JKleinRot 16:c34c5d9dfe1a 271 while(state == KALIBRATIE_ARM2)
JKleinRot 16:c34c5d9dfe1a 272 {
JKleinRot 16:c34c5d9dfe1a 273 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 274 regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 10:52b22bff409a 275
JKleinRot 10:52b22bff409a 276 arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
JKleinRot 10:52b22bff409a 277 }
JKleinRot 11:e9b906b5f572 278 wait(1); //Een seconde wachten
JKleinRot 11:e9b906b5f572 279 ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
JKleinRot 16:c34c5d9dfe1a 280 break; //Stopt acties in deze case
JKleinRot 10:52b22bff409a 281 }
JKleinRot 15:3ebd0e666a9c 282
JKleinRot 16:c34c5d9dfe1a 283 case EMG_KALIBRATIE_BICEPS: //Kalibratie biceps voor bepalen threshold
JKleinRot 16:c34c5d9dfe1a 284 {
JKleinRot 16:c34c5d9dfe1a 285 pc.printf("EMG__KALIBRATIE_BICEPS\n\r"); //Begin EMG_KALIBRATIE_BICEPS naar pc sturen
JKleinRot 16:c34c5d9dfe1a 286
JKleinRot 16:c34c5d9dfe1a 287 lcd.locate(0,0); //Zet tekst op eerste regel
JKleinRot 16:c34c5d9dfe1a 288 lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm
JKleinRot 16:c34c5d9dfe1a 289 lcd.locate(0,1); //Zet tekst op tweede regel
JKleinRot 16:c34c5d9dfe1a 290 lcd.printf(" 2 X BICEPS AAN "); //Tekst op LCD scherm
JKleinRot 15:3ebd0e666a9c 291
JKleinRot 16:c34c5d9dfe1a 292 arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1, highpass_biceps_const, highpass_biceps_states); //Highpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren
JKleinRot 16:c34c5d9dfe1a 293 arm_biquad_cascade_df1_init_f32(&notch_biceps, 1, notch_biceps_const, notch_biceps_states); //Notchfilter biceps met bijbehorende filtercoëfficiënten en states definiëren
JKleinRot 16:c34c5d9dfe1a 294 arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1, lowpass_biceps_const, lowpass_biceps_states); //Lowpassfilter biceps met bijbehorende filtercoëfficiënten en states definiëren
JKleinRot 15:3ebd0e666a9c 295
JKleinRot 16:c34c5d9dfe1a 296 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconde de flag op laten steken
JKleinRot 16:c34c5d9dfe1a 297 biceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt
JKleinRot 15:3ebd0e666a9c 298
JKleinRot 16:c34c5d9dfe1a 299 while(biceps_kalibratie.read() <= 5) //Zolang de timer nog geen 5 seconden heeft gemeten
JKleinRot 16:c34c5d9dfe1a 300 {
JKleinRot 13:54ee98850a15 301
JKleinRot 15:3ebd0e666a9c 302 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 303 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 304
JKleinRot 15:3ebd0e666a9c 305 xb = EMG_bi.read(); //EMG meten van biceps
JKleinRot 15:3ebd0e666a9c 306
JKleinRot 15:3ebd0e666a9c 307 filter_biceps();
JKleinRot 15:3ebd0e666a9c 308
JKleinRot 15:3ebd0e666a9c 309 if(int(biceps_kalibratie.read()) == 1) {
JKleinRot 16:c34c5d9dfe1a 310 lcd.locate(0,0);
JKleinRot 16:c34c5d9dfe1a 311 lcd.printf(" 4 ");
JKleinRot 15:3ebd0e666a9c 312 pc.printf("4");
JKleinRot 14:e1816efa712d 313 }
JKleinRot 16:c34c5d9dfe1a 314 if(int(biceps_kalibratie.read()) == 2) {
JKleinRot 16:c34c5d9dfe1a 315 lcd.locate(0,0);
JKleinRot 16:c34c5d9dfe1a 316 lcd.printf(" 3 ");
JKleinRot 15:3ebd0e666a9c 317 pc.printf("3");
JKleinRot 14:e1816efa712d 318 }
JKleinRot 16:c34c5d9dfe1a 319 if(int(biceps_kalibratie.read()) == 3) {
JKleinRot 16:c34c5d9dfe1a 320 lcd.locate(0,0);
JKleinRot 16:c34c5d9dfe1a 321 lcd.printf(" 2 ");
JKleinRot 15:3ebd0e666a9c 322 pc.printf("2");
JKleinRot 14:e1816efa712d 323 }
JKleinRot 16:c34c5d9dfe1a 324 if(int(biceps_kalibratie.read()) == 4) {
JKleinRot 16:c34c5d9dfe1a 325 lcd.locate(0,0);
JKleinRot 16:c34c5d9dfe1a 326 lcd.printf(" 1 ");
JKleinRot 15:3ebd0e666a9c 327 pc.printf("1");
JKleinRot 15:3ebd0e666a9c 328 }
JKleinRot 15:3ebd0e666a9c 329
JKleinRot 15:3ebd0e666a9c 330 }
JKleinRot 15:3ebd0e666a9c 331 if(xbf >= xbfmax) {
JKleinRot 15:3ebd0e666a9c 332 xbfmax = xbf;
JKleinRot 15:3ebd0e666a9c 333 }
JKleinRot 15:3ebd0e666a9c 334 xbt = xbfmax * 0.8;
JKleinRot 15:3ebd0e666a9c 335 pc.printf("threshold is %f\n\r", xbt);
JKleinRot 15:3ebd0e666a9c 336 state = EMG_KALIBRATIE_TRICEPS;
JKleinRot 15:3ebd0e666a9c 337 break;
JKleinRot 15:3ebd0e666a9c 338 }
JKleinRot 15:3ebd0e666a9c 339
JKleinRot 15:3ebd0e666a9c 340 case EMG_KALIBRATIE_TRICEPS: {
JKleinRot 15:3ebd0e666a9c 341 pc.printf("EMG__KALIBRATIE_TRICEPS\n\r");
JKleinRot 15:3ebd0e666a9c 342
JKleinRot 16:c34c5d9dfe1a 343 lcd.locate(0,0);
JKleinRot 16:c34c5d9dfe1a 344 lcd.printf(" SPAN IN 5 SEC. ");
JKleinRot 16:c34c5d9dfe1a 345 lcd.locate(0,1);
JKleinRot 16:c34c5d9dfe1a 346 lcd.printf(" 2 X TRICEPS AAN");
JKleinRot 15:3ebd0e666a9c 347
JKleinRot 15:3ebd0e666a9c 348 arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1, highpass_triceps_const, highpass_triceps_states);
JKleinRot 15:3ebd0e666a9c 349 arm_biquad_cascade_df1_init_f32(&notch_triceps, 1, notch_triceps_const, notch_triceps_states);
JKleinRot 15:3ebd0e666a9c 350 arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 1, lowpass_triceps_const, lowpass_triceps_states);
JKleinRot 15:3ebd0e666a9c 351
JKleinRot 15:3ebd0e666a9c 352 ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);
JKleinRot 15:3ebd0e666a9c 353 triceps_kalibratie.start();
JKleinRot 15:3ebd0e666a9c 354
JKleinRot 15:3ebd0e666a9c 355 while(triceps_kalibratie.read() <= 5) {
JKleinRot 15:3ebd0e666a9c 356
JKleinRot 15:3ebd0e666a9c 357 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 358 regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
JKleinRot 15:3ebd0e666a9c 359
JKleinRot 15:3ebd0e666a9c 360 xt = EMG_tri.read(); //EMG meten van biceps
JKleinRot 15:3ebd0e666a9c 361
JKleinRot 15:3ebd0e666a9c 362 filter_triceps();
JKleinRot 15:3ebd0e666a9c 363
JKleinRot 15:3ebd0e666a9c 364 if(triceps_kalibratie.read() == 1) {
JKleinRot 16:c34c5d9dfe1a 365 lcd.locate(0,0);
JKleinRot 16:c34c5d9dfe1a 366 lcd.printf(" 4 ");
JKleinRot 15:3ebd0e666a9c 367 pc.printf("4");
JKleinRot 15:3ebd0e666a9c 368 }
JKleinRot 15:3ebd0e666a9c 369 if(triceps_kalibratie.read() == 2) {
JKleinRot 16:c34c5d9dfe1a 370 lcd.locate(0,0);
JKleinRot 16:c34c5d9dfe1a 371 lcd.printf(" 3 ");
JKleinRot 15:3ebd0e666a9c 372 pc.printf("3");
JKleinRot 15:3ebd0e666a9c 373 }
JKleinRot 15:3ebd0e666a9c 374 if(triceps_kalibratie.read() == 3) {
JKleinRot 16:c34c5d9dfe1a 375 lcd.locate(0,0);
JKleinRot 16:c34c5d9dfe1a 376 lcd.printf(" 2 ");
JKleinRot 15:3ebd0e666a9c 377 pc.printf("2");
JKleinRot 15:3ebd0e666a9c 378 }
JKleinRot 15:3ebd0e666a9c 379 if(triceps_kalibratie.read() == 4) {
JKleinRot 16:c34c5d9dfe1a 380 lcd.locate(0,0);
JKleinRot 16:c34c5d9dfe1a 381 lcd.printf(" 1 ");
JKleinRot 16:c34c5d9dfe1a 382 pc.printf("1");
JKleinRot 14:e1816efa712d 383 }
JKleinRot 15:3ebd0e666a9c 384
JKleinRot 15:3ebd0e666a9c 385 }
JKleinRot 15:3ebd0e666a9c 386 if(xtf >= xtfmax) {
JKleinRot 15:3ebd0e666a9c 387 xtfmax = xtf;
JKleinRot 10:52b22bff409a 388 }
JKleinRot 15:3ebd0e666a9c 389 xtt = xtfmax * 0.8;
JKleinRot 15:3ebd0e666a9c 390 pc.printf("threshold is %f\n\r", xtt);
JKleinRot 15:3ebd0e666a9c 391 state = BEGIN_METEN;
JKleinRot 15:3ebd0e666a9c 392 break;
JKleinRot 10:52b22bff409a 393 }
JKleinRot 15:3ebd0e666a9c 394
JKleinRot 15:3ebd0e666a9c 395 case BEGIN_METEN: {
JKleinRot 16:c34c5d9dfe1a 396 lcd.locate(0,0);
JKleinRot 16:c34c5d9dfe1a 397 lcd.printf(" START ");
JKleinRot 16:c34c5d9dfe1a 398
JKleinRot 16:c34c5d9dfe1a 399 pc.printf("START\n\r");
JKleinRot 16:c34c5d9dfe1a 400
JKleinRot 15:3ebd0e666a9c 401 while(state == BEGIN_METEN) {
JKleinRot 15:3ebd0e666a9c 402 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 403 regelaar_EMG_flag = false;
JKleinRot 15:3ebd0e666a9c 404
JKleinRot 15:3ebd0e666a9c 405 xb = EMG_bi.read();
JKleinRot 15:3ebd0e666a9c 406 filter_biceps();
JKleinRot 15:3ebd0e666a9c 407 xt = EMG_tri.read();
JKleinRot 15:3ebd0e666a9c 408 filter_triceps();
JKleinRot 15:3ebd0e666a9c 409
JKleinRot 15:3ebd0e666a9c 410 if(xb >= xbt) {
JKleinRot 15:3ebd0e666a9c 411 state = B;
JKleinRot 15:3ebd0e666a9c 412 }
JKleinRot 15:3ebd0e666a9c 413 if(xt >= xtt) {
JKleinRot 15:3ebd0e666a9c 414 state = T;
JKleinRot 15:3ebd0e666a9c 415 }
JKleinRot 15:3ebd0e666a9c 416 }
JKleinRot 16:c34c5d9dfe1a 417 }
JKleinRot 16:c34c5d9dfe1a 418
JKleinRot 16:c34c5d9dfe1a 419 case B: {
JKleinRot 16:c34c5d9dfe1a 420 lcd.locate(0,0);
JKleinRot 16:c34c5d9dfe1a 421 lcd.printf(" B ");
JKleinRot 16:c34c5d9dfe1a 422 pc.printf("B\n\r");
JKleinRot 15:3ebd0e666a9c 423
JKleinRot 16:c34c5d9dfe1a 424 while(state == B) {
JKleinRot 16:c34c5d9dfe1a 425 while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
JKleinRot 16:c34c5d9dfe1a 426 regelaar_EMG_flag = false;
JKleinRot 15:3ebd0e666a9c 427
JKleinRot 16:c34c5d9dfe1a 428 xb = EMG_bi.read();
JKleinRot 16:c34c5d9dfe1a 429 filter_biceps();
JKleinRot 16:c34c5d9dfe1a 430 xt = EMG_tri.read();
JKleinRot 16:c34c5d9dfe1a 431 filter_triceps();
JKleinRot 16:c34c5d9dfe1a 432
JKleinRot 16:c34c5d9dfe1a 433 if(xb >= xbt) {
JKleinRot 16:c34c5d9dfe1a 434 state = BB;
JKleinRot 16:c34c5d9dfe1a 435 }
JKleinRot 16:c34c5d9dfe1a 436 if(xt >= xtt) {
JKleinRot 16:c34c5d9dfe1a 437 state = BT;
JKleinRot 16:c34c5d9dfe1a 438 }
JKleinRot 16:c34c5d9dfe1a 439 }
JKleinRot 16:c34c5d9dfe1a 440 }
JKleinRot 16:c34c5d9dfe1a 441
JKleinRot 16:c34c5d9dfe1a 442 case T: {
JKleinRot 16:c34c5d9dfe1a 443 lcd.locate(0,0);
JKleinRot 16:c34c5d9dfe1a 444 lcd.printf(" T ");
JKleinRot 16:c34c5d9dfe1a 445 pc.printf("T\n\r");
JKleinRot 16:c34c5d9dfe1a 446
JKleinRot 16:c34c5d9dfe1a 447 while(state == T) {
JKleinRot 16:c34c5d9dfe1a 448 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 16:c34c5d9dfe1a 449 regelaar_EMG_flag = false;
JKleinRot 16:c34c5d9dfe1a 450
JKleinRot 16:c34c5d9dfe1a 451 xb = EMG_bi.read();
JKleinRot 16:c34c5d9dfe1a 452 filter_biceps();
JKleinRot 16:c34c5d9dfe1a 453 xt = EMG_tri.read();
JKleinRot 16:c34c5d9dfe1a 454 filter_triceps();
JKleinRot 16:c34c5d9dfe1a 455
JKleinRot 16:c34c5d9dfe1a 456 if(xb >= xbt) {
JKleinRot 16:c34c5d9dfe1a 457 state = TB;
JKleinRot 16:c34c5d9dfe1a 458 }
JKleinRot 16:c34c5d9dfe1a 459 if(xt >= xtt) {
JKleinRot 16:c34c5d9dfe1a 460 state = TT;
JKleinRot 16:c34c5d9dfe1a 461 }
JKleinRot 16:c34c5d9dfe1a 462 }
JKleinRot 16:c34c5d9dfe1a 463 }
JKleinRot 16:c34c5d9dfe1a 464
JKleinRot 16:c34c5d9dfe1a 465 case BB:
JKleinRot 15:3ebd0e666a9c 466
JKleinRot 15:3ebd0e666a9c 467
JKleinRot 16:c34c5d9dfe1a 468 default:
JKleinRot 16:c34c5d9dfe1a 469 { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
JKleinRot 16:c34c5d9dfe1a 470 state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
JKleinRot 10:52b22bff409a 471 }
JKleinRot 16:c34c5d9dfe1a 472
JKleinRot 15:3ebd0e666a9c 473 pc.printf("state: %u\n\r",state);
JKleinRot 9:454e7da8ab65 474 }
JKleinRot 3:adc3052039e7 475 }
JKleinRot 0:859c89785d3f 476 }