ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asserv_Plan_B/planB.cpp@70:56086a37f31f, 2015-04-10 (annotated)

Committer:
sype
Date:
Fri Apr 10 15:23:05 2015 +0000
Revision:
70:56086a37f31f
Parent:
69:6354497f9f59
Child:
71:95d76c181b22
asserv B

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 62:454cd844fe1e 1 #include "planB.h"
Jagang 62:454cd844fe1e 2
Jagang 62:454cd844fe1e 3 aserv_planB::aserv_planB(Odometry &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
Jagang 62:454cd844fe1e 4 {
sype 64:6489bcfc1173 5 consigne_g = 10.0;
sype 64:6489bcfc1173 6 consigne_d = 10.0;
sype 64:6489bcfc1173 7 vitesse_g = 0;
sype 64:6489bcfc1173 8 vitesse_d = 0;
sype 64:6489bcfc1173 9 erreur_g = 0;
sype 64:6489bcfc1173 10 erreur_d = 0;
sype 64:6489bcfc1173 11 cmd_g = 0;
sype 64:6489bcfc1173 12 cmd_d = 0;
sype 70:56086a37f31f 13 somme_erreur_g = 0;
sype 70:56086a37f31f 14 somme_erreur_d = 0;
sype 70:56086a37f31f 15 Kp = 0.02;
sype 64:6489bcfc1173 16 }
sype 64:6489bcfc1173 17
sype 64:6489bcfc1173 18 void aserv_planB::control_speed()
sype 64:6489bcfc1173 19 {
Jagang 67:0bdf181586b5 20 vitesse_g = m_odometry.getVitRight();
Jagang 67:0bdf181586b5 21 vitesse_d = m_odometry.getVitLeft();
Jagang 62:454cd844fe1e 22
sype 64:6489bcfc1173 23 erreur_g = consigne_g - vitesse_g;
sype 64:6489bcfc1173 24 cmd_g = erreur_g*Kp;
sype 64:6489bcfc1173 25 erreur_d = consigne_d - vitesse_d;
sype 64:6489bcfc1173 26 cmd_d = erreur_d*Kp;
sype 64:6489bcfc1173 27
sype 64:6489bcfc1173 28 m_motorL.setSpeed(cmd_g);
sype 64:6489bcfc1173 29 m_motorR.setSpeed(cmd_d);
Jagang 62:454cd844fe1e 30 }
Jagang 62:454cd844fe1e 31
Jagang 62:454cd844fe1e 32 void aserv_planB::update(float dt)
Jagang 62:454cd844fe1e 33 {
Jagang 67:0bdf181586b5 34 control_speed();
sype 70:56086a37f31f 35 }
sype 70:56086a37f31f 36
sype 70:56086a37f31f 37 void aserv_planB::control_position()
sype 70:56086a37f31f 38 {
sype 70:56086a37f31f 39 position_g = m_odometry.getPosRight();
sype 70:56086a37f31f 40 position_d = m_odometry.getPosLeft();
sype 70:56086a37f31f 41
sype 70:56086a37f31f 42 erreur_position_g = consigne_position_g - position_g;
sype 70:56086a37f31f 43 cmd_g = erreur_position_g*Kp;
sype 70:56086a37f31f 44
sype 70:56086a37f31f 45 erreur_position_d = consigne_position_d - position_d;
sype 70:56086a37f31f 46 cmd_d = erreur_position_d*Kp;
sype 70:56086a37f31f 47
sype 70:56086a37f31f 48 m_motorL.setSpeed(cmd_g);
sype 70:56086a37f31f 49 m_motorR.setSpeed(cmd_d);
sype 70:56086a37f31f 50 }