ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp@106:05096985d1b2, 2015-05-05 (annotated)

Committer:
sype
Date:
Tue May 05 05:05:07 2015 +0000
Revision:
106:05096985d1b2
Parent:
93:4d5664e9188a
Child:
108:890094ee202a
Asserv B

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