mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-05-17 04:26:00 +02:00
148 lines
3.2 KiB
C
148 lines
3.2 KiB
C
/*
|
|
* Fonctions de calcul de la position du robot
|
|
*/
|
|
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <time.h>
|
|
#include <unistd.h>
|
|
|
|
#include "debug.h"
|
|
#include "dimensions.h"
|
|
#include "fpga.h"
|
|
#include "position.h"
|
|
|
|
// Globales
|
|
struct position connu;
|
|
pthread_mutex_t posConnu;
|
|
pthread_cond_t newPos;
|
|
pthread_t tPosition;
|
|
|
|
// Globales
|
|
unsigned int nbCalcPos;
|
|
long lCodTot, rCodTot;
|
|
|
|
int16_t oldL, oldR;
|
|
int16_t newL, newR;
|
|
int16_t deltaL, deltaR;
|
|
|
|
void updateDelta()
|
|
{
|
|
newL = (readI2C(fdFPGA(), CODER_LEFT_H) << 8 | readI2C(fdFPGA(), CODER_LEFT_L));
|
|
newR = (readI2C(fdFPGA(), CODER_RIGHT_H) << 8 | readI2C(fdFPGA(), CODER_RIGHT_L));
|
|
deltaL = (abs(oldL - newL) < UINT16_MAX/2) ? newL - oldL : UINT16_MAX - oldL + newL;
|
|
deltaR = (abs(oldR - newR) < UINT16_MAX/2) ? newR - oldR : UINT16_MAX - oldR + newR;
|
|
oldL = newL;
|
|
oldR = newR;
|
|
}
|
|
|
|
void* TaskPosition(void* pData)
|
|
{
|
|
(void)pData;
|
|
|
|
resetPosition();
|
|
nbCalcPos = 0;
|
|
lCodTot = 0;
|
|
rCodTot = 0;
|
|
oldL = 0;
|
|
oldR = 0;
|
|
|
|
updateDelta();
|
|
|
|
for (;;) {
|
|
|
|
updateDelta();
|
|
|
|
// Calculation
|
|
#ifdef INVERSE_L_CODER
|
|
deltaL = -deltaL;
|
|
#endif
|
|
#ifdef INVERSE_R_CODER
|
|
deltaR = -deltaR;
|
|
#endif
|
|
lCodTot += deltaL;
|
|
rCodTot += deltaR;
|
|
|
|
float dR = deltaR * AV_PER_CYCLE;
|
|
float dL = deltaL * AV_PER_CYCLE;
|
|
|
|
float deltaO = atan2(dR - dL, DISTANCE_BETWEEN_WHEELS);
|
|
float deltaD = (dL + dR) / 2;
|
|
|
|
pthread_mutex_lock(&posConnu);
|
|
connu.o += deltaO;
|
|
float deltaX = deltaD * cos(connu.o);
|
|
float deltaY = deltaD * sin(connu.o);
|
|
connu.x += deltaX;
|
|
connu.y += deltaY;
|
|
nbCalcPos++;
|
|
pthread_cond_signal(&newPos);
|
|
pthread_mutex_unlock(&posConnu);
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
void configurePosition()
|
|
{
|
|
pthread_mutex_init(&posConnu, NULL);
|
|
pthread_cond_init(&newPos, NULL);
|
|
registerDebugVar("lCodTot", ld, &lCodTot);
|
|
registerDebugVar("rCodTot", ld, &rCodTot);
|
|
registerDebugVar("newL", ld, &newL);
|
|
registerDebugVar("newR", ld, &newR);
|
|
connu.x = 0;
|
|
connu.y = 0;
|
|
connu.o = 0;
|
|
registerDebugVar("xConnu", f, &connu.x);
|
|
registerDebugVar("yConnu", f, &connu.y);
|
|
registerDebugVar("oConnu", f, &connu.o);
|
|
registerDebugVar("nbCalcPos", d, &nbCalcPos);
|
|
pthread_create(&tPosition, NULL, TaskPosition, NULL);
|
|
}
|
|
|
|
void deconfigurePosition()
|
|
{
|
|
pthread_cancel(tPosition);
|
|
}
|
|
|
|
void getCoders(long* l, long* r)
|
|
{
|
|
*l = lCodTot;
|
|
*r = rCodTot;
|
|
}
|
|
|
|
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);
|
|
}
|