ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp@112:df5388d9f706, 2015-05-05 (annotated)

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