Diff: Arriver_bon_port/bon_port.cpp

Revision:

37:f1a8734c193d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Arriver_bon_port/bon_port.cpp	Sat Jul 17 16:07:29 2021 +0000
@@ -0,0 +1,137 @@
+#include "global.h"
+
+// {X,Y,T,Sens}
+int Nord_haut[4]={175,500,-90,1};
+int Sud_haut[4]={1500,500,-90,1};
+
+int Nord_bas[4]={175,500,-90,1};
+int Sud_bas[4]={1500,500,-90,1};
+
+int X_rob;
+int Y_rob;
+int T_rob;
+int Sens;
+
+int tourner;
+
+
+void arriver_bon_port(void)
+{   
+
+//Variable de comptage des points
+port_lu = val_girou + 1;
+    
+////////////////////////////////Girouette Sud////////////////////////////////
+    if(val_girou == 0) //Sud
+    {
+        if(Hauteur == 0) //Bas
+        {
+            if(InversStrat == 1) 
+            {
+                X_rob = Sud_bas[0];
+                Y_rob = 3000 - Sud_bas[1];//Inversion du Y
+                T_rob = -Sud_bas[2];
+                Sens  = Sud_bas[3];
+            } 
+            else 
+            {
+                X_rob = Sud_bas[0];
+                Y_rob = Sud_bas[1];
+                T_rob = Sud_bas[2];
+                Sens  = Sud_bas[3];
+            }
+        }
+        else if(Hauteur == 1) //Haut
+        {
+            if(InversStrat == 1) 
+            {
+                X_rob = Sud_haut[0];
+                Y_rob = 3000 - Sud_haut[1];//Inversion du Y
+                T_rob = -Sud_haut[2];
+                Sens  = Sud_haut[3];
+            } 
+            else 
+            {
+                X_rob = Sud_haut[0];
+                Y_rob = Sud_haut[1];
+                T_rob = Sud_haut[2];
+                Sens  = Sud_haut[3];
+            }
+        }
+        
+        GoToPosition(X_rob, Y_rob, T_rob, Sens);
+        waitingAckID = ASSERVISSEMENT_XYT;
+        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+        
+        //target_x_robot = X_rob;
+        //target_y_robot = Y_rob;
+        //target_theta_robot = T_rob;
+            
+        //GoStraight(Sens*350,0,0,0); 
+    }
+    
+////////////////////////////////Girouette Nord////////////////////////////////
+    else if(val_girou == 1) //Nord
+    {
+        if(Hauteur == 0) //Bas
+        {
+            if(InversStrat == 1) 
+            {
+                X_rob = Nord_bas[0];
+                Y_rob = 3000 - Nord_bas[1];//Inversion du Y
+                T_rob = -Nord_bas[2];
+                Sens  = Nord_bas[3];
+            } 
+            else 
+            {
+                X_rob = Nord_bas[0];
+                Y_rob = Nord_bas[1];
+                T_rob = Nord_bas[2];
+                Sens  = Nord_bas[3];
+            }   
+        }
+        else if(Hauteur == 1) //Haut
+        {
+            if(InversStrat == 1) 
+            {
+                X_rob = Nord_haut[0];
+                Y_rob = 3000 - Nord_haut[1];//Inversion du Y
+                T_rob = -Nord_haut[2];
+                Sens  = Nord_haut[3];
+            } 
+            else 
+            {
+                X_rob = Nord_haut[0];
+                Y_rob = Nord_haut[1];
+                T_rob = Nord_haut[2];
+                Sens  = Nord_haut[3];
+            } 
+        }
+        GoToPosition(X_rob, Y_rob, T_rob, Sens);
+        waitingAckID = ASSERVISSEMENT_XYT;
+        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+    
+        //target_x_robot = X_rob;
+        //target_y_robot = Y_rob;
+        //target_theta_robot = T_rob;
+            
+        //GoStraight(Sens*350,0,0,0);   
+    }
+    
+////////////////////////////////Girouette Inconnu////////////////////////////////
+    else if(val_girou == 2) //Jsp
+    {
+        tourner = 3600;
+
+        if(InversStrat == 1) 
+        {
+            tourner = -tourner;
+        }
+
+        waitingAckID = ASSERVISSEMENT_ROTATION;
+        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+        Rotate(tourner); 
+    }
+}
+        
+        
\ No newline at end of file