PID

chef/src/diagnostics.c

@@ -22,7 +22,7 @@ bool diagFPGA(void* arg)
     (void)arg;
 
     recu = false;
-    registerRxHandler(C2FD_PING, setRecu);
+    registerRxHandlerCF(C2FD_PING, setRecu);
     sendCF(C2FD_PING, NULL, 0);
 
     for (int i = 0; i <= DIAGNOSTIC_SERIAL_TIMEOUT; i += DIAGNOSTIC_POLL_INTERVAL) {
@@ -31,7 +31,7 @@ bool diagFPGA(void* arg)
         }
         usleep(DIAGNOSTIC_POLL_INTERVAL * 1000);
     }
-    registerRxHandler(C2FD_PING, NULL);
+    registerRxHandlerCF(C2FD_PING, NULL);
     return recu;
 }
 

chef/src/diagnostics.h

@@ -9,7 +9,7 @@
 #define DIAGNOSTIC_SERIAL_TIMEOUT 10000
 
 #define DIAGNOSTIC_TENSION_TEST 3
-#define DIAGNOSTIC_CODEUSES_DIFF_MIN 1000
+#define DIAGNOSTIC_CODEUSES_DIFF_MIN 100
 #define DIAGNOSTIC_TEMPS_ROTATION 250
 
 // Public

chef/src/dimensions.h

@@ -20,7 +20,7 @@
 #define MOTOR_NOMINAL_TENSION 24.0 // V (from datasheet)
 #define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec)
 #define MOTOR_CONTROLLER_REFERENCE 5 // V (from wiring)
-#define MOTOR_SATURATION_MIN 0.1 // V (from random)
+#define MOTOR_SATURATION_MIN 0 // V (from random)
 #define MOTOR_SATURATION_MAX 12.0 // V (from testing)
 #define PWM_MAX 3.3 // V (from FPGA datasheet)
 #define CODER_RESOLUTION 370.0 // cycles/motor rev
@@ -35,11 +35,11 @@
 // Constantes asservissement
 #define D_DIR_ECART_MIN 1.0 // mm
 #define D_DIR_ECART_MAX 5.0 // mm
-#define O_DIR_ECART_MIN (2.5 / 360.0 * 2.0 * M_PI) // rad
+#define O_DIR_ECART_MIN (6.0 / 360.0 * 2.0 * M_PI) // rad
-#define O_DIR_ECART_MAX (7.5 / 360.0 * 2.0 * M_PI) // rad
+#define O_DIR_ECART_MAX (45.0 / 360.0 * 2.0 * M_PI) // rad
-#define O_GAIN 1
+#define O_GAIN 3.0
-#define P 2
+#define P 3.0
-#define I 0
+#define I 0.0
-#define D 0
+#define D 0.0
 
 #endif

chef/src/movement.c

@@ -27,8 +27,16 @@ bool oRetenu;
 bool dRetenu;
 float lErr;
 float rErr;
+float lErrPrev;
+float rErrPrev;
+float lVolt;
+float rVolt;
 unsigned int nbCalcCons;
 
+struct timespec pidStart;
+struct timespec pidNow;
+struct timespec pidEcoule;
+
 void configureMovement()
 {
     stop();
@@ -56,6 +64,8 @@ void configureMovement()
     registerDebugVar("oRetenu", d, &oRetenu);
     registerDebugVar("lErr", f, &lErr);
     registerDebugVar("rErr", f, &rErr);
+    registerDebugVar("lVolt", f, &lVolt);
+    registerDebugVar("rVolt", f, &rVolt);
     registerDebugVar("nbCalcCons", d, &nbCalcCons);
 
     disableConsigne();
@@ -73,6 +83,14 @@ float angleGap(float target, float actual)
     return fmod(target - actual + M_PI, 2 * M_PI) - M_PI;
 }
 
