Diff: deplacement.cpp

Revision:

89:46730de0d013

Parent:

88:e4de39dd3e2e

Child:

90:78d2c1527c95

--- a/deplacement.cpp	Fri May 06 11:25:05 2016 +0000
+++ b/deplacement.cpp	Fri May 06 11:54:27 2016 +0000
@@ -102,4 +102,21 @@
     t.reset();
     
     wait(waiting_time);
+}
+
+void GotoThet(double theta_) {
+    roboclaw.ResetEnc();
+    float diameter_left = 61.7;
+    float diameter_right = 61.8;
+    int distance_ticks_left = -theta_*ENTRAXE/(2*(diameter_left*3.14159/4096));
+    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);
+    else
+        roboclaw.SpeedAccelDeccelPositionM1M2(-accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, -accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
+
+    wait(10);
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
 }
\ No newline at end of file