ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp@108:890094ee202a, 2015-05-05 (annotated)

Committer:
sype
Date:
Tue May 05 16:35:42 2015 +0000
Revision:
108:890094ee202a
Parent:
106:05096985d1b2
Child:
112:df5388d9f706
Asserv B, 6/10

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