Revision 41:53d5990ff99d, committed 2016-04-13

Comitter:

IceTeam

Date:

Wed Apr 13 16:17:19 2016 +0000

Parent:

39:ca4dd3faffa8

Commit message:

Correction odometrie;

Changed in this revision

--- a/Odometry/Odometry.cpp	Wed Apr 13 11:27:01 2016 +0000
+++ b/Odometry/Odometry.cpp	Wed Apr 13 16:17:19 2016 +0000
@@ -56,7 +56,8 @@
     m_pulses_left = roboclawENCM2;
     
     double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right)*C / 2.0f;
-    double deltaTheta = (m_distPerTick_left*delta_left - m_distPerTick_right*delta_right)*C / m_v;
+    // deltaTheta = (m_distPerTick_left*delta_left - m_distPerTick_right*delta_right)*C / m_v;
+    double deltaTheta = (m_distPerTick_right*delta_right - m_distPerTick_left*delta_left)*C / m_v;
 
     /*double R = deltaS/deltaTheta;
 
@@ -82,6 +83,7 @@
     y += dy;
     
     theta += deltaTheta;
+    // theta = (theta%(2*PI))-PI; 
     while(theta > PI) theta -= 2*PI;
     while(theta <= -PI) theta += 2*PI;
     
@@ -89,12 +91,17 @@
 
 void Odometry::GotoXY(double x_goal, double y_goal)
 {
-    double theta_ = atan2(y_goal-y, x_goal-x);
-    logger.printf("odo::GotoXY %f\n\r", theta_);
+    double theta_;
+    if (x_goal-x != 0)
+        theta_ = atan2(y_goal-y, x_goal-x);
+    else
+        if (y_goal - y >= 0)
+            theta_ = PI/2;
+        else
+            theta_ = -PI/2;
+            
     double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
-    logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoThet(theta_);
-    logger.printf("Dist : %3.2f\tTheta : %3.2f\n\r", dist_, theta_*180/PI);
     GotoDist(dist_);
 }
 
@@ -114,7 +121,6 @@
     //pos_prog++;
     //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
     //arrived = false;
-
     int32_t distance_ticks_left;
     int32_t distance_ticks_right;
 
@@ -126,14 +132,15 @@
     while(erreur_theta >= PI) erreur_theta -= 2*PI;
     while(erreur_theta < -PI) erreur_theta += 2*PI;
     
-    logger.printf("ET : %3.2f\n\r", erreur_theta*180/PI);
+    //logger.printf("ET : %3.2f\n\r", erreur_theta*180/PI);
     
     if(erreur_theta < 0) {
-        distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
-        distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
+        distance_ticks_left = (int32_t) pos_initiale_left - (erreur_theta*m_v/2)/m_distPerTick_left;
+        distance_ticks_right = (int32_t) pos_initiale_right + (erreur_theta*m_v/2)/m_distPerTick_right;
+        
     } else {
-        distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
-        distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
+        distance_ticks_left = (int32_t) pos_initiale_left - (erreur_theta*m_v/2)/m_distPerTick_left;
+        distance_ticks_right = (int32_t) pos_initiale_right + (erreur_theta*m_v/2)/m_distPerTick_right;
     }
 
     //logger.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
@@ -142,13 +149,22 @@
 
     roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
     
-    logger.printf("On a enclenché le mouvement");
+    //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
     
-    //logger.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)); //logger.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);
+    /*while ((m_pulses_right - distance_ticks_right > 0 ? m_pulses_right - distance_ticks_right : distance_ticks_right - m_pulses_right) > 1 &&
+           (m_pulses_left - distance_ticks_left > 0 ? m_pulses_left - distance_ticks_left : distance_ticks_left - m_pulses_left) > 1)
+         logger.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);
+         
+    while(((m_pulses_right < distance_ticks_right-1) || (m_pulses_left > distance_ticks_left+1)) && sens ||
+          ((m_pulses_right > distance_ticks_right+1) || (m_pulses_left < distance_ticks_left-1)) && (!sens))
+        logger.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);
+    */
+    
+    while (m_pulses_right != distance_ticks_right && m_pulses_left != distance_ticks_left);
+        //logger.printf("[%f-%f] Theta : %f (theta %f-%f)\n\r", x, y, theta, m_pulses_right-distance_ticks_right, m_pulses_left-distance_ticks_left);
+           
     wait(0.4);
-    setTheta(theta_);
+    //setTheta(theta_);
     led = 1;
     //arrived = true;
     //logger.printf("arrivey %d\n\r",pos_prog);
@@ -170,7 +186,9 @@
 
     //logger.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)); //logger.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);
