From a6179781e094d5fbd672501721e080e39db4437d Mon Sep 17 00:00:00 2001 From: Geoffrey Frogeye Date: Mon, 7 May 2018 20:25:38 +0200 Subject: [PATCH] PID --- chef/src/diagnostics.c | 4 +-- chef/src/diagnostics.h | 2 +- chef/src/dimensions.h | 14 ++++---- chef/src/movement.c | 78 +++++++++++++++++++++++++++++++++++++++--- chef/src/parcours.c | 9 +++-- chef/src/position.c | 2 +- 6 files changed, 91 insertions(+), 18 deletions(-) diff --git a/chef/src/diagnostics.c b/chef/src/diagnostics.c index dcb8d3b..1479aed 100644 --- a/chef/src/diagnostics.c +++ b/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; } diff --git a/chef/src/diagnostics.h b/chef/src/diagnostics.h index ef5c04c..4226538 100644 --- a/chef/src/diagnostics.h +++ b/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 diff --git a/chef/src/dimensions.h b/chef/src/dimensions.h index 3f11f5a..b3fcfbb 100644 --- a/chef/src/dimensions.h +++ b/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_MAX (7.5 / 360.0 * 2.0 * M_PI) // rad -#define O_GAIN 1 -#define P 2 -#define I 0 -#define D 0 +#define O_DIR_ECART_MIN (6.0 / 360.0 * 2.0 * M_PI) // rad +#define O_DIR_ECART_MAX (45.0 / 360.0 * 2.0 * M_PI) // rad +#define O_GAIN 3.0 +#define P 3.0 +#define I 0.0 +#define D 0.0 #endif diff --git a/chef/src/movement.c b/chef/src/movement.c index 56ca090..1c8c09f 100644 --- a/chef/src/movement.c +++ b/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; - float rVoltCons = P * rErr; + // Calcul du temps écoulé par rapport à la dernière mesure + 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); diff --git a/chef/src/parcours.c b/chef/src/parcours.c index cf138f5..d188ea8 100644 --- a/chef/src/parcours.c +++ b/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; } diff --git a/chef/src/position.c b/chef/src/position.c index fa89f0b..c7cac0b 100644 --- a/chef/src/position.c +++ b/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;