Janine Klein Rot
2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
Diff: main.cpp
- Revision:
- 16:c34c5d9dfe1a
- Parent:
- 15:3ebd0e666a9c
- Child:
- 17:c694a0e63a89
--- a/main.cpp Tue Oct 28 11:11:13 2014 +0000
+++ b/main.cpp Tue Oct 28 18:19:08 2014 +0000
@@ -6,17 +6,18 @@
//Constanten definiëren en waarde geven
#define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
-#define KP_arm1 0.01 //Factor proprotionele regelaar arm 1
-#define KI_arm1 0 //Factor integraal regelaar arm 1
-#define KD_arm1 0 //Factor afgeleide regelaar arm 1
-#define KP_arm2 0.01 //Factor proprotionele regelaar arm 2
+#define KP_arm1 0.01 //Factor proprotionele regelaar arm 1
+#define KI_arm1 0 //Factor integraal regelaar arm 1
+#define KD_arm1 0 //Factor afgeleide regelaar arm 1
+#define KP_arm2 0.01 //Factor proprotionele regelaar arm 2
#define KI_arm2 0 //Factor integraal regelaar arm 2
-#define KD_arm2 0 //Factor afgeleide regelaar arm 2
+#define KD_arm2 0 //Factor afgeleide regelaar arm 2
#define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
#define PULS_ARM1_HOME_KALIBRATIE 180 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
#define PULS_ARM2_HOME_KALIBRATIE 393 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft
-//High Pass filter Filtercoëfficiënten
+//Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk
+//High Pass filter
#define A1 1
#define A2 -1.5610
#define A3 0.6414
@@ -24,7 +25,7 @@
#define B2 0.0402
#define B3 0.0201
-//Notch filter Filtercoëfficiënten
+//Notch filter
#define C1 1
#define C2 -1.1873E-16
#define C3 0.9391
@@ -32,7 +33,7 @@
#define D2 -1.1873E-16
#define D3 0.9695
-//Low pass filter Filtercoëfficiënten
+//Low pass filter
#define E1 1
#define E2 -1.9645
#define E3 0.9651
@@ -40,34 +41,30 @@
#define F2 3.1030E-4
#define F3 1.5515E-4
-
//Pinverdeling en naamgeving variabelen
-TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm
-MODSERIAL pc(USBTX, USBRX); //PC
+TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); //LCD scherm
+MODSERIAL pc(USBTX, USBRX); //PC
-PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
-DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1
-Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit)
-PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2
-DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2
-Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit)
-AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps
-AnalogIn EMG_tri(PTB2);
-//Blauw op 3,3 V en groen op GND
+PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1
+DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1
+Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit)
+PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2
+DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2
+Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit)
+AnalogIn EMG_bi(PTB1); //Uitlezen EMG signaal biceps
+AnalogIn EMG_tri(PTB2); //Uitlezen EMG signaal triceps
+//Blauw op 3,3 V en groen op GND, voeding beide encoders
Ticker ticker_regelaar; //Ticker voor regelaar motor
Ticker ticker_EMG; //Ticker voor EMG meten
-Timer biceps_kalibratie;
-Timer triceps_kalibratie;
+Timer biceps_kalibratie; //Timer voor kalibratiemeting EMG biceps
+Timer triceps_kalibratie; //Timer voor kalibratiemeting EMG triceps
//States definiëren
-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
+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
uint8_t state = RUST; //State is rust aan het begin
//Gedefinieerde datatypen en naamgeving en beginwaarden
-char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm
-char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm
-
float pwm_to_motor_arm1; //PWM output naar motor arm 1
float pwm_to_motor_arm2; //PWM output naar motor arm 2
float error_arm1_kalibratie; //Error in pulsen arm 1
@@ -79,40 +76,40 @@
float integraal_arm2 = 0; //Integraal actie van regelaar arm 2
float afgeleide_arm2; //Afgeleide actie van regelaar arm 2
float xb; //Gemeten EMG waarde biceps
-float xt;
+float xt; //Gemeten EMG waarde triceps
-arm_biquad_casd_df1_inst_f32 highpass_biceps;
-float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3};
-float highpass_biceps_states[4];
+arm_biquad_casd_df1_inst_f32 highpass_biceps; //Highpass_biceps IIR filter in direct form 1
+float highpass_biceps_const[] = {B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter
+float highpass_biceps_states[4]; //Aantal states van het filter, het aantal y(n-x) en x(n-x)
-arm_biquad_casd_df1_inst_f32 notch_biceps;
-float notch_biceps_const[] = {D1, D2, D3, -C2, -C3};
-float notch_biceps_states[4];
+arm_biquad_casd_df1_inst_f32 notch_biceps; //Notch_biceps IIR filter in direct form 1
+float notch_biceps_const[] = {D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter
+float notch_biceps_states[4]; //Aantal states van het filter
-arm_biquad_casd_df1_inst_f32 lowpass_biceps;
-float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3};
-float lowpass_biceps_states[4];
+arm_biquad_casd_df1_inst_f32 lowpass_biceps; //Lowpass_biceps IIR filter in direct form 1
+float lowpass_biceps_const[] = {F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter
+float lowpass_biceps_states[4]; //Aantal states van het filter
-float xbf;
-float xbfmax = 0;
+float xbf; //Gefilterde EMG waarde biceps
+float xbfmax = 0; //Maximale gefilterde EMG waarde biceps
-arm_biquad_casd_df1_inst_f32 highpass_triceps;
-float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3};
-float highpass_triceps_states[4];
+arm_biquad_casd_df1_inst_f32 highpass_triceps; //Highpass_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
+float highpass_triceps_const[] = {B1, B2, B3, -A2, -A3}; //Filtercoëfficiënten van het filter
+float highpass_triceps_states[4]; //Aantal states van het filter
-arm_biquad_casd_df1_inst_f32 notch_triceps;
-float notch_triceps_const[] = {D1, D2, D3, -C2, -C3};
-float notch_triceps_states[4];
+arm_biquad_casd_df1_inst_f32 notch_triceps; //Notch_triceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
+float notch_triceps_const[] = {D1, D2, D3, -C2, -C3}; //Filtercoëfficiënten van het filter
+float notch_triceps_states[4]; //Aantal states van het filter
-arm_biquad_casd_df1_inst_f32 lowpass_triceps;
-float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3};
-float lowpass_triceps_states[4];
+arm_biquad_casd_df1_inst_f32 lowpass_triceps; //Lowpass_biceps IIR filter in direct form 1, waarde wordt opgeslagen in een float
+float lowpass_triceps_const[] = {F1, F2, F3, -E2, -E3}; //Filtercoëfficiënten van het filter
+float lowpass_triceps_states[4]; //Aantal states van het filter
-float xtf;
-float xtfmax = 0;
+float xtf; //Gefilterde EMG waarde triceps
+float xtfmax = 0; //Maximale gefilterde EMG waarde triceps
-float xbt;
-float xtt;
+float xbt; //Thresholdwaarde EMG biceps
+float xtt; //Thresholdwaarde EMG triceps
volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma
void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true
@@ -128,116 +125,134 @@
void keep_in_range(float * in, float min, float max) //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt
{
- if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum
+ if (*in < min) //Als de waarde kleiner is als het minimum wordt de waarde het minimum
+ {
*in = min;
}
- if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum
+ if (*in > max) //Als de waarde groter is dan het maximum is de waarde het maximum
+ {
*in = max;
- } else { //In alle andere gevallen is de waarde de waarde zelf
+ }
+ else //In alle andere gevallen is de waarde de waarde zelf
+ {
*in = *in;
}
}
-void arm1_naar_thuispositie()
+void arm1_naar_thuispositie() //Brengt arm 1 naar de beginpositie
{
- 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
- integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
- afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
- pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1; //Output naar motor na PID regelaar
- keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
+ 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
+ integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
+ afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
+ pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1;//Output naar motor na PID regelaar
+ keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
- if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
+ if (pwm_to_motor_arm1 > 0) //Als PWM is positief, dan richting 1
+ {
dir_motor_arm1.write(1);
- } else { //Anders richting nul
+ }
+ else //Anders richting nul
+ {
dir_motor_arm1.write(0);
}
pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
- pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition());
- pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1);
+ pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
+ pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen
- 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)
+ 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)
+ {
state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
- pc.printf("KALIBRATIE_ARM1 afgerond\n"); //Tekst voor controle Arm 1 naar thuispositie
+ pc.printf("KALIBRATIE_ARM1 afgerond\n"); //Tekst voor controle Arm 1 naar thuispositie
}
}
-void arm2_naar_thuispositie()
+void arm2_naar_thuispositie() //Brengt arm 2 naar beginpositie
{
- 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
- integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
- afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
- pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2; //Output naar motor na PID regelaar
- keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
+ 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
+ integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
+ afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
+ pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;//Output naar motor na PID regelaar
+ keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
- if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1
+ if (pwm_to_motor_arm2 > 0) //Als PWM is positief, dan richting 1
+ {
dir_motor_arm2.write(1);
- } else { //Anders richting nul
+ }
+ else //Anders richting nul
+ {
dir_motor_arm2.write(0);
}
pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
- pc.printf("pulsmotorgetposition %d ", puls_motor_arm2.getPosition());
- pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2);
+ pc.printf("pulsmotorgetposition %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
+ pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm2); //Huidige PWM waarde naar pc sturen
- 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)
- state = EMG_KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
- pc.printf("KALIBRATIE_ARM2 afgerond\n\r"); //Tekst voor controle Arm 2 naar thuispositie
+ 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)
+ {
+ state = EMG_KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
+ pc.printf("KALIBRATIE_ARM2 afgerond\n\r"); //Tekst voor controle Arm 2 naar thuispositie
}
}
-void filter_biceps()
+void filter_biceps() //Filtert het EMG signaal van de biceps
{
- arm_biquad_cascade_df1_f32(&highpass_biceps, &xb, &xbf, 1);
+ 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
- arm_biquad_cascade_df1_f32(¬ch_biceps, &xbf, &xbf, 1);
+ arm_biquad_cascade_df1_f32(¬ch_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
+
+ xbf = fabs(xbf); //Rectifier, schrijft de absolute waarde van de xbf uit de notch opnieuw in de xbf, fabs omdat het een float is
- xbf = fabs(xbf);
+ 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
+
+ pc.printf("xbf is %f\n\r", xbf); //De gefilterde EMG waarde van de biceps naar pc sturen
+}
- arm_biquad_cascade_df1_f32(&lowpass_biceps, &xbf, &xbf, 1);
-
- pc.printf("xbf is %f\n\r", xbf);
+void filter_triceps() //Filtert het EMG signaal van de triceps
+{
+ 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
+
+ arm_biquad_cascade_df1_f32(¬ch_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
+ xtf = fabs(xtf); //Rectifier, schrijft de absolute waarde van de xtf uit de notch opnieuw in de xtf, fabs omdat het een float is
+
+ 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
+
+ pc.printf("xtf is %f\n\r", xtf); //De gefilterde EMG waarde van de triceps naar pc sturen
}
-void filter_triceps()
+int main() //Main script
{
- arm_biquad_cascade_df1_f32(&highpass_triceps, &xt, &xtf, 1);
-
- arm_biquad_cascade_df1_f32(¬ch_triceps, &xtf, &xtf, 1);
-
- xtf = fabs(xtf);
-
- arm_biquad_cascade_df1_f32(&lowpass_triceps, &xtf, &xtf, 1);
-
- pc.printf("xtf is %f\n\r", xtf);
-
-}
-
-int main()
-{
- while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
- //PC baud rate instellen
+ while(1) //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
+ {
pc.baud(38400); //PC baud rate is 38400 bits/seconde
- 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
+ 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
+ {
- case RUST: { //Aanzetten
- lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm
- lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm
- wait(2); //Twee seconden wachten
+ case RUST: //Aanzetten
+ {
+ pc.printf("RUST\n\r"); //Begin RUST naar pc sturen
+ lcd.locate(0,0); //Zet de tekst op de eerste regel
+ lcd.printf(" BMT K9 GR. 4 "); //Tekst op LCD scherm
+ lcd.locate(0,1); //Zet de tekst op de tweede regel
+ lcd.printf("HOI! IK BEN PIPO"); //Tekst op LCD scherm
+ wait(2); //Twee seconden wachten
+ lcd.cls(); //Maak LCD scherm leeg
pc.printf("RUST afgerond\n\r"); //Tekst voor controle Aanzetten
- state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
- break; //Stopt acties in deze case
+ state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel
+ break; //Stopt acties in deze case
}
- case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie
- pc.printf("KALIBRATIE_ARM1\n\r");
- wait(1); //Een seconde wachten
+ case KALIBRATIE_ARM1: //Arm 1 naar thuispositie
+ {
+ pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM1 naar pc sturen
+ wait(1); //Een seconde wachten
ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
- while(state == KALIBRATIE_ARM1) {
- while(regelaar_ticker_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ while(state == KALIBRATIE_ARM1)
+ {
+ while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
@@ -246,39 +261,43 @@
break; //Stopt acties in deze case
}
- case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie
- pc.printf("KALIBRATIE_ARM1\n\r");
- wait(1); //Een seconde wachten
+ case KALIBRATIE_ARM2: //Arm 2 naar thuispositie
+ {
+ pc.printf("KALIBRATIE_ARM1\n\r"); //Begin KALIBRATIE_ARM2 naar pc sturen
+ wait(1); //Een seconde wachten
//ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken
- while(state == KALIBRATIE_ARM2) {
- while(regelaar_ticker_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ while(state == KALIBRATIE_ARM2)
+ {
+ while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
}
wait(1); //Een seconde wachten
ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
- break; //Stopt acties in deze case
+ break; //Stopt acties in deze case
}
- case EMG_KALIBRATIE_BICEPS: {
- pc.printf("EMG__KALIBRATIE_BICEPS\n\r");
-
- lcd_r1 = " SPAN IN 5 SEC. ";
- lcd_r2 = " 2 X BICEPS AAN ";
- pc.printf("span biceps aan\n\r");
- pc.printf("EMG biceps %f\n\r",xb);
+ case EMG_KALIBRATIE_BICEPS: //Kalibratie biceps voor bepalen threshold
+ {
+ pc.printf("EMG__KALIBRATIE_BICEPS\n\r"); //Begin EMG_KALIBRATIE_BICEPS naar pc sturen
+
+ lcd.locate(0,0); //Zet tekst op eerste regel
+ lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm
+ lcd.locate(0,1); //Zet tekst op tweede regel
+ lcd.printf(" 2 X BICEPS AAN "); //Tekst op LCD scherm
- arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1, highpass_biceps_const, highpass_biceps_states);
- arm_biquad_cascade_df1_init_f32(¬ch_biceps, 1, notch_biceps_const, notch_biceps_states);
- arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1, lowpass_biceps_const, lowpass_biceps_states);
+ 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
+ arm_biquad_cascade_df1_init_f32(¬ch_biceps, 1, notch_biceps_const, notch_biceps_states); //Notchfilter biceps met bijbehorende filtercoëfficiënten en states definiëren
+ 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
- ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG);
- biceps_kalibratie.start();
+ ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconde de flag op laten steken
+ biceps_kalibratie.start(); //Timer aanzetten die de tijd meet vanaf dit punt
- while(biceps_kalibratie.read() <= 5) {
+ while(biceps_kalibratie.read() <= 5) //Zolang de timer nog geen 5 seconden heeft gemeten
+ {
while(regelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
@@ -288,19 +307,23 @@
filter_biceps();
if(int(biceps_kalibratie.read()) == 1) {
- lcd_r1 = " 4 ";
+ lcd.locate(0,0);
+ lcd.printf(" 4 ");
pc.printf("4");
}
- if(biceps_kalibratie.read() == 2) {
- lcd_r1 = " 3 ";
+ if(int(biceps_kalibratie.read()) == 2) {
+ lcd.locate(0,0);
+ lcd.printf(" 3 ");
pc.printf("3");
}
- if(biceps_kalibratie.read() == 3) {
- lcd_r1 = " 2 ";
+ if(int(biceps_kalibratie.read()) == 3) {
+ lcd.locate(0,0);
+ lcd.printf(" 2 ");
pc.printf("2");
}
- if(biceps_kalibratie.read() == 4) {
- lcd_r1 = " 1 ";
+ if(int(biceps_kalibratie.read()) == 4) {
+ lcd.locate(0,0);
+ lcd.printf(" 1 ");
pc.printf("1");
}
@@ -317,10 +340,10 @@
case EMG_KALIBRATIE_TRICEPS: {
pc.printf("EMG__KALIBRATIE_TRICEPS\n\r");
- lcd_r1 = " SPAN IN 5 SEC. ";
- lcd_r2 = " 2 X TRICEPS AAN";
- pc.printf("span triceps aan\n\r");
- pc.printf("EMG biceps %f\n\r",xt);
+ lcd.locate(0,0);
+ lcd.printf(" SPAN IN 5 SEC. ");
+ lcd.locate(0,1);
+ lcd.printf(" 2 X TRICEPS AAN");
arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1, highpass_triceps_const, highpass_triceps_states);
arm_biquad_cascade_df1_init_f32(¬ch_triceps, 1, notch_triceps_const, notch_triceps_states);
@@ -339,19 +362,24 @@
filter_triceps();
if(triceps_kalibratie.read() == 1) {
- lcd_r1 = " 4 ";
+ lcd.locate(0,0);
+ lcd.printf(" 4 ");
pc.printf("4");
}
if(triceps_kalibratie.read() == 2) {
- lcd_r1 = " 3 ";
+ lcd.locate(0,0);
+ lcd.printf(" 3 ");
pc.printf("3");
}
if(triceps_kalibratie.read() == 3) {
- lcd_r1 = " 2 ";
+ lcd.locate(0,0);
+ lcd.printf(" 2 ");
pc.printf("2");
}
if(triceps_kalibratie.read() == 4) {
- lcd_r1 = " 1 ";
+ lcd.locate(0,0);
+ lcd.printf(" 1 ");
+ pc.printf("1");
}
}
@@ -365,9 +393,11 @@
}
case BEGIN_METEN: {
- lcd_r1 = " BEGIN ";
- pc.printf("KEUZE1\n\r");
-
+ lcd.locate(0,0);
+ lcd.printf(" START ");
+
+ pc.printf("START\n\r");
+
while(state == BEGIN_METEN) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false;
@@ -384,14 +414,62 @@
state = T;
}
}
+ }
+
+ case B: {
+ lcd.locate(0,0);
+ lcd.printf(" B ");
+ pc.printf("B\n\r");
+ while(state == B) {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false;
+ xb = EMG_bi.read();
+ filter_biceps();
+ xt = EMG_tri.read();
+ filter_triceps();
+
+ if(xb >= xbt) {
+ state = BB;
+ }
+ if(xt >= xtt) {
+ state = BT;
+ }
+ }
+ }
+
+ case T: {
+ lcd.locate(0,0);
+ lcd.printf(" T ");
+ pc.printf("T\n\r");
+
+ while(state == T) {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false;
+
+ xb = EMG_bi.read();
+ filter_biceps();
+ xt = EMG_tri.read();
+ filter_triceps();
+
+ if(xb >= xbt) {
+ state = TB;
+ }
+ if(xt >= xtt) {
+ state = TT;
+ }
+ }
+ }
+
+ case BB:
- default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
- state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
- }
+ default:
+ { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case
+ state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan
}
+
pc.printf("state: %u\n\r",state);
}
}