geoffrey/cdf2018-principal
geoffrey/cdf2018-principal
1
0
Fork 0
mirror of https://github.com/RobotechLille/cdf2018-principal synced 2024-11-13 03:46:12 +01:00
Code Issues Releases Wiki Activity

Réécriture du système de consigne

Browse source
This commit is contained in:
Geoffrey Frogeye 2018-05-10 10:08:56 +02:00
parent 63bf4ee3c3
commit 9802230d13
11 changed files with 438 additions and 179 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
chef/src/motor.c
View file

@ -127,6 +127,7 @@ void* TaskMotor(void* pData)
r = MOTOR_SATURATION_MAX;
}
#ifdef ENABLE_RATE_LIMITER
// Rate limiter pour éviter que l'accélération soit trop brusque
float maxUp = RATE_LIMITER_UP * dt;
float maxDown = RATE_LIMITER_UP * dt;
@ -141,6 +142,7 @@ void* TaskMotor(void* pData)
} else if (rVolt - r > maxDown) {
r = rVolt - maxDown;
}
#endif
setMoteurTensionRaw(l, r, lFor, rFor);
break;
@ -193,7 +195,6 @@ int brake()
pthread_mutex_lock(&motCons);
motState = braking;
pthread_mutex_unlock(&motCons);
printf("192 Brake\n");
rawBrake();
}

4
chef/src/motor.h
View file

@ -10,9 +10,11 @@
#define INVERSE_L_MOTOR
// #define INVERSE_R_MOTOR
#define ENABLE_RATE_LIMITER
// V/s
#define RATE_LIMITER_UP 6
#define RATE_LIMITER_DOWN 1
#define RATE_LIMITER_DOWN 24
#define TESTINATOR
// #define TLE5206

235
chef/src/movement.c
View file