+float angleGap180(float target, float actual, float* dist)
+{
+    if (fabs(fmod(target - actual + M_PI, 2 * M_PI) - M_PI) > M_PI_2) {
+        *dist = -*dist;
+    }
+    return fmod(target - actual + M_PI_2, M_PI) - M_PI_2;
+}
+
 void* TaskMovement(void* pData)
 {
     (void)pData;
@@ -83,6 +101,11 @@ void* TaskMovement(void* pData)
     oRetenu = true;
     dRetenu = true;
 
+    float lErrInteg = 0;
+    float rErrInteg = 0;
+
+    clock_gettime(CLOCK_REALTIME, &pidStart);
+
     for (;;) {
 
         // Test if enabled
@@ -92,44 +115,88 @@ void* TaskMovement(void* pData)
         }
         pthread_mutex_unlock(&movEnableMutex);
 
+        // Wait for new calculation if not calculated yet
         lastPosCalc = getPositionNewer(&connu, lastPosCalc);
+
         // Destination → ordre
         pthread_mutex_lock(&movCons);
         xDiff = cons.x - connu.x;
         yDiff = cons.y - connu.y;
         oEcart = angleGap(cons.o, connu.o);
 
+        // Distance d'écart entre la position actuelle et celle de consigne
         dDirEcart = hypotf(xDiff, yDiff);
-        oDirEcart = angleGap(atan2(yDiff, xDiff), connu.o);
+        // Écart entre l'angle actuel et celui orienté vers la position de consigne
+        // Si l'angle se trouve à gauche du cercle trigo, on le remet à droite
+        // et on inverse la direction
+        oDirEcart = angleGap180(atan2(yDiff, xDiff), connu.o, &dDirEcart);
         pthread_mutex_unlock(&movCons);
 
+        // Si on est loin de la consigne, l'angle cible devient celui orienté vers la consigne
         if (dDirEcart > D_DIR_ECART_MAX) {
             oRetenu = true;
+        // Si on est proche de la consigne, l'angle cible devient celui voulu par la consigne
         } else if (dDirEcart < D_DIR_ECART_MIN) {
             oRetenu = false;
         }
         oErr = oRetenu ? oDirEcart : oEcart;
 
         float oDirEcartAbs = fabs(oDirEcart);
+        // Si l'écart avec l'angle orienté vers la consigne est grand, la distance cible devient 0
+        // pour se réorienter vers l'angle de la consigne
         if (oDirEcartAbs > O_DIR_ECART_MAX) {
             dRetenu = true;
+        // Si l'écart avec l'angle orienté vers la consigne est petit, la distance cible devient
+        // la distance entre la position actuelle et la consigne
         } else if (oDirEcartAbs < O_DIR_ECART_MIN) {
             dRetenu = false;
         }
         dErr = dRetenu ? 0 : dDirEcart;
 
         // Ordre → Volt
+        // Nombre de rotation nécessaire sur les deux roues dans le même sens pour arriver à la distance voulue
         float dErrRev = dErr / WHEEL_PERIMETER;
-        float oErrRev = O_GAIN * oErr * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER;
+        // Nombre de rotation nécessaire sur les deux roues dans le sens inverse pour arriver à l'angle voulu
+        float oErrRev = oErr * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER;
+
+        // Si on est en avancement on applique une grande priorité au retour sur la ligne
+        if (!dRetenu && oRetenu) {
+            oErrRev *= O_GAIN;
+        }
 
         lErr = dErrRev - oErrRev;
         rErr = dErrRev + oErrRev;
 
         // PID
-        float lVoltCons = P * lErr;
+        // Calcul du temps écoulé par rapport à la dernière mesure
-        float rVoltCons = P * rErr;
+        clock_gettime(CLOCK_REALTIME, &pidNow);
+        if ((pidNow.tv_nsec - pidStart.tv_nsec) < 0) {
+            pidEcoule.tv_sec = pidNow.tv_sec - pidStart.tv_sec - 1;
+            pidEcoule.tv_nsec = pidNow.tv_nsec - pidStart.tv_nsec + 1000000000UL;
+        } else {
+            pidEcoule.tv_sec = pidNow.tv_sec - pidStart.tv_sec;
+            pidEcoule.tv_nsec = pidNow.tv_nsec - pidStart.tv_nsec;
+        }
+        // Enregistrement de cette mesure comme la dernière mesure
+        pidStart.tv_sec = pidNow.tv_sec;
+        pidStart.tv_nsec = pidNow.tv_nsec;
+        float timeStep = pidEcoule.tv_sec + pidStart.tv_nsec * 1E-9;
 
-        setMoteurTension(lVoltCons, rVoltCons);
+        // Calcul des facteurs dérivé et intégrale
+        lErrInteg += (lErr + lErrPrev) / 2 * timeStep;
+        float lErrDeriv = (lErr - lErrPrev) / timeStep;
+        lErrPrev = lErr;
+
+        rErrInteg += (rErr + rErrPrev) / 2 * timeStep;
+        float rErrDeriv = (rErr - rErrPrev) / timeStep;
+        rErrPrev = rErr;
+
+        // Calcul de la commande
+        lVolt = P * lErr + I * lErrInteg + D * lErrDeriv;
+        rVolt = P * rErr + I * rErrInteg + D * rErrDeriv;
+
+        // Envoi de la commande
+        setMoteurTension(lVolt, rVolt);
 
         nbCalcCons++;
     }
@@ -146,6 +213,7 @@ void deconfigureMovement()
 void enableConsigne()
 {
     pthread_mutex_lock(&movEnableMutex);
+    clock_gettime(CLOCK_REALTIME, &pidNow);
     movEnableBool = true;
     pthread_cond_signal(&movEnableCond);
     pthread_mutex_unlock(&movEnableMutex);

chef/src/parcours.c

@@ -89,15 +89,20 @@ void* TaskParcours(void* pdata)
 
     sleep(1);
 
-    struct position dest2 = {300, -300, 0};
+    struct position dest2 = {0, 0, M_PI_2};
     setDestination(&dest2);
 
-    sleep(5);
+    sleep(10);
+
+    stop();
+
     /*  */
     /* struct position dest3 = {1000, 1000, 0}; */
     /* setDestination(&dest3); */
     /*  */
     /* sleep(5); */
 
+
+
     return NULL;
 }

chef/src/position.c

@@ -85,7 +85,7 @@ void configurePosition()
     pthread_mutex_init(&posPolling, NULL);
     pthread_mutex_init(&posConnu, NULL);
     pthread_cond_init(&newPos, NULL);
-    registerRxHandler(F2CI_CODER, onF2CI_CODER);
+    registerRxHandlerCF(F2CI_CODER, onF2CI_CODER);
     registerDebugVar("lCodTot", ld, &lCodTot);
     registerDebugVar("rCodTot", ld, &rCodTot);
     connu.x = 0;