Revision 18:48246daf0c06, committed 2020-10-07

Comitter:

Nanaud

Date:

Wed Oct 07 20:00:45 2020 +0000

Parent:

17:176a1b4a2fa8

Child:

19:c419033c0967

Commit message:

Deplacements ok

Changed in this revision

--- a/debugPC.cpp	Tue Oct 06 11:58:00 2020 +0000
+++ b/debugPC.cpp	Wed Oct 07 20:00:45 2020 +0000
@@ -185,10 +185,11 @@
             //comptG = 0;
             //comptD = 0;
             //test1();
-            //consigneOrientation = (90*3.1415)/180;
+            //consigneOrientation = (180*3.1415)/180;
             xC = (double)0;
             yC = (double)100;
-            mot_en();
+            fnc=1;
+            //mot_en();
             break;
         case 9:     //cdon
             pc.printf("Results ALL Encoders ON/OFF\n\r");
@@ -284,8 +285,8 @@
             pc.printf("y = %lf\n\r", y);
             pc.printf("O = %lf\n\r", O);
             //pc.printf("O = %lf\n\r", (O*180)/_PI_);
-            pc.printf("vitG = %lf\n\r", vitG);
-            pc.printf("vitD = %lf\n\r", vitD);
+            //pc.printf("vitG = %lf\n\r", vitG);
+            //pc.printf("vitD = %lf\n\r", vitD);
 
             bt.printf("Odometrie :\n\r");
             bt.printf("x = %lf\n\r", x);
@@ -294,8 +295,8 @@
             bt.printf("consigneAngl = %lf\n\r", consigneOrientation);
             bt.printf("consigneDist = %lf\n\r", distanceCible);
             //bt.printf("O = %lf\n\r", (O*180)/_PI_);
-            bt.printf("vitG = %lf\n\r", vitG);
-            bt.printf("vitD = %lf\n\r", vitD);
+            //bt.printf("vitG = %lf\n\r", vitG);
+            //bt.printf("vitD = %lf\n\r", vitD);
             break;
         default:
             pc.printf("Commande invalide\n\r");

--- a/motors.cpp	Tue Oct 06 11:58:00 2020 +0000
+++ b/motors.cpp	Wed Oct 07 20:00:45 2020 +0000
@@ -9,8 +9,8 @@
 
     //motGauche_fwd();
     //motDroite_bck();
-    drvGauche.moveLinSpeed(0.01);
-    drvDroite.moveLinSpeed(0.01);
+    //drvGauche.moveLinSpeed(0.01);
+    //drvDroite.moveLinSpeed(0.01);
     //mode = 0b111; // M0, M1 et M2 sont à 1
 
     mode_M0 = 1;
@@ -80,14 +80,14 @@
     //drvDroite.setDir(BACKWARD);
 }
 
-void vitesseMotG(int vitesse)
+void vitesseMotG(double vitesse)
 {
-    drvGauche.moveLinSpeed((double)vitesse/1000);
+    drvGauche.moveLinSpeed((double)vitesse);
 }
 
-void vitesseMotD(int vitesse)
+void vitesseMotD(double vitesse)
 {
-    drvDroite.moveLinSpeed((double)vitesse/1000);
+    drvDroite.moveLinSpeed((double)vitesse);
 }
 
 

--- a/motors.h	Tue Oct 06 11:58:00 2020 +0000
+++ b/motors.h	Wed Oct 07 20:00:45 2020 +0000
@@ -13,8 +13,8 @@
 void motDroite_fwd();
 void motGauche_bck();
 void motDroite_bck();
-void vitesseMotG(int vitesse);
-void vitesseMotD(int vitesse);
+void vitesseMotG(double vitesse);
+void vitesseMotD(double vitesse);
 
 void test_drv();
 void testAngle(int cmdAngle);

--- 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

--- a/odo_asserv.h	Tue Oct 06 11:58:00 2020 +0000
+++ b/odo_asserv.h	Wed Oct 07 20:00:45 2020 +0000
@@ -10,8 +10,9 @@
 extern long comptD;
 extern double distanceCible;
 extern double consigneOrientation;
-extern double vitG;
-extern double vitD;
+extern double cmdG;
+extern double cmdD;
+extern int fnc;
 
 // Prototypes
 void cdgaRise();