2018-04-04 16:17:13 +02:00
|
|
|
/*
|
|
|
|
* Fonctions de calcul de la position du robot
|
|
|
|
*/
|
|
|
|
|
2018-05-02 05:51:14 +02:00
|
|
|
#include <math.h>
|
2018-05-06 18:35:26 +02:00
|
|
|
#include <stdio.h>
|
|
|
|
#include <string.h>
|
|
|
|
#include <unistd.h>
|
2018-04-04 16:17:13 +02:00
|
|
|
|
2018-04-29 09:38:49 +02:00
|
|
|
#include "debug.h"
|
2018-05-02 05:51:14 +02:00
|
|
|
#include "dimensions.h"
|
2018-05-06 18:35:26 +02:00
|
|
|
#include "position.h"
|
2018-04-29 09:38:49 +02:00
|
|
|
|
|
|
|
// Globales
|
2018-05-02 05:51:14 +02:00
|
|
|
struct position connu;
|
2018-04-29 09:38:49 +02:00
|
|
|
struct F2CI_CODERs deltaCoders;
|
|
|
|
pthread_mutex_t posPolling;
|
2018-05-06 18:35:26 +02:00
|
|
|
pthread_mutex_t posConnu;
|
|
|
|
pthread_cond_t newPos;
|
2018-04-29 09:38:49 +02:00
|
|
|
pthread_t tPosition;
|
|
|
|
|
|
|
|
// Globales
|
|
|
|
unsigned int nbCalcPos;
|
|
|
|
long lCodTot, rCodTot;
|
|
|
|
|
2018-04-04 16:17:13 +02:00
|
|
|
void* TaskPosition(void* pData)
|
|
|
|
{
|
|
|
|
(void)pData;
|
|
|
|
|
2018-05-06 18:35:26 +02:00
|
|
|
resetPosition();
|
2018-04-04 16:17:13 +02:00
|
|
|
nbCalcPos = 0;
|
|
|
|
lCodTot = 0;
|
|
|
|
rCodTot = 0;
|
|
|
|
|
|
|
|
for (;;) {
|
|
|
|
|
|
|
|
// Sending
|
|
|
|
pthread_mutex_lock(&posPolling);
|
|
|
|
sendCF(F2CI_CODER, NULL, 0);
|
|
|
|
|
|
|
|
// Waiting for reception
|
|
|
|
pthread_mutex_lock(&posPolling);
|
|
|
|
pthread_mutex_unlock(&posPolling);
|
|
|
|
|
|
|
|
// Calculation
|
2018-05-06 12:50:03 +02:00
|
|
|
#ifdef INVERSE_L_CODER
|
|
|
|
deltaCoders.dL = -deltaCoders.dL;
|
|
|
|
#endif
|
|
|
|
#ifdef INVERSE_R_CODER
|
|
|
|
deltaCoders.dR = -deltaCoders.dR;
|
|
|
|
#endif
|
2018-04-04 16:17:13 +02:00
|
|
|
lCodTot += deltaCoders.dL;
|
|
|
|
rCodTot += deltaCoders.dR;
|
2018-05-02 05:51:14 +02:00
|
|
|
|
2018-05-06 12:50:03 +02:00
|
|
|
float dR = deltaCoders.dR * AV_PER_CYCLE;
|
|
|
|
float dL = deltaCoders.dL * AV_PER_CYCLE;
|
|
|
|
|
|
|
|
float deltaO = atan2(dR - dL, DISTANCE_BETWEEN_WHEELS);
|
|
|
|
float deltaD = (dL + dR) / 2;
|
2018-05-02 05:51:14 +02:00
|
|
|
|
2018-05-06 18:35:26 +02:00
|
|
|
pthread_mutex_lock(&posConnu);
|
2018-05-02 05:51:14 +02:00
|
|
|
connu.o += deltaO;
|
|
|
|
float deltaX = deltaD * cos(connu.o);
|
|
|
|
float deltaY = deltaD * sin(connu.o);
|
|
|
|
connu.x += deltaX;
|
|
|
|
connu.y += deltaY;
|
2018-05-06 18:35:26 +02:00
|
|
|
nbCalcPos++;
|
|
|
|
pthread_cond_signal(&newPos);
|
|
|
|
pthread_mutex_unlock(&posConnu);
|
|
|
|
|
|
|
|
|
2018-04-04 16:17:13 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
void onF2CI_CODER()
|
|
|
|
{
|
|
|
|
readCF(&deltaCoders, sizeof(struct F2CI_CODERs));
|
|
|
|
pthread_mutex_unlock(&posPolling);
|
|
|
|
}
|
|
|
|
|
|
|
|
void configurePosition()
|
|
|
|
{
|
2018-05-06 18:35:26 +02:00
|
|
|
pthread_mutex_init(&posPolling, NULL);
|
|
|
|
pthread_mutex_init(&posConnu, NULL);
|
|
|
|
pthread_cond_init(&newPos, NULL);
|
2018-05-07 20:25:38 +02:00
|
|
|
registerRxHandlerCF(F2CI_CODER, onF2CI_CODER);
|
2018-04-29 09:38:49 +02:00
|
|
|
registerDebugVar("lCodTot", ld, &lCodTot);
|
|
|
|
registerDebugVar("rCodTot", ld, &rCodTot);
|
2018-05-02 05:51:14 +02:00
|
|
|
connu.x = 0;
|
|
|
|
connu.y = 0;
|
|
|
|
connu.o = 0;
|
|
|
|
registerDebugVar("xConnu", f, &connu.x);
|
|
|
|
registerDebugVar("yConnu", f, &connu.y);
|
|
|
|
registerDebugVar("oConnu", f, &connu.o);
|
2018-04-29 09:38:49 +02:00
|
|
|
registerDebugVar("nbCalcPos", d, &nbCalcPos);
|
|
|
|
pthread_create(&tPosition, NULL, TaskPosition, NULL);
|
2018-04-04 16:17:13 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
void deconfigurePosition()
|
|
|
|
{
|
|
|
|
pthread_cancel(tPosition);
|
|
|
|
}
|
2018-05-02 08:26:35 +02:00
|
|
|
|
|
|
|
void getCoders(long* l, long* r)
|
|
|
|
{
|
|
|
|
*l = lCodTot;
|
|
|
|
*r = rCodTot;
|
2018-05-06 18:35:26 +02:00
|
|
|
}
|
2018-05-02 08:26:35 +02:00
|
|
|
|
2018-05-06 18:35:26 +02:00
|
|
|
unsigned int getPosition(struct position* pos)
|
|
|
|
{
|
|
|
|
pthread_mutex_lock(&posConnu);
|
|
|
|
unsigned int nb = nbCalcPos;
|
|
|
|
memcpy(pos, &connu, sizeof(struct position));
|
|
|
|
pthread_mutex_unlock(&posConnu);
|
|
|
|
return nb;
|
|
|
|
}
|
|
|
|
|
|
|
|
unsigned int getPositionNewer(struct position* pos, unsigned int lastCalc)
|
|
|
|
{
|
|
|
|
pthread_mutex_lock(&posConnu);
|
|
|
|
while (nbCalcPos <= lastCalc) {
|
|
|
|
pthread_cond_wait(&newPos, &posConnu);
|
|
|
|
}
|
|
|
|
pthread_mutex_unlock(&posConnu);
|
|
|
|
return getPosition(pos);
|
|
|
|
}
|
|
|
|
|
|
|
|
void setPosition(struct position* pos)
|
|
|
|
{
|
|
|
|
pthread_mutex_lock(&posConnu);
|
|
|
|
memcpy(&connu, pos, sizeof(struct position));
|
|
|
|
pthread_mutex_unlock(&posConnu);
|
|
|
|
}
|
|
|
|
|
|
|
|
void resetPosition()
|
|
|
|
{
|
|
|
|
struct position pos = {0, 0, 0};
|
|
|
|
setPosition(&pos);
|
2018-05-02 08:26:35 +02:00
|
|
|
}
|