1
0
Fork 0
mirror of https://github.com/RobotechLille/cdf2018-principal synced 2024-11-21 07:36:03 +01:00
This commit is contained in:
Geoffrey Frogeye 2018-05-07 20:25:38 +02:00
parent 4c44af3e4c
commit a6179781e0
6 changed files with 91 additions and 18 deletions

View file

@ -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;
}

View file

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

View file

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

View file

@ -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);

View file

@ -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;
}

View file

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