@ -12,13 +12,11 @@
pthread_t tMovement;
struct position cons;
pthread_mutex_t movCons;
pthread_mutex_t movEnableMutex;
pthread_cond_t movEnableCond;
bool movEnableBool;
pthread_mutex_t movDoneMutex;
pthread_cond_t movDoneCond;
bool movDoneBool;
// Si une nouvelle instruction est disponible
pthread_mutex_t movInstructionMutex;
pthread_cond_t movInstructionCond;
bool movInstructionBool;
float xDiff;
float yDiff;
@ -35,10 +33,6 @@ float lErrPrev;
float rErrPrev;
unsigned int nbCalcCons;
struct timespec pidStart;
struct timespec pidNow;
struct timespec pidEcoule;
void configureMovement()
{
stop();
@ -47,14 +41,10 @@ void configureMovement()
configureSecurite();
nbCalcCons = 0;
pthread_mutex_init(&movCons, NULL);
pthread_mutex_init(&movEnableMutex, NULL);
pthread_cond_init(&movEnableCond, NULL);
movEnableBool = false;
pthread_mutex_init(&movDoneMutex, NULL);
pthread_cond_init(&movDoneCond, NULL);
movDoneBool = false;
pthread_mutex_init(&movInstructionMutex, NULL);
pthread_cond_init(&movInstructionCond, NULL);
movInstructionBool = false;
pthread_create(&tMovement, NULL, TaskMovement, NULL);
@ -73,24 +63,24 @@ void configureMovement()
registerDebugVar("lErr", f, &lErr);
registerDebugVar("rErr", f, &rErr);
registerDebugVar("nbCalcCons", d, &nbCalcCons);
disableConsigne();
}
void setDestination(struct position* pos)
{
pthread_mutex_lock(&movCons);
pthread_mutex_lock(&movInstructionMutex);
memcpy(&cons, pos, sizeof(struct position));
pthread_mutex_unlock(&movCons);
movInstructionBool = true;
pthread_cond_signal(&movInstructionCond);
pthread_mutex_unlock(&movInstructionMutex);
}
void waitDestination()
{
pthread_mutex_lock(&movDoneMutex);
while (!movDoneBool) {
pthread_cond_wait(&movDoneCond, &movDoneMutex);
pthread_mutex_lock(&movInstructionMutex);
while (movInstructionBool) {
pthread_cond_wait(&movInstructionCond, &movInstructionMutex);
}
pthread_mutex_unlock(&movDoneMutex);
pthread_mutex_unlock(&movInstructionMutex);
}
float angleGap(float target, float actual)
@ -114,126 +104,90 @@ void* TaskMovement(void* pData)
unsigned int lastPosCalc = 0;
struct position connu;
oRetenu = true;
dRetenu = true;
float lErrInteg = 0;
float rErrInteg = 0;
clock_gettime(CLOCK_REALTIME, &pidStart);
for (;;) {
// Attend instruction
printf("Wait instruction\n");
pthread_mutex_lock(&movInstructionMutex);
while (!movInstructionBool) {
pthread_cond_wait(&movInstructionCond, &movInstructionMutex);
} // Début de l'instruction
// Test if enabled
pthread_mutex_lock(&movEnableMutex);
while (!movEnableBool) {
pthread_cond_wait(&movEnableCond, &movEnableMutex);
}
printf("Oriente dir\n");
// Oriente vers direction
// TODO Marche arrière
do {
// Wait for new calculation if not calculated yet
lastPosCalc = getPositionNewer(&connu, lastPosCalc);
lastPosCalc = getPositionNewer(&connu, lastPosCalc);
// Destination → ordre
bool angleInsignifiant = isnan(cons.o);
pthread_mutex_lock(&movCons);
xDiff = cons.x - connu.x;
yDiff = cons.y - connu.y;
oEcart = angleInsignifiant ? 0 : angleGap(cons.o, connu.o);
xDiff = cons.x - connu.x;
yDiff = cons.y - connu.y;
oDirEcart = angleGap(atan2(yDiff, xDiff), connu.o);
float oErrRev = oDirEcart * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER;
float lVolt = -oErrRev * P;
float rVolt = oErrRev * P;
setMoteurTension(lVolt, rVolt);
// Distance d'écart entre la position actuelle et celle de consigne
dDirEcart = hypotf(xDiff, yDiff);
// É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); */
oDirEcart = angleGap(atan2(yDiff, xDiff), connu.o);
pthread_mutex_unlock(&movCons);
nbCalcCons++;
} while (fabsf(oDirEcart) > O_DIR_ECART_MIN);
brake();
usleep(500 * 1000);
// Si on est loin de la consigne, l'angle cible devient celui orienté vers la consigne
float dDirEcartAbs = fabsf(dDirEcart);
if (dDirEcartAbs > D_DIR_ECART_MAX) {
oRetenu = true;
// Si on est proche de la consigne, l'angle cible devient celui voulu par la consigne
} else if (dDirEcartAbs < D_DIR_ECART_MIN) {
oRetenu = false;
}
oErr = oRetenu ? oDirEcart : oEcart;
printf("Avance dir\n");
// Avance vers direction
do {
float oDirEcartAbs = fabsf(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;
lastPosCalc = getPositionNewer(&connu, lastPosCalc);
xDiff = cons.x - connu.x;
yDiff = cons.y - connu.y;
dDirEcart = hypotf(xDiff, yDiff);
oDirEcart = angleGap(atan2(yDiff, xDiff), connu.o);
// Limitation par les valeurs des capteurs
#ifdef ENABLE_SECURITE
float avant, arriere;
getDistance(&avant, &arriere);
dErr = fmaxf(-arriere, fminf(avant, dErr));
float distDevant, distDerriere;
getDistance(&distDevant, &distDerriere);
float maxEcart = fmax(0, distDevant - SECURITE_MARGE);
float minEcart = fmin(0, -distDerriere + SECURITE_MARGE);
dErr = fmin(maxEcart, fmax(minEcart, dDirEcart));
#else
dErr = dDirEcart;
#endif
// Calcul si on est arrivé
pthread_mutex_lock(&movDoneMutex);
clock_gettime(CLOCK_REALTIME, &pidNow);
movDoneBool = !oRetenu && fabs(oEcart) < O_ECART_MAX;
if (movDoneBool) {
pthread_cond_signal(&movDoneCond);
}
pthread_mutex_unlock(&movDoneMutex);
float dErrRev = dErr / WHEEL_PERIMETER;
float oErrRev = O_GAIN * oDirEcart * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER;
// 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;
// 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;
lErr = dErrRev - oErrRev;
rErr = dErrRev + oErrRev;
float lVolt = lErr * P;
float rVolt = rErr * P;
setMoteurTension(lVolt, rVolt);
// Si on est en avancement on applique une grande priorité au retour sur la ligne
if (!dRetenu && oRetenu) {
oErrRev *= O_GAIN;
nbCalcCons++;
} while (fabsf(dDirEcart) > D_DIR_ECART_MIN);
brake();
usleep(500 * 1000);
printf("Orientation finale\n");
// Orientation finale (si nécessaire)
if (!isnan(cons.o)) {
do {
lastPosCalc = getPositionNewer(&connu, lastPosCalc);
oDirEcart = angleGap(cons.o, connu.o);
float oErrRev = oDirEcart * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER;
float lVolt = -oErrRev * P;
float rVolt = oErrRev * P;
setMoteurTension(lVolt, rVolt);
nbCalcCons++;
} while (fabsf(oDirEcart) > O_DIR_ECART_MIN);
brake();
usleep(500 * 1000);
}
lErr = dErrRev - oErrRev;
rErr = dErrRev + oErrRev;
// PID
// 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 + pidEcoule.tv_nsec * 1E-9;
// 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
float lVolt = P * lErr + I * lErrInteg + D * lErrDeriv;
float rVolt = P * rErr + I * rErrInteg + D * rErrDeriv;
// Envoi de la commande
setMoteurTension(lVolt, rVolt);
nbCalcCons++;
pthread_mutex_unlock(&movEnableMutex);
movInstructionBool = false; // Fin de l'instruction
pthread_cond_signal(&movInstructionCond);
pthread_mutex_unlock(&movInstructionMutex);
}
return NULL;
@ -246,22 +200,3 @@ void deconfigureMovement()
deconfigureSecurite();
deconfigureMotor();
}
void enableConsigne()
{
printf("251 enableConsigne\n");
pthread_mutex_lock(&movEnableMutex);
clock_gettime(CLOCK_REALTIME, &pidNow);
movEnableBool = true;
pthread_cond_signal(&movEnableCond);
pthread_mutex_unlock(&movEnableMutex);
}
void disableConsigne()
{
printf("261 disableConsigne\n");
pthread_mutex_lock(&movEnableMutex);
movEnableBool = false;
// No signal here, will be disabled on next run
pthread_mutex_unlock(&movEnableMutex);
}

7
chef/src/movement.h
View file

@ -7,7 +7,10 @@
#define ANGLE_INSIGNIFIANT NAN
// #define ENABLE_SECURITE
#define ENABLE_SECURITE
#define SECURITE_MARGE 300
#include "position.h"
@ -15,8 +18,6 @@ void configureMovement();
void deconfigureMovement();
void setDestination(struct position* pos);
void* TaskMovement(void* pData);
void enableConsigne();
void disableConsigne();
void waitDestination();
#endif

52
chef/src/parcours.c
View file

@ -8,6 +8,7 @@
#include "motor.h"
#include "parcours.h"
#include "points.h"
#include "securite.h"
#include "position.h"
pthread_t tParcours;
@ -34,7 +35,6 @@ void prepareParcours(bool orange)
printRightLCD(LCD_LINE_2, isOrange ? "Org" : "Vrt");
resetActionneurs();
enableConsigne();
}
void startParcours()
@ -73,7 +73,6 @@ int updateParcours()
void stopParcours()
{
pthread_cancel(tParcours);
disableConsigne();
stop();
resetLCD();
@ -85,17 +84,18 @@ void stopParcours()
void gotoPoint(float x, float y, float o)
{
if (isOrange) {
x = M_PISTE_WIDTH - x;
if (!isnan(o)) {
o = M_PI - o;
}
/* if (isOrange) { */
/* x = M_PISTE_WIDTH - x; */
/* if (!isnan(o)) { */
/* o = M_PI - o; */
/* } */
/* } */
if (!isOrange) {
o = -o;
}
enableConsigne();
struct position pos = { x, y, o };
setDestination(&pos);
waitDestination();
disableConsigne();
brake();
}
@ -112,6 +112,40 @@ void* TaskParcours(void* pdata)
{
(void)pdata;
float vitBase = 1.5;
float secuBase = 500;
for (int i = 0; i <= 3000; i++) {
float av, ar;
getDistance(&av, &ar);
if (av < secuBase || ar < secuBase) {
brake();
} else {
setMoteurTension(vitBase, vitBase);
}
usleep(1000);
}
return NULL;
}
void* TaskParcoursLoquet(void* pdata)
{
(void)pdata;
gotoPoint(350, 0, 1.05*M_PI/3.0);
for (int i = 0; i < 5; i++) {
setLoquet(false);
setLoquet(true);
}
gotoPoint(0, 0, 0);
return NULL;
}
void* TaskParcoursVrai(void* pdata)
{
(void)pdata;
// Récupération des balles
gotoPoint(X_RECUP_1, Y_RECUP_1, O_RECUP_1);
setLoquet(false);

19
chef/src/test1m.c
View file

@ -37,28 +37,21 @@ int main()
configureDebug();
configureCF();
configureIMU();
printf("40\n");
configureActionneurs();
printf("41\n");
configurePosition();
printf("44\n");
configureMovement();
printf("46\n");
startDebug();
printf("48\n");
debugSetActive(true);
sleep(1);
printf("46\n");
enableConsigne();
struct position pos = { 0, -100, -M_PI };
/* struct position pos = {350, 0, -0.95*M_PI/3.0 }; */
struct position pos = {100000, 0, 0 };
setDestination(&pos);
printf("50\n");
waitDestination();
printf("52\n");
disableConsigne();
printf("54\n");
brake();
for (;;) {
setLoquet(false);
setLoquet(true);
}
printf("Done\n");
// Bloque jusqu'à l'arrivée d'un signal

72
chef/src/testAvance.c Normal file
View file

@ -0,0 +1,72 @@
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h> // random seed
#include <unistd.h> // sleep
#include <wiringPi.h>
#include "CF.h"
#include "actionneurs.h"
#include "debug.h"
#include "i2c.h"
#include "ihm.h"
#include "imu.h"
#include "movement.h"
#include "motor.h"
#include "position.h"
pthread_mutex_t sRunning;
void endRunning(int signal)
{
(void)signal;
pthread_mutex_unlock(&sRunning);
}
int main()
{
if (wiringPiSetup() < 0) {
fprintf(stderr, "Impossible d'initialiser WiringPi\n");
exit(EXIT_FAILURE);
}
initI2C();
srand(time(NULL));
configureDebug();
configureCF();
configureIMU();
configureActionneurs();
configurePosition();
configureMovement();
startDebug();
debugSetActive(true);
sleep(1);
/* struct position pos = {350, 0, -0.95*M_PI/3.0 }; */
struct position pos = {100000, 0, 0 };
setDestination(&pos);
waitDestination();
for (;;) {
setLoquet(false);
setLoquet(true);
}
printf("Done\n");
// Bloque jusqu'à l'arrivée d'un signal
pthread_mutex_init(&sRunning, NULL);
signal(SIGINT, endRunning);
signal(SIGTERM, endRunning);
signal(SIGQUIT, endRunning);
pthread_mutex_lock(&sRunning);
pthread_mutex_lock(&sRunning);
deconfigureMovement();
deconfigurePosition();
deconfigureActionneurs();
deconfigureIMU();
deconfigureCF();
deconfigureDebug();
return EXIT_SUCCESS;
}

71
chef/src/testOrange.c Normal file
View file

@ -0,0 +1,71 @@
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h> // random seed
#include <unistd.h> // sleep
#include <wiringPi.h>
#include "CF.h"
#include "actionneurs.h"
#include "debug.h"
#include "i2c.h"
#include "ihm.h"
#include "imu.h"
#include "movement.h"
#include "motor.h"
#include "position.h"
pthread_mutex_t sRunning;
void endRunning(int signal)
{
(void)signal;
pthread_mutex_unlock(&sRunning);
}
int main()
{
if (wiringPiSetup() < 0) {
fprintf(stderr, "Impossible d'initialiser WiringPi\n");
exit(EXIT_FAILURE);
}
initI2C();
srand(time(NULL));
configureDebug();
configureCF();
configureIMU();
configureActionneurs();
configurePosition();
configureMovement();
startDebug();
debugSetActive(true);
sleep(1);
struct position pos = {350, 0, -0.95*M_PI/3.0 };
setDestination(&pos);
sleep(3);
for (;;) {
setLoquet(false);
setLoquet(true);
}
printf("Done\n");
// Bloque jusqu'à l'arrivée d'un signal
pthread_mutex_init(&sRunning, NULL);
signal(SIGINT, endRunning);
signal(SIGTERM, endRunning);
signal(SIGQUIT, endRunning);
pthread_mutex_lock(&sRunning);
pthread_mutex_lock(&sRunning);
deconfigureMovement();
deconfigurePosition();
deconfigureActionneurs();
deconfigureIMU();
deconfigureCF();
deconfigureDebug();
return EXIT_SUCCESS;
}

72
chef/src/testRecule.c Normal file
View file

@ -0,0 +1,72 @@
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h> // random seed
#include <unistd.h> // sleep
#include <wiringPi.h>
#include "CF.h"
#include "actionneurs.h"
#include "debug.h"
#include "i2c.h"
#include "ihm.h"
#include "imu.h"
#include "movement.h"
#include "motor.h"
#include "position.h"
pthread_mutex_t sRunning;
void endRunning(int signal)
{
(void)signal;
pthread_mutex_unlock(&sRunning);
}
int main()
{
if (wiringPiSetup() < 0) {
fprintf(stderr, "Impossible d'initialiser WiringPi\n");
exit(EXIT_FAILURE);
}
initI2C();
srand(time(NULL));
configureDebug();
configureCF();
configureIMU();
configureActionneurs();
configurePosition();
configureMovement();
startDebug();
debugSetActive(true);
sleep(1);
/* struct position pos = {350, 0, -0.95*M_PI/3.0 }; */
struct position pos = {-100000, 0, 0 };
setDestination(&pos);
waitDestination();
for (;;) {
setLoquet(false);
setLoquet(true);
}
printf("Done\n");
// Bloque jusqu'à l'arrivée d'un signal
pthread_mutex_init(&sRunning, NULL);
signal(SIGINT, endRunning);
signal(SIGTERM, endRunning);
signal(SIGQUIT, endRunning);
pthread_mutex_lock(&sRunning);
pthread_mutex_lock(&sRunning);
deconfigureMovement();
deconfigurePosition();
deconfigureActionneurs();
deconfigureIMU();
deconfigureCF();
deconfigureDebug();
return EXIT_SUCCESS;
}

77
chef/src/testRot.c Normal file
View file

@ -0,0 +1,77 @@
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h> // random seed
#include <unistd.h> // sleep
#include <wiringPi.h>
#include "CF.h"
#include "actionneurs.h"
#include "debug.h"
#include "i2c.h"
#include "ihm.h"
#include "imu.h"
#include "movement.h"
#include "motor.h"
#include "position.h"
pthread_mutex_t sRunning;
void endRunning(int signal)
{
(void)signal;
pthread_mutex_unlock(&sRunning);
}
int main()
{
if (wiringPiSetup() < 0) {
fprintf(stderr, "Impossible d'initialiser WiringPi\n");
exit(EXIT_FAILURE);
}
initI2C();
srand(time(NULL));
configureDebug();
configureCF();
configureIMU();
printf("40\n");
configureActionneurs();
printf("41\n");
configurePosition();
printf("44\n");
configureMovement();
printf("46\n");
startDebug();
printf("48\n");
debugSetActive(true);
sleep(1);
printf("46\n");
struct position pos = { 0, 0, M_PI_2 };
setDestination(&pos);
printf("50\n");
waitDestination();
printf("52\n");
printf("54\n");
brake();
printf("Done\n");
// Bloque jusqu'à l'arrivée d'un signal
pthread_mutex_init(&sRunning, NULL);
signal(SIGINT, endRunning);
signal(SIGTERM, endRunning);
signal(SIGQUIT, endRunning);
pthread_mutex_lock(&sRunning);
pthread_mutex_lock(&sRunning);
deconfigureMovement();
deconfigurePosition();
deconfigureActionneurs();
deconfigureIMU();
deconfigureCF();
deconfigureDebug();
return EXIT_SUCCESS;
}

5
chef/src/testSecu.c
View file

@ -1,6 +1,7 @@
#include <stdlib.h>
#include <unistd.h>
#include <stdio.h>
#include <unistd.h>
#include <unistd.h>
#include "CF.h"
#include "securite.h"
@ -19,6 +20,6 @@ int main(int argc, char* argv[])
for (;;) {
getDistance(&f, &b);
printf("Av: %6f Ar: %6f\n", f, b);
sleep(1);
usleep(60 * 1000);
}
}