![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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
diff -r 1cb0cf0d49ac -r f4e9f6c28de1 main.cpp --- 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 +} +