Diff: main.cpp

Revision:

12:09932ddcb089

Parent:

11:ad99b62b119f

Child:

13:9b6578fa1669

--- a/main.cpp	Sat Jun 08 00:28:55 2019 +0000
+++ b/main.cpp	Sat Jun 08 02:29:30 2019 +0000
@@ -8,10 +8,11 @@
 
 #define PI  3.1415926535898
 
-#define NSpeed  100
+#define NSpeed  800
 
 Serial      pc          (PA_2, PA_3, 921600);
 PID         motor       (TIM3, TIM4, PA_8, PA_9, PC_6, PC_5, PC_9, PC_8);
+VMA306      UltraSon    (PB_13, PB_2, PB_14, PC_7, PB_15, PA_6);
 
 DigitalIn   bp          (PC_13);
 DigitalOut  led1        (PA_5);
@@ -84,13 +85,39 @@
     }
 }
 
+void automate_evitemment(int speed, int seuil_face, int seuil_cote) {
+    motor.setSpeed(speed, speed);
+    
+    double val_us_gauche = 0, val_us_face = 0, val_us_droite = 0;
+    while (1) {
+        if (UltraSon.isUSGReady()) val_us_gauche = UltraSon.readUSG();
+        if (UltraSon.isUSBReady()) val_us_face = UltraSon.readUSB();
+        if (UltraSon.isUSDReady()) val_us_droite = UltraSon.readUSD();
+        
+        if (val_us_face < seuil_face)  {
+            movement_rotate(speed, 80);
+        }
+        else if (val_us_gauche < seuil_cote) {
+            movement_rotate(speed, -40);
+        }
+        else if (val_us_droite < seuil_cote) {
+            movement_rotate(speed, 40);
+        }
+        
+        motor.setSpeed(speed, speed);
+        wait(0.005);
+    }
+}
+
 main ()
 {
-    pc.printf ("\n\rinit\n");
-
     motor.resetPosition();
-
-    while (1) {
+    
+    automate_evitemment(400, 50, 40);
+    
+    while (1);
+    
+    /*while (1) {
         movement_acceleration(1300, 50);
         movement_linear(1300, 1000);
         movement_deceleration(1300, 30);
@@ -105,5 +132,5 @@
         movement_linear(1300, -1000);
         movement_deceleration(1300, -30);
         wait(0.5);
-    }
+    }*/
 }
\ No newline at end of file