ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp@94:5c37bcf73d14, 2015-04-30 (annotated)

Committer:
Near32
Date:
Thu Apr 30 16:03:55 2015 +0000
Revision:
94:5c37bcf73d14
Parent:
86:2fbe5db2627f
Child:
100:a827a645d6c2
maj asservA via local

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
Near32 94:5c37bcf73d14 4
Jagang 71:95d76c181b22 5 extern Serial logger;
Jagang 71:95d76c181b22 6
Jagang 71:95d76c181b22 7 aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
Jagang 62:454cd844fe1e 8 {
Near32 94:5c37bcf73d14 9 consigne_g = 0.0;
Near32 94:5c37bcf73d14 10 consigne_d = 0.0;
Near32 94:5c37bcf73d14 11 vitesse_g = 0;
Near32 94:5c37bcf73d14 12 vitesse_d = 0;
Near32 94:5c37bcf73d14 13 erreur_g = 0;
Near32 94:5c37bcf73d14 14 erreur_d = 0;
Near32 94:5c37bcf73d14 15 cmd_g = 0;
Near32 94:5c37bcf73d14 16 cmd_d = 0;
Near32 94:5c37bcf73d14 17 somme_erreur = 0;
Near32 94:5c37bcf73d14 18 somme_erreur_d = 0;
Near32 94:5c37bcf73d14 19 delta_erreur = 0;
sype 79:d97090bb6470 20 erreur_precedente = 0;
Near32 94:5c37bcf73d14 21 Kp = 0.30;
Near32 94:5c37bcf73d14 22 Ki = 0.01;
Near32 94:5c37bcf73d14 23 Kd = 0.5;
Near32 94:5c37bcf73d14 24 //etat_angle = 0;
sype 79:d97090bb6470 25 cmd = 0;
sype 79:d97090bb6470 26 N = 0;
Near32 94:5c37bcf73d14 27 moyenne = 0;
Near32 94:5c37bcf73d14 28 limite = 0;
sype 79:d97090bb6470 29 done = false;
sype 79:d97090bb6470 30 state = 0; // Etat ou l'on ne fait rien
Jagang 71:95d76c181b22 31 }
Jagang 71:95d76c181b22 32
Jagang 71:95d76c181b22 33 void aserv_planB::setGoal(float x, float y, float theta)
Jagang 71:95d76c181b22 34 {
Jagang 71:95d76c181b22 35 m_goalX = x;
Jagang 71:95d76c181b22 36 m_goalY = y;
Jagang 71:95d76c181b22 37 m_goalTheta = theta;
Near32 94:5c37bcf73d14 38
Near32 94:5c37bcf73d14 39 state = 1; // Etat de rotation 1
sype 64:6489bcfc1173 40 }
sype 64:6489bcfc1173 41
Near32 94:5c37bcf73d14 42 void aserv_planB::control_speed()
sype 64:6489bcfc1173 43 {
Jagang 72:b2a128486332 44 vitesse_d = m_odometry.getVitRight();
Jagang 72:b2a128486332 45 vitesse_g = m_odometry.getVitLeft();
sype 79:d97090bb6470 46
sype 64:6489bcfc1173 47 erreur_g = consigne_g - vitesse_g;
sype 64:6489bcfc1173 48 cmd_g = erreur_g*Kp;
sype 64:6489bcfc1173 49 erreur_d = consigne_d - vitesse_d;
sype 64:6489bcfc1173 50 cmd_d = erreur_d*Kp;
sype 79:d97090bb6470 51
sype 64:6489bcfc1173 52 m_motorL.setSpeed(cmd_g);
sype 64:6489bcfc1173 53 m_motorR.setSpeed(cmd_d);
Near32 94:5c37bcf73d14 54 }
Jagang 62:454cd844fe1e 55
Jagang 62:454cd844fe1e 56 void aserv_planB::update(float dt)
Jagang 62:454cd844fe1e 57 {
Near32 94:5c37bcf73d14 58 if(state == 1)
Near32 94:5c37bcf73d14 59 {
Near32 94:5c37bcf73d14 60 float thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
Near32 94:5c37bcf73d14 61
sype 85:8e95432d99d3 62 float erreur_theta = thetaGoal-m_odometry.getTheta();
Jagang 72:b2a128486332 63 if(erreur_theta <= PI) erreur_theta += 2.0*PI;
Jagang 72:b2a128486332 64 if(erreur_theta >= PI) erreur_theta -= 2.0*PI;
Near32 94:5c37bcf73d14 65
Near32 94:5c37bcf73d14 66 logger.printf("%.2f\r\n",erreur_theta*180/PI);
sype 84:24d727006218 67
Near32 94:5c37bcf73d14 68 /*if(erreur_theta < 0) etat_angle = 1;
Near32 94:5c37bcf73d14 69 else if(erreur_theta > 0) etat_angle = 2;
Near32 94:5c37bcf73d14 70 else etat_angle = 0;*/
Near32 94:5c37bcf73d14 71 /*limite = (0.5-Kp*erreur_theta)/Ki;
Near32 94:5c37bcf73d14 72 if(somme_erreur >= limite) somme_erreur = limite;
Near32 94:5c37bcf73d14 73 if(somme_erreur <= -limite) somme_erreur = -limite;*/
Jagang 72:b2a128486332 74
Near32 94:5c37bcf73d14 75 cmd = erreur_theta*Kp + somme_erreur*Ki - delta_erreur*Kd;
Near32 94:5c37bcf73d14 76 somme_erreur += erreur_theta;
Near32 94:5c37bcf73d14 77 delta_erreur = erreur_theta - erreur_precedente;
sype 79:d97090bb6470 78 erreur_precedente = erreur_theta;
Jagang 72:b2a128486332 79
sype 79:d97090bb6470 80 m_motorL.setSpeed(-cmd);
sype 79:d97090bb6470 81 m_motorR.setSpeed(cmd);
sype 79:d97090bb6470 82
Near32 94:5c37bcf73d14 83 //! Pas bon coeff, mais c'est l'idée
Near32 94:5c37bcf73d14 84 consigne_g = 0;//-erreur_theta*0.0001;
Near32 94:5c37bcf73d14 85 consigne_d = 0;//erreur_theta*0.0001;
Near32 94:5c37bcf73d14 86
Near32 94:5c37bcf73d14 87 /*if(erreur_theta <= abs(0.7))
Near32 94:5c37bcf73d14 88 {
Near32 94:5c37bcf73d14 89 done = true;
Near32 94:5c37bcf73d14 90 logger.printf("Posey\r\n");
sype 85:8e95432d99d3 91 state = 2;
Near32 94:5c37bcf73d14 92 }*/
sype 79:d97090bb6470 93 }
Near32 94:5c37bcf73d14 94 /*switch(etat_angle)
sype 79:d97090bb6470 95 {
Near32 94:5c37bcf73d14 96 case 0:
Near32 94:5c37bcf73d14 97
Near32 94:5c37bcf73d14 98 break;
Near32 94:5c37bcf73d14 99
Near32 94:5c37bcf73d14 100 case 1:
Near32 94:5c37bcf73d14 101
Near32 94:5c37bcf73d14 102 break;*/
sype 70:56086a37f31f 103 }