Revision 10:ae3178aa94e9, committed 2015-12-04

Comitter:

sype

Date:

Fri Dec 04 11:18:13 2015 +0000

Parent:

9:e39b218ab20d

Child:

11:9c70a7f4d7aa

Child:

12:d5e21f71c2a9

Commit message:

80% Motorisation

Changed in this revision

--- a/Odometry/Odometry.cpp	Tue Nov 24 23:05:12 2015 +0000
+++ b/Odometry/Odometry.cpp	Fri Dec 04 11:18:13 2015 +0000
@@ -2,106 +2,152 @@
 
 // M1 = Moteur droit, M2 = Moteur gauche
 
-Odometry::Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc) : roboclaw(rc){
+Odometry::Odometry(double diameter_right, double diameter_left, double v, uint16_t quadrature, RoboClaw &rc) : roboclaw(rc)
+{
     m_v = v;
-    m_distPerTick_left = diameter_left*PI/37400;
-    m_distPerTick_right = diameter_right*PI/37400;
+    m_distPerTick_left = diameter_left*PI/quadrature;
+    m_distPerTick_right = diameter_right*PI/quadrature;
 
     roboclaw.ForwardM1(ADR, 0);
     roboclaw.ForwardM2(ADR, 0);
-
+    roboclaw.ResetEnc(ADR);
     // Erreur autorisée sur le déplacement en angle
     erreur_ang = 0.01;
 
     m_pulses_right = 0;
     m_pulses_left = 0;
+    pos_prog = 0;
     wait_ms(100);
 }
 
-void Odometry::setPos(double x, double y, double theta){
+void Odometry::setPos(double x, double y, double theta)
+{
     this->x = x;
     this->y = y;
     this->theta = theta;
 }
 
-void Odometry::setX(double x){
+void Odometry::setX(double x)
+{
     this->x = x;
 }
 
-void Odometry::setY(double y){
+void Odometry::setY(double y)
+{
     this->y = y;
 }
 
-void Odometry::setTheta(double theta){
+void Odometry::setTheta(double theta)
+{
     this->theta = theta;
 }
 
-void Odometry::update_odo(void){
+void Odometry::update_odo(void)
+{
     int32_t delta_right = roboclaw.ReadEncM1(ADR) - m_pulses_right;
     m_pulses_right = roboclaw.ReadEncM1(ADR);
     int32_t delta_left = roboclaw.ReadEncM2(ADR) - m_pulses_left;
     m_pulses_left = roboclaw.ReadEncM2(ADR);
 
     double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
-    double deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left)*C / m_v;
+    double deltaTheta = (m_distPerTick_left*delta_left - m_distPerTick_right*delta_right) / m_v;
 
-    double radius = deltaS/deltaTheta;
-    double xO = x - radius*sin(theta);
-    double yO = y + radius*cos(theta);
+    double R = deltaS/deltaTheta;
+
+    double xO = x - R*sin(theta);
+    double yO = y + R*cos(theta);
 
     theta += deltaTheta;
 
-    x = xO + radius*sin(theta);
-    y = yO - radius*cos(theta);
+    if(deltaTheta == 0) {
+        x = x + deltaS*cos(theta);
+        y = y + deltaS*sin(theta);
+    }
+    else {
+        x = xO + R*sin(theta);
+        y = yO - R*cos(theta);
+    }
+
+    /*double dx = deltaS*cos(theta);
+    double dy = deltaS*sin(theta);
+    x += dx;
+    y += dy;
+    theta += deltaTheta;*/
 
     while(theta > PI) theta -= 2*PI;
     while(theta <= -PI) theta += 2*PI;
 }
 
-void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal){
+void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
+{
     double theta_ = atan2(y_goal-y, x_goal-x);
     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
+    //pc.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
+    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
     GotoThet(theta_);
     GotoDist(dist_);
 }
 
-void Odometry::GotoThet(double theta_) {
-    double distance_ticks_left;
-    double distance_ticks_right;
+void Odometry::GotoThet(double theta_)
+{
+    led = 0;
+    //pos_prog++;
+    //pc.printf("Theta : %3.2f\n\r", theta_*180/PI);
+    //arrived = false;
+
+    int32_t distance_ticks_left;
+    int32_t distance_ticks_right;
+
+    int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
 
     // Le calcul d'erreur est bon (testé), tu peux le vérifier par dessin
     double erreur_theta = theta_ - getTheta();
-    bool arrived = false;
 
     while(erreur_theta >= PI) erreur_theta -= 2*PI;
     while(erreur_theta <= -PI) erreur_theta += 2*PI;
 
     if(erreur_theta <= 0) {
-        distance_ticks_left = -(erreur_theta*m_v/2)/m_distPerTick_left;
-        distance_ticks_right = (erreur_theta*m_v/2)/m_distPerTick_right;
+        distance_ticks_left = (int32_t) -(erreur_theta*m_v/2)/m_distPerTick_left + pos_initiale_left;
+        distance_ticks_right = (int32_t) (erreur_theta*m_v/2)/m_distPerTick_right + pos_initiale_right;
     } else {
-        distance_ticks_left = (erreur_theta*m_v/2)/m_distPerTick_left;
-        distance_ticks_right = -(erreur_theta*m_v/2)/m_distPerTick_right;
+        distance_ticks_left = (int32_t) (erreur_theta*m_v/2)/m_distPerTick_left + pos_initiale_left;
+        distance_ticks_right = (int32_t) -(erreur_theta*m_v/2)/m_distPerTick_right + pos_initiale_right;
     }
+
+    //pc.printf("ET : %3.2f\n\r", erreur_theta*180/PI);
     pc.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 150000, 100000, (int32_t)distance_ticks_right, 150000, 150000, 100000, (int32_t)distance_ticks_left, 1);
-    // Il faut ici faire un espèce de bouclage pour vérifier qu'il est bien arrivé, j'avais pensé à :
-    /*
-    while(!arrived){
-        
-    }
-    */
+    //pc.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
+
+    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
+
+    //pc.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
+
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //pc.printf("%6d\t%6d\t%6d\t%6d\t%6d\n\r",m_pulses_right - pos_initiale_right, distance_ticks_right, m_pulses_left - pos_initiale_left, distance_ticks_left);
+    //setTheta(theta_);
+    led = 1;
+    //arrived = true;
+    //pc.printf("arrivey %d\n\r",pos_prog);
 }
 
-void Odometry::GotoDist(double distance) {
-    double temp1 = roboclaw.ReadEncM1(ADR), temp2 = roboclaw.ReadEncM2(ADR);
-    double distance_ticks_left = distance/m_distPerTick_left - temp2;
-    double distance_ticks_right =  distance/m_distPerTick_right - temp1;
-    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, 150000, 200000, 150000, (int32_t)distance_ticks_right, 150000, 200000, 150000, (int32_t)distance_ticks_left, 1);
+void Odometry::GotoDist(double distance)
+{
+    led = 0;
+    //pos_prog++;
+    //pc.printf("Dist : %3.2f\n\r", distance);
+    //arrived = false;
+
+    int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
+
+    int32_t distance_ticks_right = (int32_t) distance/m_distPerTick_right + pos_initiale_right;
+    int32_t distance_ticks_left = (int32_t) distance/m_distPerTick_left + pos_initiale_left;
+
+    roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
+
+    //pc.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
+
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //pc.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
+
+    led = 1;
+    //pc.printf("arrivey %d\n\r",pos_prog);
+    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
 }
-
-bool Odometry::isArrivedRot(double theta_) {
-    if(abs_d(getTheta()) <= abs_d(theta_+erreur_ang)) return true;
-    else if(abs_d(getTheta()) >= abs_d(theta_-erreur_ang)) return true;
-    else return false;
-}

--- a/Odometry/Odometry.h	Tue Nov 24 23:05:12 2015 +0000
+++ b/Odometry/Odometry.h	Fri Dec 04 11:18:13 2015 +0000
@@ -5,26 +5,35 @@
 #include "RoboClaw.h"
 
 #define PI 3.1415926535897932384626433832795
-#define C 1.1538461538461538461538461538462
+#define C 1.0
+
+#define accel_angle 8000
+#define vitesse_angle 16000
+#define deccel_angle 8000
+
+#define accel_dista 8000
+#define vitesse_dista 16000
+#define deccel_dista 8000
 
 /*
 *   Author : Benjamin Bertelone, reworked by Simon Emarre
 */
 
 extern Serial pc;
+extern DigitalOut led;
 
 /** Permet la gestion de l'odometrie du robot **/
 class Odometry
 {
 public:
-    /** Constructeur standart
+    /** Constructeur standard
     * @param diameter_right Definit le diametre de la roue droite
     * @param diameter_left Definit le diametre de la roue gauche
     * @param v Definit l'entraxe du robot
     * @param rc Definit une reference vers l'objet permettant l'asserv des moteurs
-        @note Cet objet doit etre initialise en amont
+    * @note Cet objet doit etre initialise en amont
     */
-    Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc);
+    Odometry(double diameter_right, double diameter_left, double v, uint16_t quadrature, RoboClaw &rc);
 
     /** Les fonctions suivantes permettent de reinitialiser les valeurs de position de l'odometrie.
     */
@@ -55,9 +64,8 @@
         else return x;
     }
 
-    /** Les fonction suivantes sont actuellement inutilisables, elles pourraient buguer en cas d'utilisation.
-    * @note pour que les fonctions retournent une valeur correcte, il faut actualiser l'odometrie avec update_odo auparavant
-    */
+    /* Les fonction suivantes sont actuellement inutilisables, elles pourraient buguer en cas d'utilisation. */
+
     double getVitLeft() {
         return m_vitLeft;
     }
@@ -94,7 +102,7 @@
     /** La fonction retourne vraie quand le robot atteint l'angle voulu avec une marge d'erreur definie par la fonction
     * @param theta_ valeur de l'angle devant etre atteint
     */
-    bool isArrivedRot(double theta_);
+    bool isArrived(void) {return arrived;}
     /** Permet de mettre à jour les valeurs de l'odometrie
     */
     void update_odo(void);
@@ -103,7 +111,7 @@
     RoboClaw &roboclaw;
     int32_t m_pulses_left;
     int32_t m_pulses_right;
-
+    uint8_t pos_prog;
     double x, y, theta;
     double m_vitLeft, m_vitRight;
     double m_distLeft, m_distRight;
@@ -111,6 +119,7 @@
     double m_distPerTick_left, m_distPerTick_right, m_v;
 
     double erreur_ang;
+    bool arrived;
 };
 
 #endif

--- a/RoboClaw.lib	Tue Nov 24 23:05:12 2015 +0000
+++ b/RoboClaw.lib	Fri Dec 04 11:18:13 2015 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/ARES/code/RoboClaw/#5c6a9045c8f7
+https://developer.mbed.org/teams/ARES/code/RoboClaw/#e64524a61cfe

--- a/main.cpp	Tue Nov 24 23:05:12 2015 +0000
+++ b/main.cpp	Fri Dec 04 11:18:13 2015 +0000
@@ -1,41 +1,81 @@
 #include "Odometry.h"
 
 #define dt 10000
+#define ATTENTE 0
+#define GO 1
+#define STOP 2
 
-InterruptIn button(USER_BUTTON);
+InterruptIn mybutton(USER_BUTTON);
+DigitalIn button(USER_BUTTON);
 DigitalOut led(LED1);
 Ticker ticker;
-Serial pc(USBTX, USBRX);
-//Serial pc(PA_9, PA_10);
-RoboClaw roboclaw(115200, PA_11, PA_12);
-Odometry odo(47.4, 47.1, 242.0, roboclaw);
+//Serial pc(USBTX, USBRX);
+Serial pc(PA_9, PA_10);
+RoboClaw roboclaw(460800, PA_11, PA_12);
+Odometry odo(63.2, 63.3, 252, 4096, roboclaw);
+
+int i = 0;
 
 void update_main(void);
 void init(void);
+void pressed(void);
+void unpressed(void);
 
 /** Debut du programme */
 int main(void)
-{   
+{
+    double angle_v = 2*PI/5;
+    double distance_v = 200.0;
     init();
-    odo.GotoXYT(2250,500,0);
+
+    odo.GotoXYT(500, 100, 0);
+    odo.GotoXYT(500, 50, 0);
+    //odo.GotoXYT(200, 0, 0);
+    //odo.GotoXYT(0, 0, 0);
+
+    //odo.GotoThet(-PI/2);
+    wait(2000);
+    //odo.GotoXYT(2250,500,0);
     while(1);
 }
 
 void init(void)
 {
-    pc.baud(115200);
+    pc.baud(9600);
     pc.printf("Hello from main !\n\r");
+    wait_ms(500);
+    
+    odo.setPos(0, 0, 0);
     roboclaw.ForwardM1(ADR, 0);
     roboclaw.ForwardM2(ADR, 0);
-    wait_ms(500);
-    roboclaw.ResetEnc(ADR);
-    odo.setPos(1500, 1000, 2*PI/3);
+    
+    while(button);
+    wait(1);
+    mybutton.fall(&pressed);
+    mybutton.rise(&unpressed);
     ticker.attach_us(&update_main,dt); // 100 Hz
 }
 
 void update_main(void)
 {
     odo.update_odo();
-    //pc.printf("EncM1 : %6d\tEncM2 : %6d\tTheta : %3.2f\n\r", roboclaw.ReadEncM1(ADR), roboclaw.ReadEncM2(ADR), odo.getTheta()*180/PI);
+    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
     //if(pc.readable()) if(pc.getc()=='l') led = !led;
 }
+
+void pressed(void)
+{
+    if(i==0) {
+        roboclaw.ForwardM1(ADR, 0);
+        roboclaw.ForwardM2(ADR, 0);
+        //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
+        i++;
+    }
+}
+
+void unpressed(void)
+{
+    if(i==1) {
+        i--;
+    }
+}
\ No newline at end of file