Diff: odo_asserv.cpp

Revision:

17:176a1b4a2fa8

Parent:

16:ae65ce77b1f9

Child:

18:48246daf0c06

--- a/odo_asserv.cpp	Fri Oct 02 21:09:45 2020 +0000
+++ b/odo_asserv.cpp	Tue Oct 06 11:58:00 2020 +0000
@@ -1,7 +1,7 @@
 //Nom du fichier : odo_asserv.cpp
 #include "pins.h"
 
-#define VMAX 20
+#define VMAX 50
 
 ///// VARIABLES
 Ticker ticker_odo;
@@ -105,8 +105,9 @@
 }
 */
 
+
 double distanceCible = 0;
-double xC = 0, yC = -100; // x = xR et y = yR
+double xC = 0, yC = 0; // x = xR et y = yR
 double consigneOrientation = 0;
 //double consigneOrientation = (90*3.1415)/180;
 int signe = 1;
@@ -114,18 +115,21 @@
 double erreurAngle = 0;
 double erreurPre = 0;
 double deltaErreur = 0;
-const double coeffPro = 15.0; // 5.0 de base
-const double coeffDer = 3.0;
-bool phase_acc = 1;
+const double coeffPro = 25.0; // 5.0 de base
+const double coeffDer = 50.0; // 3.0 de base
+
+// NEW NEW NEW NEW
+int etape = 1;
+int acc = 1;
 
 void asserv()
 {
+    // Odométrie
     odo2();
 
-
+    // Calcul de la cible
     distanceCible = sqrt((xC-x)*(xC-x)+(yC-y)*(yC-y));
 
-    /*
     if(y > yC) {
         signe = -1;
     } else {
@@ -133,103 +137,68 @@
     }
 
     consigneOrientation = signe * acos((xC-x)/((xC-x)*(xC-x)*(yC-y)*(yC-y)));
-    */
 
+    // Switch de sélection de l'étape
 
-    cmdD = abs((int)distanceCible);
-    if(cmdD>VMAX) {
-        cmdD = VMAX;
-    }
-    cmdG = cmdD;
-
+    switch (etape) {
+        case 0: //
+            break;
 
-    /*
-    motGauche_fwd();
-    motDroite_fwd();
-    vitesseMotG(cmdG);
-    vitesseMotD(cmdD);
-    */
+        case 1: // Rotation
+            // Asservissement en position angulaire
+            erreurAngle =  consigneOrientation - O;
 
-    erreurAngle =  consigneOrientation - O;
+            deltaErreur = erreurAngle - erreurPre;
 
-    deltaErreur = erreurAngle - erreurPre;
-
-    erreurPre  = erreurAngle;
+            erreurPre  = erreurAngle;
 
-    int deltaCommande = coeffPro * erreurAngle + coeffDer * deltaErreur;
-
-    motGauche_bck();
-    motDroite_fwd();
-
-    cmdG = 0;
-    cmdD = 0;
-
-    cmdG += deltaCommande;
-    cmdD -= deltaCommande;
+            int deltaCommande = coeffPro * erreurAngle + coeffDer * deltaErreur;
 
-    if(cmdD>VMAX) {
-        cmdD =VMAX;
-    } else if(cmdD < -VMAX) {
-        cmdD = -VMAX;
-    }
+            if (deltaCommande < VMAX) {
+                cmdG = deltaCommande;
+                cmdD = cmdG;
+            } else {
+                cmdG = VMAX;
+                cmdD = cmdG;
+            }
 
-    if(cmdG>VMAX) {
-        cmdG =VMAX;
-    } else if(cmdG < -VMAX) {
-        cmdG = -VMAX;
-    }
+            if(acc && cmdG <VMAX) {
+                cmdG+=1;
+                cmdD = cmdG;
+            }
+            else {
+                acc = 0;
+            }
 
-    /*
-    if (cmdD > 0) {
-        //motDroite_fwd();
-        motDroite_bck();
-    } else {
-        //motDroite_bck();
-        motDroite_fwd();
-    }
+            vitesseMotG(abs(cmdG));
+            vitesseMotD(abs(cmdD));
 
-    if (cmdG > 0) {
-        motGauche_fwd();
-        //motGauche_bck();
-    } else {
-        motGauche_bck();
-        //motGauche_fwd();
-    }
-    */
-
-    /*
-    if(cmdD>150) {
-        cmdD =150;
-    } else if(cmdD < 0) {
-        cmdD = 0;
-    }
+            if (O > (consigneOrientation - (2*0.0174533)) && O < (consigneOrientation + (2*0.0174533))) {
+                mot_dis();
+                //etape++;
+                acc = 1;
+            }
+            break;
 
-    if(cmdG>150) {
-        cmdG = 150;
-    } else if(cmdG < 0) {
-        cmdG = 0;
-    }
-    */
+        case 2: // Avancer
+            cmdD = abs((int)distanceCible);
+            if(cmdD>VMAX) {
+                cmdD = VMAX;
+            }
+            cmdG = cmdD;
 
-    /*
-    if (consigneOrientation - O < 3.1415) {
-        motGauche_bck();
-        motDroite_fwd();
-    } else {
-        motGauche_fwd();
-        motDroite_bck();
+            motGauche_fwd();
+            motDroite_fwd();
+            vitesseMotG(cmdG);
+            vitesseMotD(cmdD);
+
+            if (abs((int)distanceCible) < 1) mot_dis();
+            break;
+
+        case 3: // Reculer
+            break;
+
+        default:
+            mot_dis();
     }
-    */
-
-    if (phase_acc) {
-        cmdG += 1;
-        cmdD = cmdG;
-
-        if (cmdG >= VMAX) phase_acc = 0;
-    }
-
-    vitesseMotG(abs(cmdG));
-    vitesseMotD(abs(cmdD));
-
-    if (O > (consigneOrientation - (2*0.0174533)) && O < (consigneOrientation + (2*0.0174533))) mot_dis();
 }