ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp@84:24d727006218, 2015-04-16 (annotated)

Committer:
sype
Date:
Thu Apr 16 12:23:02 2015 +0000
Revision:
84:24d727006218
Parent:
79:d97090bb6470
Child:
85:8e95432d99d3
Suppression de l'integrateur AsservB -> uniquement P et D

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 62:454cd844fe1e 1 #include "planB.h"
Jagang 71:95d76c181b22 2 #include "defines.h"
Jagang 71:95d76c181b22 3
Jagang 71:95d76c181b22 4 extern Serial logger;
Jagang 71:95d76c181b22 5
Jagang 71:95d76c181b22 6 aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
Jagang 62:454cd844fe1e 7 {
sype 79:d97090bb6470 8 erreur_precedente = 0;
sype 84:24d727006218 9 Kp = 0.436; //Fixed à 0.436
sype 84:24d727006218 10 Ki = 0.0; //Limite a 0.03
sype 84:24d727006218 11 Kd = 0.0;
sype 79:d97090bb6470 12 cmd = 0;
sype 79:d97090bb6470 13 N = 0;
sype 79:d97090bb6470 14 moyenne = 0;
sype 79:d97090bb6470 15 limite = 0;
sype 79:d97090bb6470 16 done = false;
sype 79:d97090bb6470 17 state = 0; // Etat ou l'on ne fait rien
Jagang 71:95d76c181b22 18 }
Jagang 71:95d76c181b22 19
Jagang 71:95d76c181b22 20 void aserv_planB::setGoal(float x, float y, float theta)
Jagang 71:95d76c181b22 21 {
Jagang 71:95d76c181b22 22 m_goalX = x;
Jagang 71:95d76c181b22 23 m_goalY = y;
Jagang 71:95d76c181b22 24 m_goalTheta = theta;
sype 79:d97090bb6470 25
Jagang 71:95d76c181b22 26 state = 1; // Etat de rotation 1
sype 64:6489bcfc1173 27 }
sype 64:6489bcfc1173 28
sype 64:6489bcfc1173 29 void aserv_planB::control_speed()
sype 64:6489bcfc1173 30 {
Jagang 72:b2a128486332 31 vitesse_d = m_odometry.getVitRight();
Jagang 72:b2a128486332 32 vitesse_g = m_odometry.getVitLeft();
sype 79:d97090bb6470 33
sype 64:6489bcfc1173 34 erreur_g = consigne_g - vitesse_g;
sype 64:6489bcfc1173 35 cmd_g = erreur_g*Kp;
sype 64:6489bcfc1173 36 erreur_d = consigne_d - vitesse_d;
sype 64:6489bcfc1173 37 cmd_d = erreur_d*Kp;
sype 79:d97090bb6470 38
sype 64:6489bcfc1173 39 m_motorL.setSpeed(cmd_g);
sype 64:6489bcfc1173 40 m_motorR.setSpeed(cmd_d);
Jagang 62:454cd844fe1e 41 }
Jagang 62:454cd844fe1e 42
Jagang 62:454cd844fe1e 43 void aserv_planB::update(float dt)
Jagang 62:454cd844fe1e 44 {
sype 84:24d727006218 45 // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
sype 84:24d727006218 46 if(state == 1 && N < 100)
Jagang 71:95d76c181b22 47 {
Jagang 71:95d76c181b22 48 float thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
sype 84:24d727006218 49
Jagang 71:95d76c181b22 50 float erreur_theta = thetaGoal-m_odometry.getTheta();
Jagang 72:b2a128486332 51 if(erreur_theta <= PI) erreur_theta += 2.0*PI;
Jagang 72:b2a128486332 52 if(erreur_theta >= PI) erreur_theta -= 2.0*PI;
sype 84:24d727006218 53
sype 79:d97090bb6470 54 logger.printf("%.2f\r\n",erreur_theta*180/PI);
Jagang 72:b2a128486332 55
sype 84:24d727006218 56 cmd = erreur_theta*Kp + (erreur_theta-erreur_precedente)*Kd;
sype 79:d97090bb6470 57 erreur_precedente = erreur_theta;
Jagang 72:b2a128486332 58
sype 79:d97090bb6470 59 m_motorL.setSpeed(-cmd);
sype 79:d97090bb6470 60 m_motorR.setSpeed(cmd);
sype 79:d97090bb6470 61
sype 84:24d727006218 62 N++;
sype 84:24d727006218 63 if((N==100) && (abs(erreur_theta)<=1.0)) logger.printf("%.2f\r\n",abs(erreur_theta)*180/PI);
sype 79:d97090bb6470 64 }
sype 84:24d727006218 65 if(state == 2)
sype 79:d97090bb6470 66 {
sype 84:24d727006218 67
sype 84:24d727006218 68 }
sype 70:56086a37f31f 69 }