kevin eccli / Mbed 2 deprecated Freescale_CupV4

Adaptation K22F

Dependencies:   mbed

Moteur.cpp@0:6004a7230f87, 2014-12-16 (annotated)

Committer:
singularity
Date:
Tue Dec 16 15:52:17 2014 +0000
Revision:
0:6004a7230f87
Creation k22F

Who changed what in which revision?

UserRevisionLine numberNew contents of line
singularity 0:6004a7230f87 1 #include "Moteur.h"
singularity 0:6004a7230f87 2
singularity 0:6004a7230f87 3 //t_asserv en seconde
singularity 0:6004a7230f87 4 Moteur::Moteur(PinName pin_pwm1, PinName pin_pwm2, PinName pin_feedback, PinName pin_capteur1, PinName pin_capteur2) :
singularity 0:6004a7230f87 5 m1(pin_pwm1),m2(pin_pwm2),
singularity 0:6004a7230f87 6 feedbackCurrent(pin_feedback),
singularity 0:6004a7230f87 7 capteur1(pin_capteur1),
singularity 0:6004a7230f87 8 capteur2(pin_capteur2),
singularity 0:6004a7230f87 9 tmr()
singularity 0:6004a7230f87 10 {
singularity 0:6004a7230f87 11 this->derniere_mesure = 0;
singularity 0:6004a7230f87 12 //INIT DES PWM
singularity 0:6004a7230f87 13 this->m1.period(MOT_PERIOD);
singularity 0:6004a7230f87 14 this->m2.period(MOT_PERIOD);
singularity 0:6004a7230f87 15 this->setPuissance(0);
singularity 0:6004a7230f87 16
singularity 0:6004a7230f87 17 //INIT CAPTEUR
singularity 0:6004a7230f87 18 this->capteur1.rise(this,&Moteur::interruptCapteurRise);
singularity 0:6004a7230f87 19 this->capteur1.fall(this,&Moteur::interruptCapteurFall);
singularity 0:6004a7230f87 20 }
singularity 0:6004a7230f87 21
singularity 0:6004a7230f87 22 /*-1 : full marche arrière
singularity 0:6004a7230f87 23 1 : full marche avant
singularity 0:6004a7230f87 24 */
singularity 0:6004a7230f87 25 void Moteur::setPuissance(float p)
singularity 0:6004a7230f87 26 {
singularity 0:6004a7230f87 27 if (p>0) {
singularity 0:6004a7230f87 28 this->m1.write(0);
singularity 0:6004a7230f87 29 this->m2.write(p);
singularity 0:6004a7230f87 30 }else{
singularity 0:6004a7230f87 31 this->m1.write(-p);
singularity 0:6004a7230f87 32 this->m2.write(0);
singularity 0:6004a7230f87 33 }
singularity 0:6004a7230f87 34 }
singularity 0:6004a7230f87 35
singularity 0:6004a7230f87 36 void Moteur::test1()
singularity 0:6004a7230f87 37 {
singularity 0:6004a7230f87 38 //RAZ DES SORTIES
singularity 0:6004a7230f87 39 this->m1.write(0.0f);
singularity 0:6004a7230f87 40 this->m2.write(0.0f);
singularity 0:6004a7230f87 41
singularity 0:6004a7230f87 42 //DEBUT SEQUENCE DE TEST
singularity 0:6004a7230f87 43 xbee.printf("Debut Test Propulsion\n");
singularity 0:6004a7230f87 44 this->m1.write(0.5);
singularity 0:6004a7230f87 45 xbee.printf("M1 = 0.5\n");
singularity 0:6004a7230f87 46 wait(10.0f);
singularity 0:6004a7230f87 47 this->m1.write(0);
singularity 0:6004a7230f87 48 this->m2.write(0.5);
singularity 0:6004a7230f87 49 xbee.printf("M2 = 0.5\n");
singularity 0:6004a7230f87 50 wait(10.0f);
singularity 0:6004a7230f87 51 this->m2.write(0);
singularity 0:6004a7230f87 52 xbee.printf("Fin Test Propulsion\n");
singularity 0:6004a7230f87 53 }
singularity 0:6004a7230f87 54
singularity 0:6004a7230f87 55 void Moteur::test2()
singularity 0:6004a7230f87 56 {
singularity 0:6004a7230f87 57 //RAZ DES SORTIES
singularity 0:6004a7230f87 58 this->m1.write(0.0f);
singularity 0:6004a7230f87 59 this->m2.write(0.0f);
singularity 0:6004a7230f87 60
singularity 0:6004a7230f87 61 //DEBUT SEQUENCE DE TEST
singularity 0:6004a7230f87 62 xbee.printf("Debut Test Propulsion\n");
singularity 0:6004a7230f87 63 this->m1.write(0.5);
singularity 0:6004a7230f87 64 xbee.printf("M1 = 0.5\n");
singularity 0:6004a7230f87 65 wait(10.0f);
singularity 0:6004a7230f87 66 this->m1.write(0);
singularity 0:6004a7230f87 67 this->m2.write(0.5);
singularity 0:6004a7230f87 68 xbee.printf("M2 = 0.5\n");
singularity 0:6004a7230f87 69 wait(10.0f);
singularity 0:6004a7230f87 70 this->m2.write(0);
singularity 0:6004a7230f87 71 xbee.printf("Fin Test Propulsion\n");
singularity 0:6004a7230f87 72 }
singularity 0:6004a7230f87 73
singularity 0:6004a7230f87 74
singularity 0:6004a7230f87 75 void Moteur::etalonnage() {
singularity 0:6004a7230f87 76 xbee.printf("ETALONNAGE\n");
singularity 0:6004a7230f87 77 for(int i=0;i<256;i++) {
singularity 0:6004a7230f87 78 this->m1.write((float)i/256.0f);
singularity 0:6004a7230f87 79 xbee.printf("D1 : %d\n",i);
singularity 0:6004a7230f87 80 wait(0.1f);
singularity 0:6004a7230f87 81 }
singularity 0:6004a7230f87 82 }
singularity 0:6004a7230f87 83
singularity 0:6004a7230f87 84
singularity 0:6004a7230f87 85 void Moteur::asserv()
singularity 0:6004a7230f87 86 {
singularity 0:6004a7230f87 87 //Application de la nouvelle commande
singularity 0:6004a7230f87 88 this->setPuissance(regulateur.getCommande(this->erreur));
singularity 0:6004a7230f87 89 }
singularity 0:6004a7230f87 90
singularity 0:6004a7230f87 91 void Moteur::setKp(float k)
singularity 0:6004a7230f87 92 {
singularity 0:6004a7230f87 93 this->regulateur.setKp(k);
singularity 0:6004a7230f87 94 }
singularity 0:6004a7230f87 95 void Moteur::setKi(float k)
singularity 0:6004a7230f87 96 {
singularity 0:6004a7230f87 97 this->regulateur.setKp(k);
singularity 0:6004a7230f87 98 }
singularity 0:6004a7230f87 99 void Moteur::setKd(float k)
singularity 0:6004a7230f87 100 {
singularity 0:6004a7230f87 101 this->regulateur.setKp(k);
singularity 0:6004a7230f87 102 }
singularity 0:6004a7230f87 103
singularity 0:6004a7230f87 104
singularity 0:6004a7230f87 105 void Moteur::interruptCapteurRise()
singularity 0:6004a7230f87 106 {
singularity 0:6004a7230f87 107 this->mesure();
singularity 0:6004a7230f87 108 this->majErreur();
singularity 0:6004a7230f87 109 this->asserv();
singularity 0:6004a7230f87 110 }
singularity 0:6004a7230f87 111
singularity 0:6004a7230f87 112 void Moteur::interruptCapteurFall()
singularity 0:6004a7230f87 113 {
singularity 0:6004a7230f87 114 this->mesure();
singularity 0:6004a7230f87 115 this->majErreur();
singularity 0:6004a7230f87 116 this->asserv();
singularity 0:6004a7230f87 117 }
singularity 0:6004a7230f87 118
singularity 0:6004a7230f87 119 void Moteur::mesure()
singularity 0:6004a7230f87 120 {
singularity 0:6004a7230f87 121 this->derniere_mesure = 1/this->tmr.read();
singularity 0:6004a7230f87 122 if(this->capteur2) {
singularity 0:6004a7230f87 123 this->derniere_mesure = -this->derniere_mesure;
singularity 0:6004a7230f87 124 }
singularity 0:6004a7230f87 125 this->tmr.reset();
singularity 0:6004a7230f87 126 }
singularity 0:6004a7230f87 127
singularity 0:6004a7230f87 128 void Moteur::majErreur()
singularity 0:6004a7230f87 129 {
singularity 0:6004a7230f87 130 float e_prec = this->erreur.p;
singularity 0:6004a7230f87 131
singularity 0:6004a7230f87 132 this->erreur.p = this->derniere_mesure - this->vitesse_consigne;
singularity 0:6004a7230f87 133 this->erreur.i += this->erreur.p;
singularity 0:6004a7230f87 134 this->erreur.d = this->erreur.p - e_prec;
singularity 0:6004a7230f87 135 }
singularity 0:6004a7230f87 136
singularity 0:6004a7230f87 137 float Moteur::getMesure()
singularity 0:6004a7230f87 138 {
singularity 0:6004a7230f87 139 return this->derniere_mesure;
singularity 0:6004a7230f87 140 }
singularity 0:6004a7230f87 141
singularity 0:6004a7230f87 142 void Moteur::setVitesse(float v)
singularity 0:6004a7230f87 143 {
singularity 0:6004a7230f87 144 this->vitesse_consigne = v;
singularity 0:6004a7230f87 145 }