Diff: Moteur.cpp

Revision:

0:1b0dce2e60ba

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Moteur.cpp	Mon Dec 01 21:24:13 2014 +0000
@@ -0,0 +1,145 @@
+#include "Moteur.h"
+
+//t_asserv en seconde
+Moteur::Moteur(PinName pin_pwm1, PinName pin_pwm2, PinName pin_feedback, PinName pin_capteur1, PinName pin_capteur2) : 
+    m1(pin_pwm1),m2(pin_pwm2),
+    feedbackCurrent(pin_feedback),
+    capteur1(pin_capteur1),
+    capteur2(pin_capteur2),
+    tmr() 
+{
+    this->derniere_mesure = 0;
+    //INIT DES PWM
+    this->m1.period(MOT_PERIOD);
+    this->m2.period(MOT_PERIOD);
+    this->setPuissance(0);
+
+    //INIT CAPTEUR
+    this->capteur1.rise(this,&Moteur::interruptCapteurRise);
+    this->capteur1.fall(this,&Moteur::interruptCapteurFall);
+}
+
+/*-1 : full marche arrière
+ 1 : full marche avant
+ */
+void Moteur::setPuissance(float p)
+{
+    if (p>0) {
+        this->m1.write(0);
+        this->m2.write(p);
+    }else{
+        this->m1.write(-p);
+        this->m2.write(0);
+    }
+}
+
+void Moteur::test1()
+{
+        //RAZ DES SORTIES
+        this->m1.write(0.0f);
+        this->m2.write(0.0f);
+        
+        //DEBUT SEQUENCE DE TEST
+        xbee.printf("Debut Test Propulsion\n");
+        this->m1.write(0.5);
+        xbee.printf("M1 = 0.5\n");
+        wait(10.0f);
+        this->m1.write(0);
+        this->m2.write(0.5);
+        xbee.printf("M2 = 0.5\n");
+        wait(10.0f);
+        this->m2.write(0);
+        xbee.printf("Fin Test Propulsion\n");
+}
+
+void Moteur::test2()
+{
+        //RAZ DES SORTIES
+        this->m1.write(0.0f);
+        this->m2.write(0.0f);
+        
+        //DEBUT SEQUENCE DE TEST
+        xbee.printf("Debut Test Propulsion\n");
+        this->m1.write(0.5);
+        xbee.printf("M1 = 0.5\n");
+        wait(10.0f);
+        this->m1.write(0);
+        this->m2.write(0.5);
+        xbee.printf("M2 = 0.5\n");
+        wait(10.0f);
+        this->m2.write(0);
+        xbee.printf("Fin Test Propulsion\n");
+}
+
+    
+void Moteur::etalonnage() {
+    xbee.printf("ETALONNAGE\n");
+    for(int i=0;i<256;i++) { 
+        this->m1.write((float)i/256.0f);
+        xbee.printf("D1 : %d\n",i);
+        wait(0.1f);
+    }
+}
+
+
+void Moteur::asserv()
+{
+    //Application de la nouvelle commande
+    this->setPuissance(regulateur.getCommande(this->erreur));
+}
+
+void Moteur::setKp(float k)
+{
+    this->regulateur.setKp(k);
+}
+void Moteur::setKi(float k)
+{
+    this->regulateur.setKp(k);
+}
+void Moteur::setKd(float k)
+{
+    this->regulateur.setKp(k);
+}
+
+
+void Moteur::interruptCapteurRise()
+{
+    this->mesure();
+    this->majErreur();
+    this->asserv();
+}
+
+void Moteur::interruptCapteurFall()
+{
+    this->mesure();
+    this->majErreur();
+    this->asserv();
+}
+
+void Moteur::mesure()
+{
+    this->derniere_mesure = 1/this->tmr.read();
+    if(this->capteur2) {
+        this->derniere_mesure = -this->derniere_mesure;
+    }
+    this->tmr.reset();
+}
+
+void Moteur::majErreur()
+{
+    float e_prec = this->erreur.p;
+    
+    this->erreur.p = this->derniere_mesure - this->vitesse_consigne;
+    this->erreur.i += this->erreur.p;
+    this->erreur.d = this->erreur.p - e_prec;
+}
+
+float Moteur::getMesure()
+{
+    return this->derniere_mesure;
+}
+
+void Moteur::setVitesse(float v)
+{
+    this->vitesse_consigne = v;
+}
\ No newline at end of file