2018-04-04 16:17:13 +02:00
|
|
|
/*
|
|
|
|
* Fonctions de calcul de la position du robot
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include <stdio.h>
|
|
|
|
|
2018-04-29 09:38:49 +02:00
|
|
|
#include "debug.h"
|
|
|
|
#include "position.h"
|
|
|
|
|
|
|
|
// Globales
|
|
|
|
struct position actuel;
|
|
|
|
struct F2CI_CODERs deltaCoders;
|
|
|
|
pthread_mutex_t posPolling;
|
|
|
|
pthread_t tPosition;
|
|
|
|
|
|
|
|
// Globales
|
|
|
|
unsigned int nbCalcPos;
|
|
|
|
long lCodTot, rCodTot;
|
|
|
|
|
|
|
|
|
2018-04-04 16:17:13 +02:00
|
|
|
void* TaskPosition(void* pData)
|
|
|
|
{
|
|
|
|
(void)pData;
|
|
|
|
|
|
|
|
nbCalcPos = 0;
|
|
|
|
lCodTot = 0;
|
|
|
|
rCodTot = 0;
|
|
|
|
|
|
|
|
pthread_mutex_init(&posPolling, NULL);
|
|
|
|
|
|
|
|
for (;;) {
|
|
|
|
|
|
|
|
// Sending
|
|
|
|
pthread_mutex_lock(&posPolling);
|
|
|
|
sendCF(F2CI_CODER, NULL, 0);
|
|
|
|
|
|
|
|
// Waiting for reception
|
|
|
|
pthread_mutex_lock(&posPolling);
|
|
|
|
pthread_mutex_unlock(&posPolling);
|
|
|
|
|
|
|
|
// Calculation
|
|
|
|
nbCalcPos++;
|
|
|
|
lCodTot += deltaCoders.dL;
|
|
|
|
rCodTot += deltaCoders.dR;
|
|
|
|
}
|
|
|
|
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
void onF2CI_CODER()
|
|
|
|
{
|
|
|
|
readCF(&deltaCoders, sizeof(struct F2CI_CODERs));
|
|
|
|
pthread_mutex_unlock(&posPolling);
|
|
|
|
}
|
|
|
|
|
|
|
|
void configurePosition()
|
|
|
|
{
|
|
|
|
registerRxHandler(F2CI_CODER, onF2CI_CODER);
|
2018-04-29 09:38:49 +02:00
|
|
|
registerDebugVar("lCodTot", ld, &lCodTot);
|
|
|
|
registerDebugVar("rCodTot", ld, &rCodTot);
|
|
|
|
registerDebugVar("nbCalcPos", d, &nbCalcPos);
|
|
|
|
pthread_create(&tPosition, NULL, TaskPosition, NULL);
|
2018-04-04 16:17:13 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
void deconfigurePosition()
|
|
|
|
{
|
|
|
|
pthread_cancel(tPosition);
|
|
|
|
}
|