ajout de avancer, pivoter et etat PLS

Mestari Yousra
1 parent 8167aac3
Showing 1 changed file with 29 additions and 2 deletions Show diff stats
main.c
@@ -16,6 +16,8 @@
 #include "AX12.h"
 #include "AXS1.h"
  
+#include <math.h>
+
 /* Private typedef -----------------------------------------------------------*/
 /* Private define ------------------------------------------------------------*/
 #define P_GOAL_POSITION_L		30
@@ -261,6 +263,21 @@ void getAngle(unsigned char id, unsigned int* outAngle) {
     }
 }
  
+void avancer(int vitesse){
+    setSpeed(MOTOR_up_left, vitesse);
+    setSpeed(MOTOR_up_right, vitesse);
+    setSpeed(MOTOR_down_left, vitesse);
+    setSpeed(MOTOR_down_right, vitesse);
+}
+void pivoter (int rotation){
+    setSpeed(MOTOR_up_left, rotation);
+    setSpeed(MOTOR_up_right, - rotation);
+    setSpeed(MOTOR_down_left, 0);
+    setSpeed(MOTOR_down_right, 0);
+}
+
+
+
 /////////////////////////////////////////////////////
 ////////////////////////////// AX S1 ////////////////
 /////////////////////////////////////////////////////
@@ -610,8 +627,18 @@ int main(void)
  
  
      }
-
-  }
+     
+    while (state==PLS){
+        int vitesse=500
+        while (field<thresholdInfrared){
+            setSpeed(MOTOR_up_left, 1023);
+            setSpeed(MOTOR_up_right, vitesse);
+            setSpeed(MOTOR_down_left, 1023);
+            setSpeed(MOTOR_down_right, vitesse);
+            mdelay(floor(M_PI/vitesse));
+            vitesse=vitesse+50;
+        }
+        state==START
  
   while (1) {} ;