Diff: Odometry/Odometry.cpp

Revision:

39:309f38d1e49c

Parent:

38:da3a2c781672

Child:

46:be4eebf40568

Child:

47:5658af4e5149

--- a/Odometry/Odometry.cpp	Sat Feb 06 10:11:28 2016 +0000
+++ b/Odometry/Odometry.cpp	Tue Apr 05 15:02:12 2016 +0000
@@ -193,3 +193,18 @@
     while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); 
     wait(0.4);
 }
+
+void Odometry::Forward(float i) {    
+    int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
+
+    int32_t distance_ticks_right = (int32_t) i/m_distPerTick_right + pos_initiale_right;
+    int32_t distance_ticks_left = (int32_t) i/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);
+
+    //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);
+    led = 1;
+}
\ No newline at end of file