+    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left));
+        //logger.printf("[%f-%f] Theta : %f (dist %f-%f)\n\r", x, y, theta, m_pulses_right-distance_ticks_right, m_pulses_left-distance_ticks_left);
+        
     wait(0.4);
     led = 1;
     //logger.printf("arrivey %d\n\r",pos_prog);

--- a/Odometry/Odometry.h	Wed Apr 13 11:27:01 2016 +0000
+++ b/Odometry/Odometry.h	Wed Apr 13 16:17:19 2016 +0000
@@ -8,13 +8,15 @@
 #define C 1.0
 
 /* Vitesse d'acceleration d'angle reduite de 8000->4000 */
-#define accel_angle 2100
+#define accel_angle 1500
 #define vitesse_angle 3000
-#define deccel_angle 2100
+#define deccel_angle 1500
 
-#define accel_dista 9000
-#define vitesse_dista 20000
-#define deccel_dista 9000
+#define accel_dista 6000
+#define vitesse_dista 16000
+#define deccel_dista 6000
+
+// on attend un cran en dessous dans GotoDist  GotoThet
 
 /*
 *   Author : Benjamin Bertelone, reworked by Simon Emarre

--- a/main.cpp	Wed Apr 13 11:27:01 2016 +0000
+++ b/main.cpp	Wed Apr 13 16:17:19 2016 +0000
@@ -27,21 +27,37 @@
 int main(void)
 {
     init();
+    
     double angle_v = 2*PI/5;
     double distance_v = 200.0;
     float the;
     
-    odo.setPos(0, 1000, 0);
+    logger.printf("Yeah !\n\r");
+    
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    
+    odo.setPos(110, 1000, 0);
+    
+    odo.GotoXY(1500,1500);
+    odo.GotoXY(1500,500);
+    odo.GotoXY(1000,1500);
+    odo.GotoXY(500,1000);
+    
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    
+    /*
     point dep (odo.getX(), odo.getY());
-    point arr (1400, 1000);
+    point arr (1500, 1000);
     
     map m;
-    m.addObs(obsCarr (550, 1030, 250, 230));
-    m.addObs(obsCarr (740, 720, 240, 240));
+    m.addObs(obsCarr (550, 600, 50, 600));
     m.addObs(obsCarr (1170, 1260, 220, 220));
+
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
     
-    /*m.addObst (carre (1000, 1000, 210, 210));
-    m.addObst (carre (1300, 700, 180, 180));*/
     m.FindWay (dep, arr);
     logger.printf("On a cherche un chemin\n\r");
     
@@ -50,6 +66,11 @@
         logger.printf ("Il y a %d points de parcours\n\r", p.size());
         for (int i = 0; i < p.size (); i++) {
             logger.printf("Goto [%f, %f]\n\r", p[i].getX(), p[i].getY());
+        }
+        logger.printf("\n\n\r");
+        
+        for (int i = 0; i < p.size (); i++) {
+            logger.printf("Goto [%f, %f]\n\r", p[i].getX(), p[i].getY());
             //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX())); 
             odo.GotoXY(p[i].getX(), p[i].getY());
         }
@@ -63,70 +84,13 @@
     //odo.TestEntraxe(3);
     
     //odo.GotoThet(-PI/2);
+    
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    
     wait(2000);
     while (1);
     //odo.GotoXYT(2250,500,0);*/
-    /*roboclaw.ForwardM1(ADR, 0);
-    roboclaw.ForwardM2(ADR, 0);
-    int state = 0;
-    while(1)
-    {
-       // while(logger.readable()) 
-        //{
-            char c = logger.getc();
-            if(c=='z')
-            {
-                if (state != 1) {
-                    state = 1;
-                    logger.printf("Z\r\n");
-                    roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
-                    roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
-                }
-            }
-            else if(c == 's')
-            {
-                if (state != 2) {
-                    state = 2;
-                    roboclaw.ForwardM1(ADR, 0);
-                    roboclaw.ForwardM2(ADR, 0);
-                }
-            }
-            else if(c == 'd')
-            {
-                if (state != 3) {
-                    state = 3;
-                    logger.printf("D\r\n");
-                    roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
-                    roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
-                }
-            }
-            else if(c == 'q')
-            {
-                if (state != 4) {
-                    state = 4;
-                    logger.printf("Q\r\n");
-                    roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
-                    roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
-                }
-            }
-            else if(c == 'x')
-            {
-                if (state != 5) {
-                    state = 5;
-                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, -vitesse_angle, accel_angle, -vitesse_angle);
-                }
-            }
-            else if (c == '\0') { ; }
-            else {
-                if (state != 0) {
-                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
-                    state = 0;
-                }
-            }
-       // }
-       // roboclaw.ForwardM1(ADR, 0);
-       // roboclaw.ForwardM2(ADR, 0);
-    }*/
 }
 
 void init(void)