mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-21 07:36:03 +01:00
PID
This commit is contained in:
parent
4c44af3e4c
commit
a6179781e0
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in a new issue