Anthony JUTON
/
Test_asservissement
test asservissement AJ
Revision 2:5b742cba102c, committed 2018-10-12
- Comitter:
- ajuton
- Date:
- Fri Oct 12 16:30:27 2018 +0000
- Parent:
- 1:fe931ebff0b4
- Commit message:
- prog asservissement Nucleo AJ
Changed in this revision
QEI.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Fri Oct 12 16:30:27 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa
--- a/main.cpp Wed Jun 07 13:27:00 2017 +0000 +++ b/main.cpp Fri Oct 12 16:30:27 2018 +0000 @@ -1,29 +1,60 @@ #include "mbed.h" +#include "QEI.h" DigitalOut my_led(LED1); -InterruptIn my_button(USER_BUTTON); -PwmOut my_pwm(PB_3); +DigitalIn bouton(USER_BUTTON); +PwmOut pwm_moteur1(PA_8); +DigitalOut Enable(PB_5); +PwmOut pwm_moteur2(PB_3); +Serial pc(USBTX,USBRX); +QEI codeur(D8, D9, NC, 48, QEI::X4_ENCODING ); +AnalogOut sortie_position(A2); +AnalogIn entree_consigne(A0); +Ticker ticker_regul; + +void moteur(float ratio); + +void boucle_regul(void){ + float consigne; + float position_roue; + static float erreur; + static float Ierreur; + float Kp = 0.05; + float KiTe = 0.00001; -void pressed() { - if (my_pwm.read() == 0.25) { - my_pwm.write(0.75); + Ierreur = Ierreur + erreur; + consigne = entree_consigne * 360.0; + position_roue = ( ((codeur.getPulses()*10)%9792) * 36.0/(48*20.4) ); + erreur = consigne - position_roue; + moteur(Kp * erreur + KiTe *Ierreur ); + sortie_position.write(position_roue/360.0); + } - else { - my_pwm.write(0.25); + +int main() +{ + + pwm_moteur1.period_us(50); + pwm_moteur2.period_us(50); + pwm_moteur2.write(0); + Enable = 1; + ticker_regul.attach(&boucle_regul,0.001); + + pc.printf("TP asservissement position\n\r"); + + while (1) { + } } -int main() -{ - // Set PWM - my_pwm.period_ms(10); - my_pwm.write(0.5); - - // Set button - my_button.fall(&pressed); - - while (1) { - my_led = !my_led; - wait(0.5); // 500 ms +void moteur(float ratio){ + if (ratio >= (float)0.0) { + pwm_moteur1.write(0); + pwm_moteur2.write(ratio); } + else{ + pwm_moteur1.write(-ratio); + pwm_moteur2.write(0); + } } + \ No newline at end of file