Diff: main.cpp

Revision:

81:e7b03e81b025

Parent:

80:cd4960dfa47e

Child:

82:07e13071dd7b

--- a/main.cpp	Thu May 05 16:01:55 2016 +0000
+++ b/main.cpp	Thu May 05 18:17:08 2016 +0000
@@ -1,18 +1,6 @@
-#include "RoboClaw/RoboClaw.h"
-#include "mbed.h"
-
-#define ADR 0x80
-#define accel_angle 12000
-#define vitesse_angle 10000
-#define deccel_angle 12000
+#include "entete.h"
 
-#define accel_dista 12000
-#define vitesse_dista 12000
-#define deccel_dista 12000
-
-void Sharps();
 BusOut drapeau (PC_8, PC_6, PC_5);
-
 RoboClaw roboclaw(ADR, 460800, PA_11, PA_12);
 AnalogIn Rcapt1(PA_4);
 int RvalRcapt1 = 0;
@@ -20,62 +8,16 @@
 int RvalRcapt2 = 0;
 AnalogIn Rcapt3(PC_1);
 int RvalRcapt3 = 0;
-
 int Ravance = 1;
-#define R_SEUIL_SHARP 0.3
+DigitalIn start(PB_7);
 
-void GotoDist(float timer) {
-    Timer t;
-    t.reset();
-    roboclaw.SpeedAccelM1M2(accel_dista, vitesse_dista, vitesse_dista);
-    t.start();
-    
-    while (t.read() < timer) {
-        if (Ravance != 1) {
-            roboclaw.ForwardM1(0);
-            roboclaw.ForwardM2(0);
-            t.stop();
-            while (Ravance != 1);
-            t.start();
-        }
-    }
-    
-    roboclaw.ForwardM1(0);
-    roboclaw.ForwardM2(0);
-    t.stop();
-    t.reset();
-}
-
-void GotoThet (float timer) {
-    Timer t;
-    t.reset();
-    t.start();
-    roboclaw.SpeedAccelM1M2(accel_dista, vitesse_dista, -vitesse_dista);
-    
-    while (t.read() < timer) {
-        if (Ravance != 1) {
-            roboclaw.ForwardM1(0);
-            roboclaw.ForwardM2(0);
-            t.stop();
-            while (Ravance != 1);
-            t.start();
-        }
-    }
-    
-    roboclaw.ForwardM1(0);
-    roboclaw.ForwardM2(0);
-    
-    t.stop();
-    t.reset();
-}
 /* Debut du programme */
 int main(void)
 {
     Ticker ticker;
     ticker.attach(&Sharps, 0.01);
     
-    GotoDist(7);
-    GotoThet(7);
+    TestDist3(1,1);
     
     while(1);
 }
@@ -105,4 +47,76 @@
         drapeau = 1;
     else 
         drapeau = 2;
+}
+
+void GotoDist(float timer) {
+    Timer t;
+    
+    roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
+    roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
+    
+    t.reset();
+    t.start();
+    
+    while (t.read() < timer) {
+        if (Ravance != 1) {
+            roboclaw.ForwardM1(0);
+            roboclaw.ForwardM2(0);
+            t.stop();
+            while (Ravance != 1);
+            roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
+            roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
+            t.start();
+        }
+    }
+    
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    
+    t.stop();
+    t.reset();
+    
+    wait(waiting_time);
+}
+
+void GotoThet (float timer, int signe) {
+    Timer t;
+    
+    if (signe < 0) {
+        roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
+        roboclaw.SpeedAccelM2(-accel_dista, -vitesse_dista);
+    }
+    else {
+        roboclaw.SpeedAccelM1(-accel_dista, -vitesse_dista);
+        roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
+    }
+    
+    t.reset();
+    t.start();
+    
+    while (t.read() < timer) {
+        if (Ravance != 1) {
+            roboclaw.ForwardM1(0);
+            roboclaw.ForwardM2(0);
+            t.stop();
+            while (Ravance != 1);
+            t.start();
+        }
+        if (signe < 0) {
+            roboclaw.SpeedAccelM1(accel_dista, vitesse_dista);
+            roboclaw.SpeedAccelM2(-accel_dista, -vitesse_dista);
+        }
+        else {
+            roboclaw.SpeedAccelM1(-accel_dista, -vitesse_dista);
+            roboclaw.SpeedAccelM2(accel_dista, vitesse_dista);
+        }
+    }
+    
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    
+    t.stop();
+    t.reset();
+    
+    wait(waiting_time);
 }
\ No newline at end of file