ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp@121:0cc17ba879cb, 2015-05-07 (annotated)

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