mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-14 12:26:06 +01:00
PID
This commit is contained in:
parent
4c44af3e4c
commit
a6179781e0
|
@ -22,7 +22,7 @@ bool diagFPGA(void* arg)
|
||||||
(void)arg;
|
(void)arg;
|
||||||
|
|
||||||
recu = false;
|
recu = false;
|
||||||
registerRxHandler(C2FD_PING, setRecu);
|
registerRxHandlerCF(C2FD_PING, setRecu);
|
||||||
sendCF(C2FD_PING, NULL, 0);
|
sendCF(C2FD_PING, NULL, 0);
|
||||||
|
|
||||||
for (int i = 0; i <= DIAGNOSTIC_SERIAL_TIMEOUT; i += DIAGNOSTIC_POLL_INTERVAL) {
|
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);
|
usleep(DIAGNOSTIC_POLL_INTERVAL * 1000);
|
||||||
}
|
}
|
||||||
registerRxHandler(C2FD_PING, NULL);
|
registerRxHandlerCF(C2FD_PING, NULL);
|
||||||
return recu;
|
return recu;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -9,7 +9,7 @@
|
||||||
#define DIAGNOSTIC_SERIAL_TIMEOUT 10000
|
#define DIAGNOSTIC_SERIAL_TIMEOUT 10000
|
||||||
|
|
||||||
#define DIAGNOSTIC_TENSION_TEST 3
|
#define DIAGNOSTIC_TENSION_TEST 3
|
||||||
#define DIAGNOSTIC_CODEUSES_DIFF_MIN 1000
|
#define DIAGNOSTIC_CODEUSES_DIFF_MIN 100
|
||||||
#define DIAGNOSTIC_TEMPS_ROTATION 250
|
#define DIAGNOSTIC_TEMPS_ROTATION 250
|
||||||
|
|
||||||
// Public
|
// Public
|
||||||
|
|
|
@ -20,7 +20,7 @@
|
||||||
#define MOTOR_NOMINAL_TENSION 24.0 // V (from datasheet)
|
#define MOTOR_NOMINAL_TENSION 24.0 // V (from datasheet)
|
||||||
#define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec)
|
#define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec)
|
||||||
#define MOTOR_CONTROLLER_REFERENCE 5 // V (from wiring)
|
#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 MOTOR_SATURATION_MAX 12.0 // V (from testing)
|
||||||
#define PWM_MAX 3.3 // V (from FPGA datasheet)
|
#define PWM_MAX 3.3 // V (from FPGA datasheet)
|
||||||
#define CODER_RESOLUTION 370.0 // cycles/motor rev
|
#define CODER_RESOLUTION 370.0 // cycles/motor rev
|
||||||
|
@ -35,11 +35,11 @@
|
||||||
// Constantes asservissement
|
// Constantes asservissement
|
||||||
#define D_DIR_ECART_MIN 1.0 // mm
|
#define D_DIR_ECART_MIN 1.0 // mm
|
||||||
#define D_DIR_ECART_MAX 5.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
|
#endif
|
||||||
|
|
|
@ -27,8 +27,16 @@ bool oRetenu;
|
||||||
bool dRetenu;
|
bool dRetenu;
|
||||||
float lErr;
|
float lErr;
|
||||||
float rErr;
|
float rErr;
|
||||||
|
float lErrPrev;
|
||||||
|
float rErrPrev;
|
||||||
|
float lVolt;
|
||||||
|
float rVolt;
|
||||||
unsigned int nbCalcCons;
|
unsigned int nbCalcCons;
|
||||||
|
|
||||||
|
struct timespec pidStart;
|
||||||
|
struct timespec pidNow;
|
||||||
|
struct timespec pidEcoule;
|
||||||
|
|
||||||
void configureMovement()
|
void configureMovement()
|
||||||
{
|
{
|
||||||
stop();
|
stop();
|
||||||
|
@ -56,6 +64,8 @@ void configureMovement()
|
||||||
registerDebugVar("oRetenu", d, &oRetenu);
|
registerDebugVar("oRetenu", d, &oRetenu);
|
||||||
registerDebugVar("lErr", f, &lErr);
|
registerDebugVar("lErr", f, &lErr);
|
||||||
registerDebugVar("rErr", f, &rErr);
|
registerDebugVar("rErr", f, &rErr);
|
||||||
|
registerDebugVar("lVolt", f, &lVolt);
|
||||||
|
registerDebugVar("rVolt", f, &rVolt);
|
||||||
registerDebugVar("nbCalcCons", d, &nbCalcCons);
|
registerDebugVar("nbCalcCons", d, &nbCalcCons);
|
||||||
|
|
||||||
disableConsigne();
|
disableConsigne();
|
||||||
|
@ -73,6 +83,14 @@ float angleGap(float target, float actual)
|
||||||
return fmod(target - actual + M_PI, 2 * M_PI) - M_PI;
|
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* TaskMovement(void* pData)
|
||||||
{
|
{
|
||||||
(void)pData;
|
(void)pData;
|
||||||
|
@ -83,6 +101,11 @@ void* TaskMovement(void* pData)
|
||||||
oRetenu = true;
|
oRetenu = true;
|
||||||
dRetenu = true;
|
dRetenu = true;
|
||||||
|
|
||||||
|
float lErrInteg = 0;
|
||||||
|
float rErrInteg = 0;
|
||||||
|
|
||||||
|
clock_gettime(CLOCK_REALTIME, &pidStart);
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
|
|
||||||
// Test if enabled
|
// Test if enabled
|
||||||
|
@ -92,44 +115,88 @@ void* TaskMovement(void* pData)
|
||||||
}
|
}
|
||||||
pthread_mutex_unlock(&movEnableMutex);
|
pthread_mutex_unlock(&movEnableMutex);
|
||||||
|
|
||||||
|
// Wait for new calculation if not calculated yet
|
||||||
lastPosCalc = getPositionNewer(&connu, lastPosCalc);
|
lastPosCalc = getPositionNewer(&connu, lastPosCalc);
|
||||||
|
|
||||||
// Destination → ordre
|
// Destination → ordre
|
||||||
pthread_mutex_lock(&movCons);
|
pthread_mutex_lock(&movCons);
|
||||||
xDiff = cons.x - connu.x;
|
xDiff = cons.x - connu.x;
|
||||||
yDiff = cons.y - connu.y;
|
yDiff = cons.y - connu.y;
|
||||||
oEcart = angleGap(cons.o, connu.o);
|
oEcart = angleGap(cons.o, connu.o);
|
||||||
|
|
||||||
|
// Distance d'écart entre la position actuelle et celle de consigne
|
||||||
dDirEcart = hypotf(xDiff, yDiff);
|
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);
|
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) {
|
if (dDirEcart > D_DIR_ECART_MAX) {
|
||||||
oRetenu = true;
|
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) {
|
} else if (dDirEcart < D_DIR_ECART_MIN) {
|
||||||
oRetenu = false;
|
oRetenu = false;
|
||||||
}
|
}
|
||||||
oErr = oRetenu ? oDirEcart : oEcart;
|
oErr = oRetenu ? oDirEcart : oEcart;
|
||||||
|
|
||||||
float oDirEcartAbs = fabs(oDirEcart);
|
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) {
|
if (oDirEcartAbs > O_DIR_ECART_MAX) {
|
||||||
dRetenu = true;
|
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) {
|
} else if (oDirEcartAbs < O_DIR_ECART_MIN) {
|
||||||
dRetenu = false;
|
dRetenu = false;
|
||||||
}
|
}
|
||||||
dErr = dRetenu ? 0 : dDirEcart;
|
dErr = dRetenu ? 0 : dDirEcart;
|
||||||
|
|
||||||
// Ordre → Volt
|
// 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 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;
|
lErr = dErrRev - oErrRev;
|
||||||
rErr = dErrRev + oErrRev;
|
rErr = dErrRev + oErrRev;
|
||||||
|
|
||||||
// PID
|
// 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++;
|
nbCalcCons++;
|
||||||
}
|
}
|
||||||
|
@ -146,6 +213,7 @@ void deconfigureMovement()
|
||||||
void enableConsigne()
|
void enableConsigne()
|
||||||
{
|
{
|
||||||
pthread_mutex_lock(&movEnableMutex);
|
pthread_mutex_lock(&movEnableMutex);
|
||||||
|
clock_gettime(CLOCK_REALTIME, &pidNow);
|
||||||
movEnableBool = true;
|
movEnableBool = true;
|
||||||
pthread_cond_signal(&movEnableCond);
|
pthread_cond_signal(&movEnableCond);
|
||||||
pthread_mutex_unlock(&movEnableMutex);
|
pthread_mutex_unlock(&movEnableMutex);
|
||||||
|
|
|
@ -89,15 +89,20 @@ void* TaskParcours(void* pdata)
|
||||||
|
|
||||||
sleep(1);
|
sleep(1);
|
||||||
|
|
||||||
struct position dest2 = {300, -300, 0};
|
struct position dest2 = {0, 0, M_PI_2};
|
||||||
setDestination(&dest2);
|
setDestination(&dest2);
|
||||||
|
|
||||||
sleep(5);
|
sleep(10);
|
||||||
|
|
||||||
|
stop();
|
||||||
|
|
||||||
/* */
|
/* */
|
||||||
/* struct position dest3 = {1000, 1000, 0}; */
|
/* struct position dest3 = {1000, 1000, 0}; */
|
||||||
/* setDestination(&dest3); */
|
/* setDestination(&dest3); */
|
||||||
/* */
|
/* */
|
||||||
/* sleep(5); */
|
/* sleep(5); */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
|
@ -85,7 +85,7 @@ void configurePosition()
|
||||||
pthread_mutex_init(&posPolling, NULL);
|
pthread_mutex_init(&posPolling, NULL);
|
||||||
pthread_mutex_init(&posConnu, NULL);
|
pthread_mutex_init(&posConnu, NULL);
|
||||||
pthread_cond_init(&newPos, NULL);
|
pthread_cond_init(&newPos, NULL);
|
||||||
registerRxHandler(F2CI_CODER, onF2CI_CODER);
|
registerRxHandlerCF(F2CI_CODER, onF2CI_CODER);
|
||||||
registerDebugVar("lCodTot", ld, &lCodTot);
|
registerDebugVar("lCodTot", ld, &lCodTot);
|
||||||
registerDebugVar("rCodTot", ld, &rCodTot);
|
registerDebugVar("rCodTot", ld, &rCodTot);
|
||||||
connu.x = 0;
|
connu.x = 0;
|
||||||
|
|
Loading…
Reference in a new issue