Diff: odo_asserv.cpp

Revision:

18:48246daf0c06

Parent:

17:176a1b4a2fa8

Child:

19:c419033c0967

--- a/odo_asserv.cpp	Tue Oct 06 11:58:00 2020 +0000
+++ b/odo_asserv.cpp	Wed Oct 07 20:00:45 2020 +0000
@@ -1,7 +1,7 @@
 //Nom du fichier : odo_asserv.cpp
 #include "pins.h"
 
-#define VMAX 50
+#define VMAX 0.020
 
 ///// VARIABLES
 Ticker ticker_odo;
@@ -60,14 +60,8 @@
 double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
 double x = 0, y = 0, O = 0;
 
-double vitG = 0, vitD = 0;
-#define tpsTicker 0.020f
-
 void odo2()
 {
-    vitG = (double) ((comptG * coeffG) / tpsTicker);
-    vitD = (double) ((comptD * coeffD) / tpsTicker);
-
     dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f;
     dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe;
 
@@ -75,9 +69,11 @@
     y += (double) dDist * sin(O);
     O += (double) dAngl;
 
+
     if (O > 3.1415) O = O - (2.0f * 3.1415f);
     if (O < -3.1415) O = O + (2.0f * 3.1415f);
 
+
     comptG = 0;
     comptD = 0;
 }
@@ -111,16 +107,16 @@
 double consigneOrientation = 0;
 //double consigneOrientation = (90*3.1415)/180;
 int signe = 1;
-int cmdD = 0, cmdG = 0;
+double cmdD = 0, cmdG = 0;
 double erreurAngle = 0;
 double erreurPre = 0;
 double deltaErreur = 0;
-const double coeffPro = 25.0; // 5.0 de base
-const double coeffDer = 50.0; // 3.0 de base
+const double coeffPro = 0.06; // 0.023 de base
+const double coeffDer = 0.06; // 0.023 de base
 
 // NEW NEW NEW NEW
-int etape = 1;
-int acc = 1;
+int fnc = 0;
+bool acc = 1;
 
 void asserv()
 {
@@ -128,7 +124,8 @@
     odo2();
 
     // Calcul de la cible
-    distanceCible = sqrt((xC-x)*(xC-x)+(yC-y)*(yC-y));
+
+    distanceCible = sqrt(((xC-x)*(xC-x))+((yC-y)*(yC-y)));
 
     if(y > yC) {
         signe = -1;
@@ -136,15 +133,30 @@
         signe = 1;
     }
 
-    consigneOrientation = signe * acos((xC-x)/((xC-x)*(xC-x)*(yC-y)*(yC-y)));
+    if (xC >= x)
+        consigneOrientation = signe * acos((abs(xC-x))/distanceCible);
+
+    else
+        consigneOrientation = signe * (3.1415 - acos((abs(xC-x))/distanceCible));
 
     // Switch de sélection de l'étape
 
-    switch (etape) {
-        case 0: //
+    switch (fnc) {
+        case 0: // Stand-by
+            mot_dis();
             break;
 
         case 1: // Rotation
+            mot_en();
+
+            if (consigneOrientation > 0) {
+                motGauche_bck();
+                motDroite_fwd();
+            } else {
+                motGauche_fwd();
+                motDroite_bck();
+            }
+
             // Asservissement en position angulaire
             erreurAngle =  consigneOrientation - O;
 
@@ -152,36 +164,39 @@
 
             erreurPre  = erreurAngle;
 
-            int deltaCommande = coeffPro * erreurAngle + coeffDer * deltaErreur;
+            double deltaCommande = (abs(coeffPro * erreurAngle) + abs(coeffDer * deltaErreur));
 
-            if (deltaCommande < VMAX) {
-                cmdG = deltaCommande;
-                cmdD = cmdG;
-            } else {
-                cmdG = VMAX;
+            if(acc) {
+                cmdG = cmdG + 0.0005;
                 cmdD = cmdG;
-            }
+
+                if (cmdG >= VMAX) acc = 0;
+            } else {
+                //acc = 0;
 
-            if(acc && cmdG <VMAX) {
-                cmdG+=1;
-                cmdD = cmdG;
-            }
-            else {
-                acc = 0;
+                if (deltaCommande < VMAX) {
+                    cmdG = deltaCommande;
+                    cmdD = cmdG;
+                } else {
+                    cmdG = VMAX;
+                    cmdD = cmdG;
+                }
             }
 
             vitesseMotG(abs(cmdG));
             vitesseMotD(abs(cmdD));
 
             if (O > (consigneOrientation - (2*0.0174533)) && O < (consigneOrientation + (2*0.0174533))) {
-                mot_dis();
-                //etape++;
+                //mot_dis();
+                fnc++;
                 acc = 1;
             }
             break;
 
         case 2: // Avancer
-            cmdD = abs((int)distanceCible);
+            mot_en();
+
+            cmdD = abs((int)distanceCible)*0.0005;
             if(cmdD>VMAX) {
                 cmdD = VMAX;
             }
@@ -192,7 +207,10 @@
             vitesseMotG(cmdG);
             vitesseMotD(cmdD);
 
-            if (abs((int)distanceCible) < 1) mot_dis();
+            if (abs((int)distanceCible) < 3) {
+                //mot_dis();
+                fnc=0;
+            }
             break;
 
         case 3: // Reculer