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:
- 21:f4e9f6c28de1
- Parent:
- 20:1cb0cf0d49ac
- Child:
- 22:838a17065bc7
--- a/main.cpp Thu Oct 30 19:29:31 2014 +0000
+++ b/main.cpp Fri Oct 31 12:37:06 2014 +0000
@@ -9,9 +9,9 @@
#define KP_arm1 0.02 //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.3 //Factor proprotionele regelaar arm 2
-#define KI_arm2 0.1 //Factor integraal regelaar arm 2
-#define KD_arm2 0.1 //Factor afgeleide regelaar arm 2
+#define KP_arm2 0.05 //Factor proprotionele regelaar arm 2
+#define KI_arm2 0 //Factor integraal regelaar arm 2
+#define KD_arm2 0 //Factor afgeleide regelaar arm 2
#define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten
//Filtercoëfficiënten per filter, allemaal 2e orde filters, zo cascade van hogere orde mogelijk
@@ -145,14 +145,12 @@
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);
+ vorige_error_arm1 = error_arm1_kalibratie;
+ keep_in_range(&pwm_to_motor_arm1, -1, 1);
- 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);
}
@@ -165,6 +163,7 @@
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
+ vorige_error_arm2 = error_arm2_kalibratie;
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
@@ -175,31 +174,6 @@
pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
}
-
-void arm2_naar_thuispositie() //Brengt arm 2 naar beginpositie
-{
- referentie_arm2 = referentie_arm2 + 35/200;
- 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
- 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
- dir_motor_arm2.write(1);
- } 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()); //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
- }
-}
-
void filter_biceps() //Filtert het EMG signaal van de biceps
{
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
@@ -231,7 +205,7 @@
{
ticker_motor_arm1_pid.attach(motor_arm1_pid,SAMPLETIME_REGELAAR);
ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
-
+
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
@@ -261,17 +235,16 @@
regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
referentie_arm1 = referentie_arm1 + 180.0/200.0;
-
+
pc.printf("pulsmotorgetposition1 %d ", puls_motor_arm1.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
pc.printf("pwmmotor1 %f ", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen
pc.printf("referentie1 %f\n\r", referentie_arm1);
-
- if (referentie_arm1 >= 180)
- {
- referentie_arm1 = 180;
- state = KALIBRATIE_ARM2;
+
+ if (referentie_arm1 >= 180) {
+ referentie_arm1 = 180;
+ state = KALIBRATIE_ARM2;
}
-
+
}
wait(1); //Een seconde wachten
break; //Stopt acties in deze case
@@ -287,18 +260,17 @@
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
- referentie_arm2 = referentie_arm2 + 35/200;
-
+ referentie_arm2 = referentie_arm2 + 35.0/200.0;
+
pc.printf("pulsmotorgetposition2 %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
pc.printf("pwmmotor2 %f ", pwm_to_motor_arm2); //Huidige PWM waarde naar motor naar pc sturen
pc.printf("referentie2 %f\n\r", referentie_arm2);
-
- if(referentie_arm2 >= 35)
- {
+
+ if(referentie_arm2 >= 35) {
referentie_arm2 = 35;
state = EMG_KALIBRATIE_BICEPS;
}
-
+
}
wait(1); //Een seconde wachten
ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
@@ -312,6 +284,8 @@
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
+ wait(1);
+ lcd.cls();
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
arm_biquad_cascade_df1_init_f32(¬ch_biceps, 2, notch_biceps_const, notch_biceps_states); //Notchfilter biceps met bijbehorende filtercoëfficiënten en states definiëren
@@ -355,12 +329,13 @@
pc.printf("1"); //Controle naar pc sturen
}
- }
- if(xbf >= xbfmax) { //Als de gefilterde EMG waarde groter is dan xbfmax
- xbfmax = xbf; //Dan wordt deze gefilterde EMG waarde de nieuwe xbfmax
+ if(xbf >= xbfmax) { //Als de gefilterde EMG waarde groter is dan xbfmax
+ xbfmax = xbf; //Dan wordt deze gefilterde EMG waarde de nieuwe xbfmax
+ }
}
- xbt = xbfmax * 0.8; //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten
+ xbt = xbfmax * 0.95; //De threshold voor de biceps wordt 80% van de xfbmax na de 5 seconden meten
+ pc.printf("maximale biceps %f", xbfmax);
pc.printf("threshold is %f\n\r", xbt); //Thresholdwaarde naar pc sturen
state = EMG_KALIBRATIE_TRICEPS; //Gaat door naar kalibratie van de EMG van de triceps
break; //Stopt alle acties in deze case
@@ -373,6 +348,8 @@
lcd.printf(" SPAN IN 5 SEC. "); //Tekst op LCD scherm
lcd.locate(0,1); //Zet tekst op eerste regel
lcd.printf(" 2 X TRICEPS AAN"); //Tekst op LCD scherm
+ wait(1);
+ lcd.cls();
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
arm_biquad_cascade_df1_init_f32(¬ch_triceps, 2, notch_triceps_const, notch_triceps_states); //Notchfilter triceps met bijbehorende filtercoëfficiënten en states definiëren
@@ -415,12 +392,14 @@
pc.printf("1"); //Controle naar pc sturen
}
- }
- if(xtf >= xtfmax) { //Als de gefilterde EMG waarde groter is dan xtfmax
- xtfmax = xtf; //Dan wordt deze gefilterde EMG waarde de nieuwe xtfmax
+
+ if(xtf >= xtfmax) { //Als de gefilterde EMG waarde groter is dan xtfmax
+ xtfmax = xtf; //Dan wordt deze gefilterde EMG waarde de nieuwe xtfmax
+ }
}
- xtt = xtfmax * 0.8; //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten
+ xtt = xtfmax * 0.90; //Thresholdwaarde is 80% van de xtfmax na 5 seconden meten
+ pc.printf("maximale triceps %f", xtfmax);
pc.printf("threshold is %f\n\r", xtt); //Thresholdwaarde naar pc sturen
state = START; //Gaat door naar het slaan, kalibratie nu afgerond
break; //Stopt alle acties in deze case
@@ -441,10 +420,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = B; //Ga door naar state B
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = T; //Ga door naar state T
}
}
@@ -455,20 +434,24 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" B "); //Tekst op LCD scherm
pc.printf("B\n\r"); //Controle naar pc sturen
+
+ wait(3);
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; //Flag weer naar beneden, zodat deze straks weer omhoog kan
-
+
+ xb = 0;
+ xbf = 0;
xb = EMG_bi.read(); //EMG signaal biceps uitlezen
filter_biceps(); //EMG signaal biceps filteren
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BB; //Ga door naar state BB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf>= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = BT; //Ga door naar state BT
}
}
@@ -479,6 +462,8 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" T "); //Tekst op LCD scherm
pc.printf("T\n\r"); //Controle naar pc sturen
+
+ wait(3);
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
@@ -489,10 +474,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TB; //Ga door naar state TB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = TT; //Ga door naar state TT
}
}
@@ -503,20 +488,23 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BB "); //Tekst op LCD scherm
pc.printf("BB\n\r"); //Controle naar pc sturen
+
+ wait(3);
while(state == BB) {
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
xb = EMG_bi.read(); //EMG signaal biceps uitlezen
+ xbf = 0;
filter_biceps(); //EMG signaal biceps filteren
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BBB; //Ga door naar state BBB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = BBT; //Ga door naar state BBT
}
}
@@ -527,6 +515,8 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BT "); //Tekst op LCD scherm
pc.printf("BT\n\r"); //Controle naar pc sturen
+
+ wait(3);
while(state == BT) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -537,10 +527,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BTB; //Ga door naar state BTB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = BTT; //Ga door naar state BTT
}
}
@@ -551,6 +541,8 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TB "); //Tekst op LCD scherm
pc.printf("TB\n\r"); //Controle naar pc sturen
+
+ wait(3);
while(state == TB) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -561,10 +553,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TBB; //Ga door naar state TBB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = TBT; //Ga door naar state TBT
}
}
@@ -575,6 +567,8 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TT "); //Tekst op LCD scherm
pc.printf("TT\n\r"); //Controle naar pc sturen
+
+ wait(3);
while(state == TT) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -585,10 +579,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TTB; //Ga door naar state TTB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = TTT; //Ga door naar state TTT
}
}
@@ -599,6 +593,8 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BBB "); //Tekst op LCD scherm
pc.printf("BBB\n\r"); //Controle naar pc sturen
+
+ wait(3);
while(state == BBB) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -609,10 +605,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BBBB; //Ga door naar state BBBB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = BBBT; //Ga door naar state BBBT
}
}
@@ -624,6 +620,8 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BBT "); //Tekst op LCD scherm
pc.printf("BBT\n\r"); //Controle naar pc sturen
+
+ wait(3);
while(state == BBT) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -634,10 +632,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BBTB; //Ga door naar state BBTB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = BBTT; //Ga door naar state BBTT
}
}
@@ -648,6 +646,8 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BTB "); //Tekst op LCD scherm
pc.printf("BTB\n\r"); //Controle naar pc sturen
+
+ wait(3);
while(state == BTB) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -658,10 +658,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BTBB; //Ga door naar state BTBB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = BTBT; //Ga door naar state BTBT
}
}
@@ -672,6 +672,8 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BTT "); //Tekst op LCD scherm
pc.printf("BTT\n\r"); //Controle naar pc sturen
+
+ wait(3);
while(state == BTT) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -682,10 +684,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BTTB; //Ga door naar state BTTB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = BTTT; //Ga door naar state BTTT
}
}
@@ -696,6 +698,8 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TBB "); //Tekst op LCD scherm
pc.printf("TBB\n\r"); //Controle naar pc sturen
+
+ wait(3);
while(state == TBB) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -706,10 +710,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TBBB; //Ga door naar state TBBB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = TBBT; //Ga door naar state TBBT
}
}
@@ -720,6 +724,8 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TBT "); //Tekst op LCD scherm
pc.printf("TBT\n\r"); //Controle naar pc sturen
+
+ wait(3);
while(state == TBT) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -730,10 +736,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TBTB; //Ga door naar state TBTB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = TBTT; //Ga door naar state TBTT
}
}
@@ -744,6 +750,8 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" BBB "); //Tekst op LCD scherm
pc.printf("BBB\n\r"); //Controle naar pc sturen
+
+ wait(3);
while(state == BBB) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -754,10 +762,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TTBB; //Ga door naar state TTBB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = TTBT; //Ga door naar state TTBT
}
}
@@ -768,6 +776,8 @@
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TTT "); //Tekst op LCD scherm
pc.printf("TTT\n\r"); //Controle naar pc sturen
+
+ wait(3);
while(state == TTT) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -778,10 +788,10 @@
xt = EMG_tri.read(); //EMG signaal triceps uitlezen
filter_triceps(); //EMG signaal triceps filteren
- if(xb >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
+ if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TTTB; //Ga door naar state TTTB
}
- if(xt >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
+ if(xtf >= xtt) { //Als de gefilterde EMG van de triceps groter is dan de ingestelde threshold
state = TTTT; //Ga door naar state TTTT
}
}
@@ -956,7 +966,7 @@
case TTTT: {
lcd.locate(0,0); //Zet tekst op eerste regel
lcd.printf(" TTTT "); //Tekst op LCD scherm
- pc.printf("TTBB\n\r"); //Controle naar pc sturen
+ pc.printf("TTTT\n\r"); //Controle naar pc sturen
while(state == TTTT) {
while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
@@ -965,52 +975,27 @@
//Positie arm 1 is goed
//Snelheid arm 2
- float referentie_speed_TTTT;
- float error_speed_TTTT;
- float integraal_speed_TTTT = 0;
- float afgeleide_speed_TTTT;
- float vorige_error_speed_TTTT = 0;
- float pwm_to_motor_speed_TTTT;
-#define KPs 0.001
-#define KIs 0
-#define KDs 0
- referentie_speed_TTTT = referentie_speed_TTTT + 1902/40;
- error_speed_TTTT = (referentie_speed_TTTT - puls_motor_arm2.getSpeed());
- integraal_speed_TTTT = integraal_speed_TTTT + error_speed_TTTT*SAMPLETIME_REGELAAR;
- afgeleide_speed_TTTT = (error_speed_TTTT - vorige_error_speed_TTTT)/SAMPLETIME_REGELAAR;
- pwm_to_motor_speed_TTTT = error_speed_TTTT*KPs + integraal_speed_TTTT*KIs + afgeleide_speed_TTTT*KDs;
- keep_in_range(&pwm_to_motor_speed_TTTT, -1, 1);
-
- if(pwm_to_motor_speed_TTTT > 0) {
- dir_motor_arm2.write(1);
- } else {
- dir_motor_arm2.write(0);
- }
-
- pwm_motor_arm1.write(0);
+ referentie_arm2 = referentie_arm2 + 0.1902;
- if(puls_motor_arm2.getPosition() >= 97) {
- pwm_motor_arm1.write(-fabs(pwm_to_motor_speed_TTTT));
+ if(referentie_arm2 >= 9.51) {
+ while(pwm_to_motor_arm2 != 0) {
+ referentie_arm2 = referentie_arm2 - 90/200;
+ }
+ if(pwm_to_motor_arm2 == 0) {
+ state = START;
+ }
}
-
- if(referentie_speed_TTTT == 1902) {
- referentie_speed_TTTT = 1902;
- }
-
- referentie_arm1 = referentie_arm1 + 180/200;
-
- break; //Stop met alle acties in deze case
}
-
- 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);
- }
- //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
+ pc.printf("state: %u\n\r",state);
}
+ //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
}
-}
\ No newline at end of file
+}
+