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:
- 20:1cb0cf0d49ac
- Parent:
- 19:38c9d177b6ee
- Child:
- 21:f4e9f6c28de1
diff -r 38c9d177b6ee -r 1cb0cf0d49ac main.cpp --- a/main.cpp Thu Oct 30 16:12:46 2014 +0000 +++ b/main.cpp Thu Oct 30 19:29:31 2014 +0000 @@ -6,9 +6,9 @@ //Constanten definiëren en waarde geven #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor -#define KP_arm1 0.5 //Factor proprotionele regelaar arm 1 -#define KI_arm1 0.1 //Factor integraal regelaar arm 1 -#define KD_arm1 0.01 //Factor afgeleide regelaar arm 1 +#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 @@ -58,6 +58,8 @@ Ticker ticker_EMG; //Ticker voor EMG meten Timer biceps_kalibratie; //Timer voor kalibratiemeting EMG biceps Timer triceps_kalibratie; //Timer voor kalibratiemeting EMG triceps +Ticker ticker_motor_arm1_pid; +Ticker ticker_motor_arm2_pid; //States definiëren enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, START, B, T, BB, BT, TB, TT, BBB, BBT, BTB, BTT, TBB, TBT, TTB, TTT, BBBB, BBBT, BBTB, BBTT, BTBB, BTBT, BTTB, BTTT, TBBB, TBBT, TBTB, TBTT, TTBB, TTBT, TTTB, TTTT}; //Alle states benoemen, ieder krijgt een getal beginnend met 0 @@ -137,35 +139,43 @@ } } -void arm1_naar_thuispositie() //Brengt arm 1 naar de beginpositie +void motor_arm1_pid() { - referentie_arm1 = referentie_arm1 + 180.0/200.0; - - if (referentie_arm1 >= 180) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer) - referentie_arm1 = 180; - - } error_arm1_kalibratie = (referentie_arm1 - 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); + 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); - 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()); //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 +} - 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 +void motor_arm2_pid() +{ + 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 } + void arm2_naar_thuispositie() //Brengt arm 2 naar beginpositie { referentie_arm2 = referentie_arm2 + 35/200; @@ -219,9 +229,11 @@ int main() //Main script { + 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 - 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 @@ -248,7 +260,18 @@ 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 + 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; + } + } wait(1); //Een seconde wachten break; //Stopt acties in deze case @@ -264,7 +287,18 @@ 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 + referentie_arm2 = referentie_arm2 + 35/200; + + 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) + { + referentie_arm2 = 35; + state = EMG_KALIBRATIE_BICEPS; + } + } wait(1); //Een seconde wachten ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer @@ -978,4 +1012,5 @@ } //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) } - } \ No newline at end of file + } +} \ No newline at end of file