ARES / Mbed 2 deprecated Timer

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

Asserv_Plan_B/planB.cpp@13:5355aed288b0, 2016-01-05 (annotated)

Committer:
IceTeam
Date:
Tue Jan 05 17:08:15 2016 +0000
Revision:
13:5355aed288b0
Ajout du d?but de l'IA;

Who changed what in which revision?

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