Diff: main.cpp

Revision:

80:cd4960dfa47e

Parent:

79:b11b50108ae5

Child:

81:e7b03e81b025

--- a/main.cpp	Thu May 05 15:11:06 2016 +0000
+++ b/main.cpp	Thu May 05 16:01:55 2016 +0000
@@ -10,13 +10,99 @@
 #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;
+AnalogIn Rcapt2(PB_0);
+int RvalRcapt2 = 0;
+AnalogIn Rcapt3(PC_1);
+int RvalRcapt3 = 0;
 
+int Ravance = 1;
+#define R_SEUIL_SHARP 0.3
+
+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)
 {
-    int distance_ticks_right = 10000;
-    int distance_ticks_left = 10000;
-    roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
+    Ticker ticker;
+    ticker.attach(&Sharps, 0.01);
+    
+    GotoDist(7);
+    GotoThet(7);
+    
     while(1);
 }
+
+void Sharps() {
+    if (Rcapt1.read() > R_SEUIL_SHARP) RvalRcapt1++;
+    else RvalRcapt1--;
+    RvalRcapt1 = RvalRcapt1 > 10 ? 10 : RvalRcapt1;
+    RvalRcapt1 = RvalRcapt1 < 0 ? 0 : RvalRcapt1;
+    
+    if (Rcapt2.read() > R_SEUIL_SHARP) RvalRcapt2++;
+    else RvalRcapt2--;
+    RvalRcapt2 = RvalRcapt2 > 10 ? 10 : RvalRcapt2;
+    RvalRcapt2 = RvalRcapt2 < 0 ? 0 : RvalRcapt2;
+    
+    if (Rcapt3.read() > R_SEUIL_SHARP) RvalRcapt3++;
+    else RvalRcapt3--;
+    RvalRcapt3 = RvalRcapt3 > 10 ? 10 : RvalRcapt3;
+    RvalRcapt3 = RvalRcapt3 < 0 ? 0 : RvalRcapt3;
+    
+    if (RvalRcapt1 >= 5 || RvalRcapt2 >= 5 || RvalRcapt3 >= 5)
+        Ravance = 0;
+    else 
+        Ravance = 1;
+        
+    if (Ravance == 0)
+        drapeau = 1;
+    else 
+        drapeau = 2;
+}
\ No newline at end of file