Revision 94:86b9bd6d5c28, committed 2016-05-06

Comitter:

IceTeam

Date:

Fri May 06 18:25:24 2016 +0000

Parent:

93:c0b040954eac

Child:

96:1e91cac784fe

Commit message:

Save new GotoThet

Changed in this revision

--- a/ControlleurPince/ControlleurPince.cpp	Fri May 06 17:33:43 2016 +0000
+++ b/ControlleurPince/ControlleurPince.cpp	Fri May 06 18:25:24 2016 +0000
@@ -7,8 +7,6 @@
     pos_r = 0;
     pos_z = 0;
     pos_l = 0;
-    
-    
 }
 
 void ControlleurPince::init()
@@ -67,8 +65,8 @@
         => pos_l = (130-delta)/2 + center
         */
         
-        float r = (130.f-delta)/2.f - center;
-        float l = (130.f-delta)/2.f + center;
+        float r = (150.f-delta)/2.f - center;
+        float l = (150.f-delta)/2.f + center;
         
         RMot.mm(r-pos_r,true);
         pos_r = r;
@@ -86,13 +84,13 @@
 {
     Lax12.setGoal(150);
     Rax12.setGoal(150);
-    wait(0.2);
+    wait(2);
 }
 
 
 void ControlleurPince::open()
 {
-    Lax12.setGoal(100);
-    Rax12.setGoal(200);
-    wait(0.2);
+    Lax12.setGoal(46);
+    Rax12.setGoal(248);
+    wait(2);
 }
\ No newline at end of file

--- a/deplacement.cpp	Fri May 06 17:33:43 2016 +0000
+++ b/deplacement.cpp	Fri May 06 18:25:24 2016 +0000
@@ -75,11 +75,30 @@
     int distance_ticks_right = theta_*ENTRAXE/(2*(diameter_right*3.14159/4096));
 
     if (theta_ < 0)
-        roboclaw.SpeedAccelDeccelPositionM1M2(-accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
+        roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
     else
-        roboclaw.SpeedAccelDeccelPositionM1M2(-accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, -accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
+        roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
 
-    wait(10);
+    wait(3);
     roboclaw.ForwardM1(0);
     roboclaw.ForwardM2(0);
+}
+
+void Odometry::GotoDistPos(double distance)
+{
+    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(accel_dista, avant ? vitesse_dista : 0 - vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, avant ? vitesse_dista : 0 - vitesse_dista, deccel_dista, distance_ticks_left, 1);
+
+    //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);
+    
+    wait(0.4);
+    //logger.printf("arrivey %d\n\r",pos_prog);
+    //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
+    logger.printf("End\n\r");
 }
\ No newline at end of file

--- a/entete.h	Fri May 06 17:33:43 2016 +0000
+++ b/entete.h	Fri May 06 18:25:24 2016 +0000
@@ -48,9 +48,9 @@
 
 #define ENTRAXE 243.8
 
-#define accel_angle 400
-#define vitesse_angle 400
-#define deccel_angle 400
+#define accel_angle 3200
+#define vitesse_angle 3200
+#define deccel_angle 3200
 
 #define accel_dista 2000
 #define vitesse_dista 2000

--- a/main.cpp	Fri May 06 17:33:43 2016 +0000
+++ b/main.cpp	Fri May 06 18:25:24 2016 +0000
@@ -87,8 +87,8 @@
         GotoDist(4.6);
     }
     else if (SCouleur == NOIR) {
-        end.attach(&endFonc, 90);
-        GotoDist(9.0);
+        GotoThet(-3.14159/2);
+        GotoDist(1);
     }
 
     while(1);