2018-05-06 08:14:51 +02:00
|
|
|
#include <math.h>
|
2018-05-06 18:35:26 +02:00
|
|
|
#include <pthread.h>
|
|
|
|
#include <stdio.h>
|
2018-05-09 01:00:16 +02:00
|
|
|
#include <stdlib.h>
|
2018-05-06 18:35:26 +02:00
|
|
|
#include <string.h>
|
|
|
|
#include <unistd.h>
|
2018-02-16 15:44:45 +01:00
|
|
|
|
2018-05-06 18:35:26 +02:00
|
|
|
#include "debug.h"
|
2018-05-06 08:14:51 +02:00
|
|
|
#include "motor.h"
|
|
|
|
#include "movement.h"
|
2018-05-09 04:10:45 +02:00
|
|
|
#include "securite.h"
|
2018-05-01 08:45:02 +02:00
|
|
|
|
2018-05-06 18:35:26 +02:00
|
|
|
pthread_t tMovement;
|
|
|
|
struct position cons;
|
|
|
|
pthread_mutex_t movCons;
|
|
|
|
pthread_mutex_t movEnableMutex;
|
|
|
|
pthread_cond_t movEnableCond;
|
|
|
|
bool movEnableBool;
|
2018-05-09 01:00:16 +02:00
|
|
|
pthread_mutex_t movDoneMutex;
|
|
|
|
pthread_cond_t movDoneCond;
|
|
|
|
bool movDoneBool;
|
2018-05-06 18:35:26 +02:00
|
|
|
|
|
|
|
float xDiff;
|
|
|
|
float yDiff;
|
|
|
|
float oEcart;
|
|
|
|
float dDirEcart;
|
|
|
|
float oDirEcart;
|
|
|
|
float dErr;
|
|
|
|
float oErr;
|
|
|
|
bool oRetenu;
|
|
|
|
bool dRetenu;
|
|
|
|
float lErr;
|
|
|
|
float rErr;
|
2018-05-07 20:25:38 +02:00
|
|
|
float lErrPrev;
|
|
|
|
float rErrPrev;
|
2018-05-06 18:35:26 +02:00
|
|
|
unsigned int nbCalcCons;
|
|
|
|
|
2018-05-07 20:25:38 +02:00
|
|
|
struct timespec pidStart;
|
|
|
|
struct timespec pidNow;
|
|
|
|
struct timespec pidEcoule;
|
|
|
|
|
2018-04-04 16:17:13 +02:00
|
|
|
void configureMovement()
|
2018-02-16 15:44:45 +01:00
|
|
|
{
|
2018-05-01 08:45:02 +02:00
|
|
|
stop();
|
2018-05-06 18:35:26 +02:00
|
|
|
|
2018-05-09 09:57:11 +02:00
|
|
|
configureMotor();
|
2018-05-09 04:10:45 +02:00
|
|
|
configureSecurite();
|
|
|
|
|
2018-05-06 18:35:26 +02:00
|
|
|
nbCalcCons = 0;
|
|
|
|
pthread_mutex_init(&movCons, NULL);
|
|
|
|
|
|
|
|
pthread_mutex_init(&movEnableMutex, NULL);
|
|
|
|
pthread_cond_init(&movEnableCond, NULL);
|
|
|
|
movEnableBool = false;
|
2018-05-09 01:00:16 +02:00
|
|
|
pthread_mutex_init(&movDoneMutex, NULL);
|
|
|
|
pthread_cond_init(&movDoneCond, NULL);
|
|
|
|
movDoneBool = false;
|
2018-05-06 18:35:26 +02:00
|
|
|
|
|
|
|
pthread_create(&tMovement, NULL, TaskMovement, NULL);
|
|
|
|
|
|
|
|
registerDebugVar("xCons", f, &cons.x);
|
|
|
|
registerDebugVar("yCons", f, &cons.y);
|
|
|
|
registerDebugVar("oCons", f, &cons.o);
|
|
|
|
registerDebugVar("xDiff", f, &xDiff);
|
|
|
|
registerDebugVar("yDiff", f, &yDiff);
|
|
|
|
registerDebugVar("oEcart", f, &oEcart);
|
|
|
|
registerDebugVar("dErr", f, &dErr);
|
|
|
|
registerDebugVar("oErr", f, &oErr);
|
|
|
|
registerDebugVar("dDirEcart", f, &dDirEcart);
|
|
|
|
registerDebugVar("oDirEcart", f, &oDirEcart);
|
|
|
|
registerDebugVar("dRetenu", d, &dRetenu);
|
|
|
|
registerDebugVar("oRetenu", d, &oRetenu);
|
|
|
|
registerDebugVar("lErr", f, &lErr);
|
|
|
|
registerDebugVar("rErr", f, &rErr);
|
|
|
|
registerDebugVar("nbCalcCons", d, &nbCalcCons);
|
|
|
|
|
|
|
|
disableConsigne();
|
2018-02-16 15:44:45 +01:00
|
|
|
}
|
|
|
|
|
2018-05-06 18:35:26 +02:00
|
|
|
void setDestination(struct position* pos)
|
|
|
|
{
|
|
|
|
pthread_mutex_lock(&movCons);
|
|
|
|
memcpy(&cons, pos, sizeof(struct position));
|
|
|
|
pthread_mutex_unlock(&movCons);
|
|
|
|
}
|
|
|
|
|
2018-05-09 01:00:16 +02:00
|
|
|
void waitDestination()
|
|
|
|
{
|
|
|
|
pthread_mutex_lock(&movDoneMutex);
|
|
|
|
while (!movDoneBool) {
|
|
|
|
pthread_cond_wait(&movDoneCond, &movDoneMutex);
|
|
|
|
}
|
|
|
|
pthread_mutex_unlock(&movDoneMutex);
|
|
|
|
}
|
|
|
|
|
2018-05-06 18:35:26 +02:00
|
|
|
float angleGap(float target, float actual)
|
|
|
|
{
|
|
|
|
return fmod(target - actual + M_PI, 2 * M_PI) - M_PI;
|
|
|
|
}
|
|
|
|
|
2018-05-07 20:25:38 +02:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2018-05-06 18:35:26 +02:00
|
|
|
void* TaskMovement(void* pData)
|
|
|
|
{
|
|
|
|
(void)pData;
|
|
|
|
|
|
|
|
unsigned int lastPosCalc = 0;
|
|
|
|
struct position connu;
|
|
|
|
|
|
|
|
oRetenu = true;
|
|
|
|
dRetenu = true;
|
|
|
|
|
2018-05-07 20:25:38 +02:00
|
|
|
float lErrInteg = 0;
|
|
|
|
float rErrInteg = 0;
|
|
|
|
|
|
|
|
clock_gettime(CLOCK_REALTIME, &pidStart);
|
|
|
|
|
2018-05-06 18:35:26 +02:00
|
|
|
for (;;) {
|
|
|
|
|
|
|
|
// Test if enabled
|
|
|
|
pthread_mutex_lock(&movEnableMutex);
|
|
|
|
while (!movEnableBool) {
|
|
|
|
pthread_cond_wait(&movEnableCond, &movEnableMutex);
|
|
|
|
}
|
|
|
|
pthread_mutex_unlock(&movEnableMutex);
|
|
|
|
|
2018-05-07 20:25:38 +02:00
|
|
|
// Wait for new calculation if not calculated yet
|
2018-05-06 18:35:26 +02:00
|
|
|
lastPosCalc = getPositionNewer(&connu, lastPosCalc);
|
2018-05-07 20:25:38 +02:00
|
|
|
|
2018-05-06 18:35:26 +02:00
|
|
|
// Destination → ordre
|
2018-05-09 04:10:45 +02:00
|
|
|
bool angleInsignifiant = isnan(cons.o);
|
2018-05-06 18:35:26 +02:00
|
|
|
pthread_mutex_lock(&movCons);
|
|
|
|
xDiff = cons.x - connu.x;
|
|
|
|
yDiff = cons.y - connu.y;
|
2018-05-09 04:10:45 +02:00
|
|
|
oEcart = angleInsignifiant ? 0 : angleGap(cons.o, connu.o);
|
2018-05-06 18:35:26 +02:00
|
|
|
|
2018-05-07 20:25:38 +02:00
|
|
|
// Distance d'écart entre la position actuelle et celle de consigne
|
2018-05-06 18:35:26 +02:00
|
|
|
dDirEcart = hypotf(xDiff, yDiff);
|
2018-05-07 20:25:38 +02:00
|
|
|
// É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);
|
2018-05-06 18:35:26 +02:00
|
|
|
pthread_mutex_unlock(&movCons);
|
|
|
|
|
2018-05-07 20:25:38 +02:00
|
|
|
// Si on est loin de la consigne, l'angle cible devient celui orienté vers la consigne
|
2018-05-06 18:35:26 +02:00
|
|
|
if (dDirEcart > D_DIR_ECART_MAX) {
|
|
|
|
oRetenu = true;
|
2018-05-09 01:00:16 +02:00
|
|
|
// Si on est proche de la consigne, l'angle cible devient celui voulu par la consigne
|
2018-05-06 18:35:26 +02:00
|
|
|
} else if (dDirEcart < D_DIR_ECART_MIN) {
|
|
|
|
oRetenu = false;
|
|
|
|
}
|
|
|
|
oErr = oRetenu ? oDirEcart : oEcart;
|
|
|
|
|
|
|
|
float oDirEcartAbs = fabs(oDirEcart);
|
2018-05-07 20:25:38 +02:00
|
|
|
// 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
|
2018-05-06 18:35:26 +02:00
|
|
|
if (oDirEcartAbs > O_DIR_ECART_MAX) {
|
|
|
|
dRetenu = true;
|
2018-05-09 01:00:16 +02:00
|
|
|
// 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
|
2018-05-06 18:35:26 +02:00
|
|
|
} else if (oDirEcartAbs < O_DIR_ECART_MIN) {
|
|
|
|
dRetenu = false;
|
|
|
|
}
|
|
|
|
dErr = dRetenu ? 0 : dDirEcart;
|
|
|
|
|
2018-05-09 04:10:45 +02:00
|
|
|
// Limitation par les valeurs des capteurs
|
2018-05-09 09:57:11 +02:00
|
|
|
#ifdef ENABLE_SECURITE
|
2018-05-09 04:10:45 +02:00
|
|
|
float avant, arriere;
|
|
|
|
getDistance(&avant, &arriere);
|
|
|
|
dErr = fmaxf(-arriere, fminf(avant, dErr));
|
2018-05-09 09:57:11 +02:00
|
|
|
#endif
|
2018-05-09 04:10:45 +02:00
|
|
|
|
2018-05-09 01:00:16 +02:00
|
|
|
// Calcul si on est arrivé
|
|
|
|
pthread_mutex_lock(&movDoneMutex);
|
|
|
|
clock_gettime(CLOCK_REALTIME, &pidNow);
|
|
|
|
movDoneBool = !dRetenu && !oRetenu && dDirEcart < D_CONS_THRESOLD && oEcart < O_CONS_THRESOLD;
|
2018-05-09 04:10:45 +02:00
|
|
|
if (movDoneBool) {
|
|
|
|
pthread_cond_signal(&movDoneCond);
|
|
|
|
}
|
2018-05-09 01:00:16 +02:00
|
|
|
pthread_mutex_unlock(&movDoneMutex);
|
|
|
|
|
2018-05-06 18:35:26 +02:00
|
|
|
// Ordre → Volt
|
2018-05-07 20:25:38 +02:00
|
|
|
// Nombre de rotation nécessaire sur les deux roues dans le même sens pour arriver à la distance voulue
|
2018-05-06 18:35:26 +02:00
|
|
|
float dErrRev = dErr / WHEEL_PERIMETER;
|
2018-05-07 20:25:38 +02:00
|
|
|
// 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;
|
|
|
|
}
|
2018-05-06 18:35:26 +02:00
|
|
|
|
|
|
|
lErr = dErrRev - oErrRev;
|
|
|
|
rErr = dErrRev + oErrRev;
|
|
|
|
|
|
|
|
// PID
|
2018-05-07 20:25:38 +02:00
|
|
|
// 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;
|
2018-05-09 09:57:11 +02:00
|
|
|
float timeStep = pidEcoule.tv_sec + pidEcoule.tv_nsec * 1E-9;
|
2018-05-07 20:25:38 +02:00
|
|
|
|
|
|
|
// 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
|
2018-05-09 09:57:11 +02:00
|
|
|
float lVolt = P * lErr + I * lErrInteg + D * lErrDeriv;
|
|
|
|
float rVolt = P * rErr + I * rErrInteg + D * rErrDeriv;
|
2018-05-06 18:35:26 +02:00
|
|
|
|
2018-05-07 20:25:38 +02:00
|
|
|
// Envoi de la commande
|
|
|
|
setMoteurTension(lVolt, rVolt);
|
2018-05-06 18:35:26 +02:00
|
|
|
|
|
|
|
nbCalcCons++;
|
|
|
|
}
|
|
|
|
|
|
|
|
return NULL;
|
|
|
|
}
|
2018-04-04 16:17:13 +02:00
|
|
|
|
2018-05-01 09:03:33 +02:00
|
|
|
void deconfigureMovement()
|
|
|
|
{
|
|
|
|
stop();
|
2018-05-06 18:35:26 +02:00
|
|
|
pthread_cancel(tMovement);
|
2018-05-09 04:10:45 +02:00
|
|
|
deconfigureSecurite();
|
2018-05-09 09:57:11 +02:00
|
|
|
deconfigureMotor();
|
2018-05-06 18:35:26 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
void enableConsigne()
|
|
|
|
{
|
|
|
|
pthread_mutex_lock(&movEnableMutex);
|
2018-05-07 20:25:38 +02:00
|
|
|
clock_gettime(CLOCK_REALTIME, &pidNow);
|
2018-05-06 18:35:26 +02:00
|
|
|
movEnableBool = true;
|
|
|
|
pthread_cond_signal(&movEnableCond);
|
|
|
|
pthread_mutex_unlock(&movEnableMutex);
|
|
|
|
}
|
|
|
|
|
|
|
|
void disableConsigne()
|
|
|
|
{
|
|
|
|
pthread_mutex_lock(&movEnableMutex);
|
|
|
|
movEnableBool = false;
|
|
|
|
// No signal here, will be disabled on next run
|
|
|
|
pthread_mutex_unlock(&movEnableMutex);
|
2018-05-01 09:03:33 +02:00
|
|
|
}
|