Moteur.cpp

00001 #include "Moteur.h"
00002 
00003 //t_asserv en seconde
00004 Moteur::Moteur(PinName pin_pwm1, PinName pin_pwm2, PinName pin_feedback, PinName pin_capteur1, PinName pin_capteur2) : 
00005     m1(pin_pwm1),m2(pin_pwm2),
00006     feedbackCurrent(pin_feedback),
00007     capteur1(pin_capteur1),
00008     capteur2(pin_capteur2),
00009     tmr() 
00010 {
00011     this->derniere_mesure = 0;
00012     //INIT DES PWM
00013     this->m1.period(MOT_PERIOD);
00014     this->m2.period(MOT_PERIOD);
00015     this->setPuissance(0);
00016 
00017     //INIT CAPTEUR
00018     this->capteur1.rise(this,&Moteur::interruptCapteurRise);
00019     this->capteur1.fall(this,&Moteur::interruptCapteurFall);
00020 }
00021 
00022 /*-1 : full marche arrière
00023  1 : full marche avant
00024  */
00025 void Moteur::setPuissance(float p)
00026 {
00027     if (p>0) {
00028         this->m1.write(0);
00029         this->m2.write(p);
00030     }else{
00031         this->m1.write(-p);
00032         this->m2.write(0);
00033     }
00034 }
00035 
00036 void Moteur::test1()
00037 {
00038         //RAZ DES SORTIES
00039         this->m1.write(0.0f);
00040         this->m2.write(0.0f);
00041         
00042         //DEBUT SEQUENCE DE TEST
00043         xbee.printf("Debut Test Propulsion\n");
00044         this->m1.write(0.5);
00045         xbee.printf("M1 = 0.5\n");
00046         wait(10.0f);
00047         this->m1.write(0);
00048         this->m2.write(0.5);
00049         xbee.printf("M2 = 0.5\n");
00050         wait(10.0f);
00051         this->m2.write(0);
00052         xbee.printf("Fin Test Propulsion\n");
00053 }
00054 
00055 void Moteur::test2()
00056 {
00057         //RAZ DES SORTIES
00058         this->m1.write(0.0f);
00059         this->m2.write(0.0f);
00060         
00061         //DEBUT SEQUENCE DE TEST
00062         xbee.printf("Debut Test Propulsion\n");
00063         this->m1.write(0.5);
00064         xbee.printf("M1 = 0.5\n");
00065         wait(10.0f);
00066         this->m1.write(0);
00067         this->m2.write(0.5);
00068         xbee.printf("M2 = 0.5\n");
00069         wait(10.0f);
00070         this->m2.write(0);
00071         xbee.printf("Fin Test Propulsion\n");
00072 }
00073 
00074     
00075 void Moteur::etalonnage() {
00076     xbee.printf("ETALONNAGE\n");
00077     for(int i=0;i<256;i++) { 
00078         this->m1.write((float)i/256.0f);
00079         xbee.printf("D1 : %d\n",i);
00080         wait(0.1f);
00081     }
00082 }
00083 
00084 
00085 void Moteur::asserv()
00086 {
00087     //Application de la nouvelle commande
00088     this->setPuissance(regulateur.getCommande(this->erreur));
00089 }
00090 
00091 void Moteur::setKp(float k)
00092 {
00093     this->regulateur.setKp(k);
00094 }
00095 void Moteur::setKi(float k)
00096 {
00097     this->regulateur.setKp(k);
00098 }
00099 void Moteur::setKd(float k)
00100 {
00101     this->regulateur.setKp(k);
00102 }
00103 
00104 
00105 void Moteur::interruptCapteurRise()
00106 {
00107     this->mesure();
00108     this->majErreur();
00109     this->asserv();
00110 }
00111 
00112 void Moteur::interruptCapteurFall()
00113 {
00114     this->mesure();
00115     this->majErreur();
00116     this->asserv();
00117 }
00118 
00119 void Moteur::mesure()
00120 {
00121     this->derniere_mesure = 1/this->tmr.read();
00122     if(this->capteur2) {
00123         this->derniere_mesure = -this->derniere_mesure;
00124     }
00125     this->tmr.reset();
00126 }
00127 
00128 void Moteur::majErreur()
00129 {
00130     float e_prec = this->erreur.p;
00131     
00132     this->erreur.p = this->derniere_mesure - this->vitesse_consigne;
00133     this->erreur.i += this->erreur.p;
00134     this->erreur.d = this->erreur.p - e_prec;
00135 }
00136 
00137 float Moteur::getMesure()
00138 {
00139     return this->derniere_mesure;
00140 }
00141 
00142 void Moteur::setVitesse(float v)
00143 {
00144     this->vitesse_consigne = v;
00145 }