ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp@119:c45efcd706d9, 2015-05-06 (annotated)

Committer:
Jagang
Date:
Wed May 06 15:17:16 2015 +0000
Revision:
119:c45efcd706d9
Parent:
116:73d7d87e0299
Child:
121:0cc17ba879cb
Modif asserv B; Impl?mentation de l'IA et des objectifs (Ramasser PC et d?poser)

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 {
Jagang 119:c45efcd706d9 8 limite = 0.5;
sype 79:d97090bb6470 9 cmd = 0;
sype 116:73d7d87e0299 10 cmd_g = 0;
sype 116:73d7d87e0299 11 cmd_d = 0;
sype 116:73d7d87e0299 12
sype 116:73d7d87e0299 13 somme_erreur_theta = 0;
sype 116:73d7d87e0299 14 delta_erreur_theta = 0;
sype 116:73d7d87e0299 15 erreur_precedente_theta = 0;
sype 116:73d7d87e0299 16
sype 116:73d7d87e0299 17 somme_erreur_distance = 0;
sype 116:73d7d87e0299 18 delta_erreur_distance = 0;
sype 116:73d7d87e0299 19 erreur_precedente_distance = 0;
sype 116:73d7d87e0299 20 distanceGoal = 0;
sype 116:73d7d87e0299 21 distance = 0;
sype 116:73d7d87e0299 22
sype 116:73d7d87e0299 23 Kp_angle = 3.5; //Fixed à 3.0 pour 180 deg
sype 116:73d7d87e0299 24 Ki_angle = 0.0;
sype 116:73d7d87e0299 25 Kd_angle = 0.1;
sype 116:73d7d87e0299 26
Jagang 119:c45efcd706d9 27 Kp_distance = 0.0040;
Jagang 119:c45efcd706d9 28 Ki_distance = 0.00003;//0.000001
sype 116:73d7d87e0299 29 Kd_distance = 0.0;//0.05
sype 116:73d7d87e0299 30
sype 79:d97090bb6470 31 N = 0;
sype 108:890094ee202a 32 arrived = false;
sype 106:05096985d1b2 33 squip = false;
sype 79:d97090bb6470 34 state = 0; // Etat ou l'on ne fait rien
Jagang 71:95d76c181b22 35 }
Jagang 71:95d76c181b22 36
sype 106:05096985d1b2 37 void aserv_planB::setGoal(float x, float y, float phi)
Jagang 71:95d76c181b22 38 {
Jagang 71:95d76c181b22 39 m_goalX = x;
Jagang 71:95d76c181b22 40 m_goalY = y;
sype 106:05096985d1b2 41 m_goalPhi = phi;
sype 108:890094ee202a 42 distanceGoal = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
sype 108:890094ee202a 43 state = 1; // Etat de rotation 1
sype 106:05096985d1b2 44 N = 0;
sype 108:890094ee202a 45 arrived = false;
sype 108:890094ee202a 46 }
sype 108:890094ee202a 47
sype 108:890094ee202a 48 void aserv_planB::stop(void)
sype 108:890094ee202a 49 {
sype 108:890094ee202a 50 m_motorL.setSpeed(0);
sype 108:890094ee202a 51 m_motorR.setSpeed(0);
sype 116:73d7d87e0299 52 /*m_goalX = m_odometry.getX();
sype 108:890094ee202a 53 m_goalY = m_odometry.getY();
sype 116:73d7d87e0299 54 setGoal(m_goalX, m_goalY);*/
sype 106:05096985d1b2 55 }
sype 106:05096985d1b2 56
sype 106:05096985d1b2 57 void aserv_planB::setGoal(float x, float y)
sype 106:05096985d1b2 58 {
sype 106:05096985d1b2 59 squip = true;
sype 106:05096985d1b2 60 setGoal(x, y, 0);
sype 106:05096985d1b2 61 squip = false;
sype 64:6489bcfc1173 62 }
sype 64:6489bcfc1173 63
sype 106:05096985d1b2 64 void aserv_planB::update(float dt)
sype 108:890094ee202a 65 {
sype 116:73d7d87e0299 66 if(state == 1) thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
sype 116:73d7d87e0299 67 else if(state == 3) thetaGoal = m_goalPhi;
sype 106:05096985d1b2 68 float erreur_theta = thetaGoal-m_odometry.getTheta();
sype 116:73d7d87e0299 69
sype 108:890094ee202a 70 float erreur_distance = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
sype 116:73d7d87e0299 71
sype 116:73d7d87e0299 72 delta_erreur_theta = erreur_theta - erreur_precedente_theta;
sype 116:73d7d87e0299 73 erreur_precedente_theta = erreur_theta;
sype 116:73d7d87e0299 74 somme_erreur_theta += erreur_theta;
sype 116:73d7d87e0299 75
sype 116:73d7d87e0299 76 delta_erreur_distance = erreur_distance - erreur_precedente_distance;
sype 116:73d7d87e0299 77 erreur_precedente_distance = erreur_distance;
sype 116:73d7d87e0299 78 somme_erreur_distance += erreur_distance;
sype 116:73d7d87e0299 79
sype 106:05096985d1b2 80 if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
sype 106:05096985d1b2 81 if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
sype 108:890094ee202a 82
sype 106:05096985d1b2 83 // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
sype 108:890094ee202a 84 if(state == 1)
sype 108:890094ee202a 85 {
sype 106:05096985d1b2 86 //logger.printf("%.2f\r\n", erreur_theta*180/PI);
sype 116:73d7d87e0299 87 cmd = erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
sype 106:05096985d1b2 88
sype 112:df5388d9f706 89 if(cmd > limite) cmd = limite;
sype 112:df5388d9f706 90 else if(cmd < -limite) cmd = -limite;
sype 108:890094ee202a 91
sype 106:05096985d1b2 92 m_motorL.setSpeed(-cmd);
sype 106:05096985d1b2 93 m_motorR.setSpeed(cmd);
sype 106:05096985d1b2 94
Jagang 119:c45efcd706d9 95 if(abs(erreur_theta) < 0.05f) N++;
sype 108:890094ee202a 96 else N = 0;
sype 108:890094ee202a 97 if(N > 10)
sype 108:890094ee202a 98 {
sype 108:890094ee202a 99 m_motorL.setSpeed(0);
sype 108:890094ee202a 100 m_motorR.setSpeed(0);
sype 106:05096985d1b2 101 state = 2;
sype 106:05096985d1b2 102 logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
sype 116:73d7d87e0299 103 somme_erreur_theta = 0;
sype 106:05096985d1b2 104 N = 0;
sype 116:73d7d87e0299 105 Kp_angle = 3.5;
sype 106:05096985d1b2 106 }
sype 106:05096985d1b2 107 }
sype 108:890094ee202a 108
sype 106:05096985d1b2 109 // Etat 2 : Parcours du robot jusqu'au point M(x,y)
sype 108:890094ee202a 110 if(state == 2)
sype 108:890094ee202a 111 {
Jagang 119:c45efcd706d9 112 if(delta_erreur_distance > 0) // On a dépassé le point
Jagang 119:c45efcd706d9 113 {
Jagang 119:c45efcd706d9 114 state = 1;
Jagang 119:c45efcd706d9 115 return;
Jagang 119:c45efcd706d9 116 }
Jagang 119:c45efcd706d9 117
Jagang 119:c45efcd706d9 118 if(abs(erreur_distance) > 55.0f) somme_erreur_distance = 0;
Jagang 119:c45efcd706d9 119
sype 116:73d7d87e0299 120 cmd_g = erreur_distance*Kp_distance + somme_erreur_distance*Ki_distance + delta_erreur_distance*Kd_distance - (erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle);
sype 116:73d7d87e0299 121 cmd_d = erreur_distance*Kp_distance + somme_erreur_distance*Ki_distance + delta_erreur_distance*Kd_distance + erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
sype 106:05096985d1b2 122
sype 108:890094ee202a 123
Jagang 119:c45efcd706d9 124 if(abs(erreur_distance) > 55.0f)
Jagang 119:c45efcd706d9 125 {
Jagang 119:c45efcd706d9 126 if(cmd_g > limite) cmd_g = limite;
Jagang 119:c45efcd706d9 127 else if(cmd_g < -limite) cmd_g = -limite;
Jagang 119:c45efcd706d9 128
Jagang 119:c45efcd706d9 129 if(cmd_d > limite) cmd_d = limite;
Jagang 119:c45efcd706d9 130 else if(cmd_d < -limite) cmd_d = -limite;
Jagang 119:c45efcd706d9 131 }
Jagang 119:c45efcd706d9 132 else
Jagang 119:c45efcd706d9 133 {
Jagang 119:c45efcd706d9 134 if(cmd_g > 0.2f) cmd_g = 0.2f;
Jagang 119:c45efcd706d9 135 else if(cmd_g < -0.2f) cmd_g = -0.2f;
Jagang 119:c45efcd706d9 136
Jagang 119:c45efcd706d9 137 if(cmd_d > 0.2f) cmd_d = 0.2f;
Jagang 119:c45efcd706d9 138 else if(cmd_d < -0.2f) cmd_d = -0.2f;
Jagang 119:c45efcd706d9 139 }
Jagang 119:c45efcd706d9 140
Jagang 119:c45efcd706d9 141 if(cmd_g > 0.01f && cmd_g < 0.14f) cmd_g = 0.14f;
Jagang 119:c45efcd706d9 142 if(cmd_g < -0.01f && cmd_g > -0.14f) cmd_g = -0.14f;
Jagang 119:c45efcd706d9 143
Jagang 119:c45efcd706d9 144 if(cmd_d > 0.01f && cmd_d < 0.14f) cmd_d = 0.14f;
Jagang 119:c45efcd706d9 145 if(cmd_d < -0.01f && cmd_d > -0.14f) cmd_d = -0.14f;
Jagang 119:c45efcd706d9 146
sype 106:05096985d1b2 147 m_motorL.setSpeed(cmd_g);
sype 106:05096985d1b2 148 m_motorR.setSpeed(cmd_d);
sype 106:05096985d1b2 149
Jagang 119:c45efcd706d9 150 if(abs(erreur_distance) < 10.0f) N++;
sype 108:890094ee202a 151 else N = 0;
sype 116:73d7d87e0299 152 if(N > 10)
sype 108:890094ee202a 153 {
sype 108:890094ee202a 154 logger.printf("Erreur distance : %.2f\r\n", erreur_distance);
sype 116:73d7d87e0299 155 somme_erreur_distance = 0;
sype 108:890094ee202a 156 state = 3;
sype 108:890094ee202a 157 N = 0;
sype 116:73d7d87e0299 158 Kp_angle = 3.5;
sype 112:df5388d9f706 159 }
sype 106:05096985d1b2 160 }
sype 108:890094ee202a 161
sype 106:05096985d1b2 162 // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
sype 116:73d7d87e0299 163 if(state == 3 && squip == false)
sype 106:05096985d1b2 164 {
sype 116:73d7d87e0299 165 /*cmd = erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
sype 116:73d7d87e0299 166
sype 116:73d7d87e0299 167 if(cmd > limite) cmd = limite;
sype 116:73d7d87e0299 168 else if(cmd < -limite) cmd = -limite;
sype 116:73d7d87e0299 169
sype 116:73d7d87e0299 170 m_motorL.setSpeed(-cmd);
sype 116:73d7d87e0299 171 m_motorR.setSpeed(cmd);
sype 116:73d7d87e0299 172
sype 116:73d7d87e0299 173 if(abs(erreur_theta)< 0.05) N++;
sype 116:73d7d87e0299 174 else N = 0;
sype 116:73d7d87e0299 175 if(N > 10)
sype 116:73d7d87e0299 176 {
sype 116:73d7d87e0299 177 m_motorL.setSpeed(0);
sype 116:73d7d87e0299 178 m_motorR.setSpeed(0);
sype 116:73d7d87e0299 179 logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
sype 116:73d7d87e0299 180 somme_erreur_theta = 0;
sype 116:73d7d87e0299 181 N = 0;
sype 116:73d7d87e0299 182 Kp_angle = 3.0;
sype 116:73d7d87e0299 183 arrived = true;
sype 116:73d7d87e0299 184 }*/
sype 106:05096985d1b2 185 m_motorL.setSpeed(0);
sype 106:05096985d1b2 186 m_motorR.setSpeed(0);
sype 108:890094ee202a 187 arrived = true;
Jagang 119:c45efcd706d9 188 state = 0;
sype 106:05096985d1b2 189 }
sype 106:05096985d1b2 190 }