Revision 30:58bfac39e701, committed 2016-01-22

Comitter:

IceTeam

Date:

Fri Jan 22 15:46:43 2016 +0000

Parent:

29:13aa1d1a1c12

Child:

31:8bcc3a0bfa8a

Commit message:

Test avec un theta venant de l'odometrie;

Changed in this revision

--- a/Odometry/Odometry.cpp	Thu Jan 21 14:58:47 2016 +0000
+++ b/Odometry/Odometry.cpp	Fri Jan 22 15:46:43 2016 +0000
@@ -138,7 +138,7 @@
     //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_);
+    //setTheta(theta_);
     led = 1;
     //arrived = true;
     //pc.printf("arrivey %d\n\r",pos_prog);

--- a/main.cpp	Thu Jan 21 14:58:47 2016 +0000
+++ b/main.cpp	Fri Jan 22 15:46:43 2016 +0000
@@ -45,19 +45,27 @@
     //Construction des obstacles
     map.build();
     
-    map.AStar(0, 1000, 1400, 1000, 40);
+    float x=odo.getX();
+    float y=odo.getY();
+    float the = 0;
+    
+    map.AStar(x, y, 1400, 1000, 75);
     path = map.path;
     
     for(int i=0; i<path.size();i++) {
-        odo.GotoXYT(path[i].x, path[i].y, 0);
+        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX())); 
+        odo.GotoXYT(path[i].x, path[i].y, the);
     }
     
-    map.AStar(odo.getX(), odo.getY(), 0, 1000, 40);
+    map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
     path = map.path;
     
     for(int i=0; i<path.size();i++) {
-        odo.GotoXYT(path[i].x, path[i].y, PI);
+        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX())); 
+        odo.GotoXYT(path[i].x, path[i].y, the);
     }
+    
+    odo.GotoThet(0);
      
     //odo.GotoXYT(500, 50, 0);
     //odo.GotoXYT(200, 0, 0);