Diff: source/Gestion_Moteur.cpp

Revision:

0:3af30bfbc3e5

Child:

5:4d1a524433ca

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/Gestion_Moteur.cpp	Tue Jan 13 15:37:52 2015 +0000
@@ -0,0 +1,147 @@
+//Bibliothéque
+#include "QEI.h"
+#include "mbed.h"
+#include "Gestion_Moteur.h"
+#include "Servo.h"
+
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+//DigitalOut FLAG(PTA5);
+
+
+AnalogOut FLAG(DAC0_OUT);
+Timeout timeout;
+PwmOut Pwm_moteur_1(PTD4);  // Moteur gauche
+PwmOut Pwm_moteur_2(PTA1);  // Moteur droit
+Ticker ticker_asserv;
+Ticker ticker_servo;
+//  Declaration des differents objets, variables necessaires au bon fonctionnement de la regulation des moteurs dediés à la propulsion
+double Periode_echantillonage=0.0001;
+// QEI moteur gauche (QEI pinA, QEI pinB, NC, pulses/tour)
+QEI decodeur_1 (PTB0,PTB1, NC, 30);
+// QEI moteur droit (QEI pinA, QEI pinB, NC, pulses/tour)
+QEI decodeur_2 (PTB18,PTB19, NC, 30);
+
+
+// Variables d'asserv moteur gauche
+// Mesure brute QEI 1, Mesure brute QEI 1 précédente, Écart mesuré, Écart de consigne
+int mesure1=0,mesure_precedente1=0,mesure_moteur_1,consigne_moteur_1=4; 
+// Coeff proportionnel, Intégral, Valeur intégrale, valeur sortie de l'asserv, consigne PWM envoyée au moteur gauche
+float Kp_moteur_1=2,Ki_moteur_1=2,I_x_moteur_1=0,sortie_asserv1,consigne_PWM1;
+// Variables d'asserv moteur droit
+int mesure2=0,mesure_precedente2=0,mesure_moteur_2,consigne_moteur_2=4;
+// Coeff proportionnel, Intégral, Valeur intégrale, valeur sortie de l'asserv, consigne PWM envoyée au moteur droit
+float Kp_moteur_2=2,Ki_moteur_2=2,I_x_moteur_2=0,sortie_asserv2,consigne_PWM2;
+
+// Pins de sortie PWM
+
+
+double i=0.4;
+/*
+void move_servo()
+{
+    
+    float mem;
+    mem=FLAG.read();
+    FLAG.write(0.5);
+
+    if (i<0.7) i+=0.001;
+    else i=0.4;
+    servo = i; 
+
+    FLAG.write(mem);
+}
+*/
+void mesure_interruption(void)
+{
+    float mem;
+    mem=FLAG.read();
+    FLAG.write(1);
+    // Calcul de l'écart pour le moteur gauche
+    mesure1=decodeur_1.getPulses();
+    if (mesure1 >= mesure_precedente1)
+        mesure_moteur_1=mesure1-mesure_precedente1;
+    else
+        mesure_moteur_1=mesure1-mesure_precedente1+32767;
+    mesure_precedente1=mesure1;
+    
+    // Calcul de l'écart pour le moteur droit
+      mesure2=decodeur_2.getPulses();
+    if (mesure2 >= mesure_precedente2)
+        mesure_moteur_2=mesure2-mesure_precedente2;
+    else
+        mesure_moteur_2=mesure2-mesure_precedente2+32767;
+    mesure_precedente2=mesure2;
+        
+        
+    Bloc_propulsion();   
+    FLAG.write(mem);
+}
+void init_led(void)
+{
+        // Ticker asserv périodique :
+        //timeout.attach_us(&retard_servo,10);
+        //ticker_servo.attach_us(&move_servo,0.05*1000000);
+        ticker_asserv.attach_us(&mesure_interruption,Periode_echantillonage*1000000); 
+        led_red=1;
+        led_green=1;
+        Pwm_moteur_1.period_us(100);
+        Pwm_moteur_2.period_us(100);
+        //
+}
+    
+
+double Regulateur_PI_decodeur(double consigne,double mesure, double Kp, double Ki ,double Periode_echantillonage,double I_x )
+{
+    // Kp Ki sont des parametres Globals modifiable via la transmission série 
+    // Periode_echantillonage est une constante à tous le système
+    // I_x est une valeur interne du regulateur
+    // la consigne est la vitesse calculé par le calculateur moteur
+    // la mesure  est la vitesse mesuré par les qei
+    
+    //Consigne et Mesure doivent être dans les mêm unité : rad/s
+    double ecart, P_x,commande;
+    /////// Régulation PI ///////
+    // Ecart entre la consigne et la mesure
+    ecart = consigne - mesure;
+    
+    // Terme proportionnel
+    P_x = Kp * ecart;
+     
+    // Calcul de la commande
+    commande = P_x + I_x;
+     
+    // Terme intégral (sera utilisé lors du pas d'échantillonnage suivant)
+    I_x = I_x + Ki * Periode_echantillonage * ecart;
+    /////// Fin régulation PID ///////
+    return commande;
+    }
+
+void Bloc_propulsion()
+{
+    //
+    sortie_asserv1 = (float)Regulateur_PI_decodeur(consigne_moteur_1, mesure_moteur_1, Kp_moteur_1, Ki_moteur_1, Periode_echantillonage,I_x_moteur_1 );
+    //
+    sortie_asserv2 = (float)Regulateur_PI_decodeur(consigne_moteur_2, mesure_moteur_2, Kp_moteur_2, Ki_moteur_2, Periode_echantillonage,I_x_moteur_2 );
+    
+    //Conversion en PWM
+    consigne_PWM1=sortie_asserv1/20;
+    consigne_PWM2=sortie_asserv2/20; 
+    
+    // Saturation
+        // Moteur gauche
+    if (consigne_PWM1 > 1)
+        consigne_PWM1=1;
+    else if  (consigne_PWM1 <0)   
+        consigne_PWM1=0;
+        // Moteur droit
+    if (consigne_PWM2 > 1)
+        consigne_PWM2=1;
+    else if  (consigne_PWM2 <0)   
+        consigne_PWM2=0;   
+        
+    // Envoi PWM aux moteurs
+        Pwm_moteur_1.write(consigne_PWM1);
+        Pwm_moteur_2.write(consigne_PWM2);
+    }
+