ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp@116:73d7d87e0299, 2015-05-06 (annotated)

Committer:
sype
Date:
Wed May 06 11:14:02 2015 +0000
Revision:
116:73d7d87e0299
Parent:
112:df5388d9f706
Child:
119:c45efcd706d9
A 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 116:73d7d87e0299 8 limite = 0.65;
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
sype 116:73d7d87e0299 27 Kp_distance = 0.0042;
sype 116:73d7d87e0299 28 Ki_distance = 0.00000;//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
sype 108:890094ee202a 95 if(abs(erreur_theta)< 0.05) 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 {
sype 116:73d7d87e0299 112 //logger.printf("%.2f %.2f %.2f\r\n", erreur_distance, cmd_g, cmd_d); //+ somme_erreur_distance*Ki_distance
sype 116:73d7d87e0299 113 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 114 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 115
sype 112:df5388d9f706 116 if(cmd_g > limite) cmd_g = limite;
sype 112:df5388d9f706 117 else if(cmd_g < -limite) cmd_g = -limite;
sype 108:890094ee202a 118
sype 112:df5388d9f706 119 if(cmd_d > limite) cmd_d = limite;
sype 112:df5388d9f706 120 else if(cmd_d < -limite) cmd_d = -limite;
sype 108:890094ee202a 121
sype 106:05096985d1b2 122 m_motorL.setSpeed(cmd_g);
sype 106:05096985d1b2 123 m_motorR.setSpeed(cmd_d);
sype 106:05096985d1b2 124
sype 112:df5388d9f706 125 if(abs(erreur_distance) < 20.0) N++;
sype 108:890094ee202a 126 else N = 0;
sype 116:73d7d87e0299 127 if(N > 10)
sype 108:890094ee202a 128 {
sype 108:890094ee202a 129 logger.printf("Erreur distance : %.2f\r\n", erreur_distance);
sype 116:73d7d87e0299 130 somme_erreur_distance = 0;
sype 108:890094ee202a 131 state = 3;
sype 108:890094ee202a 132 N = 0;
sype 116:73d7d87e0299 133 Kp_angle = 3.5;
sype 112:df5388d9f706 134 }
sype 106:05096985d1b2 135 }
sype 108:890094ee202a 136
sype 106:05096985d1b2 137 // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
sype 116:73d7d87e0299 138 if(state == 3 && squip == false)
sype 106:05096985d1b2 139 {
sype 116:73d7d87e0299 140 /*cmd = erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
sype 116:73d7d87e0299 141
sype 116:73d7d87e0299 142 if(cmd > limite) cmd = limite;
sype 116:73d7d87e0299 143 else if(cmd < -limite) cmd = -limite;
sype 116:73d7d87e0299 144
sype 116:73d7d87e0299 145 m_motorL.setSpeed(-cmd);
sype 116:73d7d87e0299 146 m_motorR.setSpeed(cmd);
sype 116:73d7d87e0299 147
sype 116:73d7d87e0299 148 if(abs(erreur_theta)< 0.05) N++;
sype 116:73d7d87e0299 149 else N = 0;
sype 116:73d7d87e0299 150 if(N > 10)
sype 116:73d7d87e0299 151 {
sype 116:73d7d87e0299 152 m_motorL.setSpeed(0);
sype 116:73d7d87e0299 153 m_motorR.setSpeed(0);
sype 116:73d7d87e0299 154 logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
sype 116:73d7d87e0299 155 somme_erreur_theta = 0;
sype 116:73d7d87e0299 156 N = 0;
sype 116:73d7d87e0299 157 Kp_angle = 3.0;
sype 116:73d7d87e0299 158 arrived = true;
sype 116:73d7d87e0299 159 }*/
sype 106:05096985d1b2 160 m_motorL.setSpeed(0);
sype 106:05096985d1b2 161 m_motorR.setSpeed(0);
sype 108:890094ee202a 162 arrived = true;
sype 106:05096985d1b2 163 }
sype 106:05096985d1b2 164 }