ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp@85:8e95432d99d3, 2015-04-18 (annotated)

Committer:
sype
Date:
Sat Apr 18 09:39:15 2015 +0000
Revision:
85:8e95432d99d3
Parent:
84:24d727006218
Child:
86:2fbe5db2627f
Mise en place de l'asserv de l'ETAT 2

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 85:8e95432d99d3 9 Kp_angle = 1.2; //Fixed à 0.436 pour 180 deg
sype 85:8e95432d99d3 10 Kd_angle = 4.455;
sype 79:d97090bb6470 11 cmd = 0;
sype 85:8e95432d99d3 12 cmd_g = 0, cmd_d = 0;
sype 79:d97090bb6470 13 N = 0;
sype 79:d97090bb6470 14 done = false;
sype 79:d97090bb6470 15 state = 0; // Etat ou l'on ne fait rien
sype 85:8e95432d99d3 16 distance_g = 0;
sype 85:8e95432d99d3 17 distance_d = 0;
sype 85:8e95432d99d3 18 Kp_distance = 0.0075;
sype 85:8e95432d99d3 19 Ki_distance = 0;
sype 85:8e95432d99d3 20 Kd_distance = 0;
Jagang 71:95d76c181b22 21 }
Jagang 71:95d76c181b22 22
Jagang 71:95d76c181b22 23 void aserv_planB::setGoal(float x, float y, float theta)
Jagang 71:95d76c181b22 24 {
Jagang 71:95d76c181b22 25 m_goalX = x;
Jagang 71:95d76c181b22 26 m_goalY = y;
Jagang 71:95d76c181b22 27 m_goalTheta = theta;
sype 85:8e95432d99d3 28 thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
sype 85:8e95432d99d3 29 distance_g = sqrt(carre(m_goalY-m_odometry.getY())+carre(m_goalX-m_odometry.getX()));
sype 85:8e95432d99d3 30 distance_d = distance_g;
sype 85:8e95432d99d3 31 state = 2; // Etat de rotation 1
sype 64:6489bcfc1173 32 }
sype 64:6489bcfc1173 33
sype 85:8e95432d99d3 34 /*void aserv_planB::control_speed()
sype 64:6489bcfc1173 35 {
Jagang 72:b2a128486332 36 vitesse_d = m_odometry.getVitRight();
Jagang 72:b2a128486332 37 vitesse_g = m_odometry.getVitLeft();
sype 79:d97090bb6470 38
sype 64:6489bcfc1173 39 erreur_g = consigne_g - vitesse_g;
sype 64:6489bcfc1173 40 cmd_g = erreur_g*Kp;
sype 64:6489bcfc1173 41 erreur_d = consigne_d - vitesse_d;
sype 64:6489bcfc1173 42 cmd_d = erreur_d*Kp;
sype 79:d97090bb6470 43
sype 64:6489bcfc1173 44 m_motorL.setSpeed(cmd_g);
sype 64:6489bcfc1173 45 m_motorR.setSpeed(cmd_d);
sype 85:8e95432d99d3 46 }*/
Jagang 62:454cd844fe1e 47
Jagang 62:454cd844fe1e 48 void aserv_planB::update(float dt)
Jagang 62:454cd844fe1e 49 {
sype 84:24d727006218 50 // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
sype 84:24d727006218 51 if(state == 1 && N < 100)
sype 85:8e95432d99d3 52 {
sype 85:8e95432d99d3 53 float erreur_theta = thetaGoal-m_odometry.getTheta();
sype 84:24d727006218 54
Jagang 72:b2a128486332 55 if(erreur_theta <= PI) erreur_theta += 2.0*PI;
Jagang 72:b2a128486332 56 if(erreur_theta >= PI) erreur_theta -= 2.0*PI;
sype 84:24d727006218 57
sype 85:8e95432d99d3 58 //logger.printf("%.2f\r\n", erreur_theta*180/PI);
Jagang 72:b2a128486332 59
sype 85:8e95432d99d3 60 cmd = erreur_theta*Kp_angle + (erreur_theta-erreur_precedente)*Kd_angle;
sype 79:d97090bb6470 61 erreur_precedente = erreur_theta;
Jagang 72:b2a128486332 62
sype 79:d97090bb6470 63 m_motorL.setSpeed(-cmd);
sype 79:d97090bb6470 64 m_motorR.setSpeed(cmd);
sype 79:d97090bb6470 65
sype 84:24d727006218 66 N++;
sype 85:8e95432d99d3 67 if(N==100) // && (abs(erreur_theta)<=2.0)
sype 85:8e95432d99d3 68 {
sype 85:8e95432d99d3 69 state = 2;
sype 85:8e95432d99d3 70 logger.printf("%.2f %.2f\r\n", erreur_theta*180/PI, thetaGoal*180/PI);
sype 85:8e95432d99d3 71 m_odometry.setDistLeft(0);
sype 85:8e95432d99d3 72 m_odometry.setDistRight(0);
sype 85:8e95432d99d3 73 memo_g = m_odometry.getDistLeft();
sype 85:8e95432d99d3 74 memo_d = m_odometry.getDistRight();
sype 85:8e95432d99d3 75 }
sype 79:d97090bb6470 76 }
sype 85:8e95432d99d3 77
sype 85:8e95432d99d3 78 // Etat 2 : Parcours du robot jusqu'au point M(x,y)
sype 85:8e95432d99d3 79 if(state == 2 && N < 100)
sype 79:d97090bb6470 80 {
sype 85:8e95432d99d3 81 float erreur_distance_g = distance_g-(m_odometry.getDistLeft()-memo_g); //- distance parcourue par la roue gauche depuis l'état 2
sype 85:8e95432d99d3 82 float erreur_distance_d = distance_d-(m_odometry.getDistRight()-memo_d);
sype 85:8e95432d99d3 83 cmd_g = erreur_distance_g*Kp_distance;
sype 85:8e95432d99d3 84 cmd_d = erreur_distance_d*Kp_distance;
sype 84:24d727006218 85
sype 85:8e95432d99d3 86 m_motorL.setSpeed(0);
sype 85:8e95432d99d3 87 m_motorR.setSpeed(0);
sype 85:8e95432d99d3 88
sype 85:8e95432d99d3 89 logger.printf("%.2f %.2f\r\n", m_odometry.getDistLeft(), m_odometry.getDistRight());
sype 85:8e95432d99d3 90
sype 85:8e95432d99d3 91 //N++;
sype 85:8e95432d99d3 92 if(N==100) state = 2;
sype 85:8e95432d99d3 93 }
sype 85:8e95432d99d3 94
sype 85:8e95432d99d3 95 // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
sype 85:8e95432d99d3 96 if(state == 3)
sype 85:8e95432d99d3 97 {
sype 85:8e95432d99d3 98 m_motorL.setSpeed(0);
sype 85:8e95432d99d3 99 m_motorR.setSpeed(0);
sype 84:24d727006218 100 }
sype 70:56086a37f31f 101 }