ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp@72:b2a128486332, 2015-04-10 (annotated)

Committer:
Jagang
Date:
Fri Apr 10 18:40:58 2015 +0000
Revision:
72:b2a128486332
Parent:
71:95d76c181b22
Child:
79:d97090bb6470
calcul d'erreur de theta asserv; Gros probl?me d'instabilit? en vitesse (consignes nulles)

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 62:454cd844fe1e 4
Jagang 71:95d76c181b22 5 extern Serial logger;
Jagang 71:95d76c181b22 6
Jagang 71:95d76c181b22 7 aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
Jagang 62:454cd844fe1e 8 {
Jagang 72:b2a128486332 9 consigne_g = 0.0;
Jagang 72:b2a128486332 10 consigne_d = 0.0;
sype 64:6489bcfc1173 11 vitesse_g = 0;
sype 64:6489bcfc1173 12 vitesse_d = 0;
sype 64:6489bcfc1173 13 erreur_g = 0;
sype 64:6489bcfc1173 14 erreur_d = 0;
sype 64:6489bcfc1173 15 cmd_g = 0;
sype 64:6489bcfc1173 16 cmd_d = 0;
sype 70:56086a37f31f 17 somme_erreur_g = 0;
sype 70:56086a37f31f 18 somme_erreur_d = 0;
sype 70:56086a37f31f 19 Kp = 0.02;
Jagang 71:95d76c181b22 20
Jagang 71:95d76c181b22 21
Jagang 71:95d76c181b22 22 state = 0; // Etat ou on fait rien
Jagang 71:95d76c181b22 23 }
Jagang 71:95d76c181b22 24
Jagang 71:95d76c181b22 25 void aserv_planB::setGoal(float x, float y, float theta)
Jagang 71:95d76c181b22 26 {
Jagang 71:95d76c181b22 27 m_goalX = x;
Jagang 71:95d76c181b22 28 m_goalY = y;
Jagang 71:95d76c181b22 29 m_goalTheta = theta;
Jagang 71:95d76c181b22 30
Jagang 71:95d76c181b22 31 state = 1; // Etat de rotation 1
sype 64:6489bcfc1173 32 }
sype 64:6489bcfc1173 33
sype 64:6489bcfc1173 34 void aserv_planB::control_speed()
sype 64:6489bcfc1173 35 {
Jagang 72:b2a128486332 36 vitesse_d = m_odometry.getVitRight();
Jagang 72:b2a128486332 37 vitesse_g = m_odometry.getVitLeft();
Jagang 62:454cd844fe1e 38
sype 64:6489bcfc1173 39 erreur_g = consigne_g - vitesse_g;
sype 64:6489bcfc1173 40 cmd_g = erreur_g*Kp;
sype 64:6489bcfc1173 41 erreur_d = consigne_d - vitesse_d;
sype 64:6489bcfc1173 42 cmd_d = erreur_d*Kp;
sype 64:6489bcfc1173 43
sype 64:6489bcfc1173 44 m_motorL.setSpeed(cmd_g);
sype 64:6489bcfc1173 45 m_motorR.setSpeed(cmd_d);
Jagang 62:454cd844fe1e 46 }
Jagang 62:454cd844fe1e 47
Jagang 62:454cd844fe1e 48 void aserv_planB::update(float dt)
Jagang 62:454cd844fe1e 49 {
Jagang 71:95d76c181b22 50 if(state == 1)
Jagang 71:95d76c181b22 51 {
Jagang 71:95d76c181b22 52 float thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
Jagang 71:95d76c181b22 53
Jagang 71:95d76c181b22 54 float erreur_theta = thetaGoal-m_odometry.getTheta();
Jagang 72:b2a128486332 55 if(erreur_theta <= PI) erreur_theta += 2.0*PI;
Jagang 72:b2a128486332 56 if(erreur_theta >= PI) erreur_theta -= 2.0*PI;
Jagang 72:b2a128486332 57
Jagang 72:b2a128486332 58 //logger.printf("%.2f %.2f %.2f\r\n",m_odometry.getTheta()*180/PI,thetaGoal*180/PI,erreur_theta*180/PI);
Jagang 72:b2a128486332 59
Jagang 72:b2a128486332 60
Jagang 72:b2a128486332 61 //! Pas bon coeff, mais c'est l'idée
Jagang 72:b2a128486332 62 consigne_g = 0;//-erreur_theta*0.0001;
Jagang 72:b2a128486332 63 consigne_d = 0;//erreur_theta*0.0001;
Jagang 71:95d76c181b22 64
Jagang 71:95d76c181b22 65 control_speed();
Jagang 71:95d76c181b22 66 }
sype 70:56086a37f31f 67 }
sype 70:56086a37f31f 68
sype 70:56086a37f31f 69 void aserv_planB::control_position()
sype 70:56086a37f31f 70 {
Jagang 71:95d76c181b22 71 //position_g = m_odometry.getPosRight();
Jagang 71:95d76c181b22 72 //position_d = m_odometry.getPosLeft();
sype 70:56086a37f31f 73
sype 70:56086a37f31f 74 erreur_position_g = consigne_position_g - position_g;
sype 70:56086a37f31f 75 cmd_g = erreur_position_g*Kp;
sype 70:56086a37f31f 76
sype 70:56086a37f31f 77 erreur_position_d = consigne_position_d - position_d;
sype 70:56086a37f31f 78 cmd_d = erreur_position_d*Kp;
sype 70:56086a37f31f 79
sype 70:56086a37f31f 80 m_motorL.setSpeed(cmd_g);
sype 70:56086a37f31f 81 m_motorR.setSpeed(cmd_d);
sype 70:56086a37f31f 82 }