ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp@83:6bcc38b1c5b5, 2015-04-14 (annotated)

Committer:
sype
Date:
Tue Apr 14 19:25:00 2015 +0000
Revision:
83:6bcc38b1c5b5
Parent:
79:d97090bb6470 
pid angle

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