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

Rate limiter

This commit is contained in:
Geoffrey Frogeye 2018-05-09 09:57:11 +02:00
parent 9cdebaa915
commit 4195842e85
19 changed files with 305 additions and 55 deletions

View file

@ -11,6 +11,7 @@
// Variables globales
pthread_t tDebug;
bool debugConfigured = false;
struct debugArg* listeDebugArgs = NULL;
@ -117,24 +118,28 @@ void configureDebug()
printf("Writing log in file: %s\n", path);
fprintf(debugFd, "time");
debugConfigured = true;
}
void registerDebugVar(char* name, enum debugArgTypes type, void* var)
{
fprintf(debugFd, ",%s", name);
if (debugConfigured) {
fprintf(debugFd, ",%s", name);
struct debugArg* arg = NULL;
struct debugArg** addrArg = &listeDebugArgs;
while (*addrArg != NULL) {
addrArg = &((*addrArg)->next);
struct debugArg* arg = NULL;
struct debugArg** addrArg = &listeDebugArgs;
while (*addrArg != NULL) {
addrArg = &((*addrArg)->next);
}
arg = malloc(sizeof(struct debugArg));
arg->type = type;
arg->var = var;
arg->next = NULL;
*addrArg = arg;
}
arg = malloc(sizeof(struct debugArg));
arg->type = type;
arg->var = var;
arg->next = NULL;
*addrArg = arg;
}
void startDebug()
@ -144,7 +149,10 @@ void startDebug()
void deconfigureDebug()
{
pthread_cancel(tDebug);
fclose(debugFd);
// TODO Vider la liste des arguments
if (debugConfigured) {
pthread_cancel(tDebug);
fclose(debugFd);
debugConfigured = false;
// TODO Vider la liste des arguments
}
}

View file

@ -97,6 +97,7 @@ bool diagJustRun(void* arg)
{
void (*fonction)(void) = arg;
fonction();
usleep(1000*1000);
return true;
}

View file

@ -40,7 +40,7 @@
#define D_CONS_THRESOLD 1.0 // mm
#define O_CONS_THRESOLD (6.0 / 360.0 * 2.0 * M_PI) // rad
#define O_GAIN 3.0
#define P 3.0
#define P 5.0
#define I 0.0
#define D 0.0

View file

@ -93,7 +93,7 @@ void* TaskIMU(void* pData)
}
imuStart.tv_sec = imuNow.tv_sec;
imuStart.tv_nsec = imuNow.tv_nsec;
float dt = imuEcoule.tv_sec + imuStart.tv_nsec * 1E-9;
float dt = imuEcoule.tv_sec + imuEcoule.tv_nsec * 1E-9;
gyroNew = readGyro();
gyro.x += gyroNew.x * dt;

View file

@ -1,5 +1,35 @@
#include "motor.h"
#include <unistd.h>
#include "actionneurs.h"
#include "debug.h"
#include "motor.h"
float lVolt;
float rVolt;
float lVoltCons;
float rVoltCons;
struct timespec motStart;
struct timespec motNow;
struct timespec motEcoule;
pthread_mutex_t motCons;
pthread_t tMotor;
enum motorState motState = braking;
static struct C2FD_PWMs msgBrake = { 0, 0, UINT8_MAX };
static struct C2FD_PWMs msgFree = { 0, 0, (1 << IN1) | (1 << IN2) | (1 << IN3) | (1 << IN4) };
void configureMotor()
{
registerDebugVar("lVolt", f, &lVolt);
registerDebugVar("rVolt", f, &rVolt);
pthread_mutex_init(&motCons, NULL);
pthread_create(&tMotor, NULL, TaskMotor, NULL);
}
void deconfigureMotor()
{
pthread_cancel(tMotor);
}
uint8_t tensionToPWM(float V)
{
@ -22,17 +52,19 @@ uint8_t moteurTensionToPWM(float V)
return tensionToPWM(V * MOTOR_CONTROLLER_REFERENCE / MOTOR_CONTROLLER_ALIMENTATION);
}
void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
void setMoteurTensionRaw(float l, float r, bool lFor, bool rFor)
{
lVolt = l;
rVolt = r;
#ifdef INVERSE_L_MOTOR
lFor = !lFor;
#endif
static struct C2FD_PWMs msg;
msg.in = 0x00;
if (lVolt > 0) {
if (l > 0) {
msg.in |= 1 << (lFor ? IN1 : IN2);
msg.ena = moteurTensionToPWM(lVolt);
msg.ena = moteurTensionToPWM(l);
} else {
// Nothing needs to be changed for this motor controller
}
@ -40,9 +72,9 @@ void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
#ifdef INVERSE_R_MOTOR
rFor = !rFor;
#endif
if (rVolt > 0) {
if (r > 0) {
msg.in |= 1 << (rFor ? IN3 : IN4);
msg.enb = moteurTensionToPWM(rVolt);
msg.enb = moteurTensionToPWM(r);
} else {
// Nothing needs to be changed for this motor controller
}
@ -50,28 +82,99 @@ void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
sendCF(C2FD_PWM, &msg, sizeof(struct C2FD_PWMs));
}
void setMoteurTension(float lVolt, float rVolt)
void* TaskMotor(void* pData)
{
(void)pData;
// Gauche
bool lFor = lVolt < 0;
lVolt = fabs(lVolt);
if (lVolt < MOTOR_SATURATION_MIN) {
lVolt = 0;
} else if (lVolt > MOTOR_SATURATION_MAX) {
lVolt = MOTOR_SATURATION_MAX;
float debug = 0;
clock_gettime(CLOCK_REALTIME, &motStart);
for (;;) {
// Calcul du temps écoulé depuis la dernière mise à jour des moteurs
clock_gettime(CLOCK_REALTIME, &motNow);
if ((motNow.tv_nsec - motStart.tv_nsec) < 0) {
motEcoule.tv_sec = motNow.tv_sec - motStart.tv_sec - 1;
motEcoule.tv_nsec = motNow.tv_nsec - motStart.tv_nsec + 1000000000L;
} else {
motEcoule.tv_sec = motNow.tv_sec - motStart.tv_sec;
motEcoule.tv_nsec = motNow.tv_nsec - motStart.tv_nsec;
}
motStart.tv_sec = motNow.tv_sec;
motStart.tv_nsec = motNow.tv_nsec;
float dt = motEcoule.tv_sec + (float) motEcoule.tv_nsec * 1E-9;
debug += dt;
pthread_mutex_lock(&motCons);
bool lFor = lVoltCons < 0;
float l = fabs(lVoltCons);
bool rFor = rVoltCons < 0;
float r = fabs(rVoltCons);
enum motorState state = motState;
pthread_mutex_unlock(&motCons);
switch (state) {
case running:
if (l < MOTOR_SATURATION_MIN) {
l = 0;
} else if (l > MOTOR_SATURATION_MAX) {
l = MOTOR_SATURATION_MAX;
}
if (r < MOTOR_SATURATION_MIN) {
r = 0;
} else if (r > MOTOR_SATURATION_MAX) {
r = MOTOR_SATURATION_MAX;
}
// Rate limiter pour éviter que l'accélération soit trop brusque
float maxUp = RATE_LIMITER_UP * dt;
float maxDown = RATE_LIMITER_UP * dt;
if (l - lVolt > maxUp) {
l = lVolt + maxUp;
} else if (lVolt - l > maxDown) {
l = lVolt - maxDown;
}
if (r - rVolt > maxUp) {
r = rVolt + maxUp;
} else if (rVolt - r > maxDown) {
r = rVolt - maxDown;
}
setMoteurTensionRaw(l, r, lFor, rFor);
break;
case braking:
rawBrake();
break;
case freewheeling:
rawFreewheel();
sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
break;
}
usleep(1000);
}
// Droite
bool rFor = rVolt < 0;
rVolt = fabs(rVolt);
if (rVolt < MOTOR_SATURATION_MIN) {
rVolt = 0;
} else if (rVolt > MOTOR_SATURATION_MAX) {
rVolt = MOTOR_SATURATION_MAX;
}
return NULL;
}
setMoteurTensionRaw(lVolt, rVolt, lFor, rFor);
void rawBrake()
{
sendCF(C2FD_PWM, &msgBrake, sizeof(struct C2FD_PWMs));
}
void rawFreewheel()
{
sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
}
void setMoteurTension(float l, float r)
{
pthread_mutex_lock(&motCons);
motState = running;
lVoltCons = l;
rVoltCons = r;
pthread_mutex_unlock(&motCons);
}
void setPWMTension(float lVolt, float rVolt)
@ -81,17 +184,20 @@ void setPWMTension(float lVolt, float rVolt)
rVolt * MOTOR_CONTROLLER_ALIMENTATION / MOTOR_CONTROLLER_REFERENCE);
}
static struct C2FD_PWMs msgBrake = { 0, 0, 0x00 };
static struct C2FD_PWMs msgFree = { 0, 0, (1 << IN1) | (1 << IN2) | (1 << IN3) | (1 << IN4) };
int brake()
{
sendCF(C2FD_PWM, &msgBrake, sizeof(struct C2FD_PWMs));
pthread_mutex_lock(&motCons);
motState = braking;
pthread_mutex_unlock(&motCons);
rawBrake();
}
int freewheel()
{
sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
pthread_mutex_lock(&motCons);
motState = freewheeling;
pthread_mutex_unlock(&motCons);
rawFreewheel();
}
int stop()

View file

@ -4,12 +4,16 @@
#include <math.h>
#include <stdint.h>
#include "dimensions.h"
#include "CF.h"
#include "dimensions.h"
#define INVERSE_L_MOTOR
// #define INVERSE_R_MOTOR
// V/s
#define RATE_LIMITER_UP 12
#define RATE_LIMITER_DOWN 1
#define TESTINATOR
// #define TLE5206
@ -18,7 +22,15 @@
#define IN3 2
#define IN4 3
enum motorState {
running,
braking,
freewheeling
};
// Public
void configureMotor();
void deconfigureMotor();
void setMoteurTension(float lVolt, float rVolt);
void setPWMTension(float lVolt, float rVolt);
int brake();
@ -26,7 +38,10 @@ int freewheel();
int stop();
// Private
void* TaskMotor(void* pData);
uint8_t moteurTensionToPWM(float V);
void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor);
void rawFreewheel();
void rawBrake();
#endif

View file

@ -33,8 +33,6 @@ float lErr;
float rErr;
float lErrPrev;
float rErrPrev;
float lVolt;
float rVolt;
unsigned int nbCalcCons;
struct timespec pidStart;
@ -45,6 +43,7 @@ void configureMovement()
{
stop();
configureMotor();
configureSecurite();
nbCalcCons = 0;
@ -73,8 +72,6 @@ void configureMovement()
registerDebugVar("oRetenu", d, &oRetenu);
registerDebugVar("lErr", f, &lErr);
registerDebugVar("rErr", f, &rErr);
registerDebugVar("lVolt", f, &lVolt);
registerDebugVar("rVolt", f, &rVolt);
registerDebugVar("nbCalcCons", d, &nbCalcCons);
disableConsigne();
@ -173,9 +170,11 @@ void* TaskMovement(void* pData)
dErr = dRetenu ? 0 : dDirEcart;
// Limitation par les valeurs des capteurs
#ifdef ENABLE_SECURITE
float avant, arriere;
getDistance(&avant, &arriere);
dErr = fmaxf(-arriere, fminf(avant, dErr));
#endif
// Calcul si on est arrivé
pthread_mutex_lock(&movDoneMutex);
@ -213,7 +212,7 @@ void* TaskMovement(void* pData)
// 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 + pidStart.tv_nsec * 1E-9;
float timeStep = pidEcoule.tv_sec + pidEcoule.tv_nsec * 1E-9;
// Calcul des facteurs dérivé et intégrale
lErrInteg += (lErr + lErrPrev) / 2 * timeStep;
@ -225,8 +224,8 @@ void* TaskMovement(void* pData)
rErrPrev = rErr;
// Calcul de la commande
lVolt = P * lErr + I * lErrInteg + D * lErrDeriv;
rVolt = P * rErr + I * rErrInteg + D * rErrDeriv;
float lVolt = P * lErr + I * lErrInteg + D * lErrDeriv;
float rVolt = P * rErr + I * rErrInteg + D * rErrDeriv;
// Envoi de la commande
setMoteurTension(lVolt, rVolt);
@ -242,6 +241,7 @@ void deconfigureMovement()
stop();
pthread_cancel(tMovement);
deconfigureSecurite();
deconfigureMotor();
}
void enableConsigne()

View file

@ -7,6 +7,8 @@
#define ANGLE_INSIGNIFIANT NAN
// #define ENABLE_SECURITE
#include "position.h"
void configureMovement();

View file

@ -168,3 +168,4 @@ void* TaskParcours(void* pdata)
return NULL;
}

View file

@ -45,5 +45,6 @@ void startParcours();
int updateParcours();
void stopParcours();
void* TaskParcours(void* pdata);
void gotoPoint(float x, float y, float o);
#endif

View file

@ -53,7 +53,7 @@ void configureSecurite()
{
pthread_mutex_init(&secPolling, NULL);
pthread_mutex_init(&secData, NULL);
registerRxHandlerCF(F2CI_CODER, onF2CI_CAPT);
registerRxHandlerCF(F2CI_CAPT, onF2CI_CAPT);
registerDebugVar("secFront", ld, &secFront);
registerDebugVar("secBack", ld, &secBack);
pthread_create(&tSecurite, NULL, TaskSecurite, NULL);

68
chef/src/test1m.c Normal file
View file

@ -0,0 +1,68 @@
#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);
enableConsigne();
struct position pos = { 100, 0, 0 };
setDestination(&pos);
waitDestination();
brake();
disableConsigne();
// 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;
}

View file

@ -16,7 +16,7 @@
pthread_mutex_t sRunning;
#define VIT 1
#define VIT 0
void endRunning(int signal)
{
@ -30,6 +30,7 @@ int main()
configureDebug();
configureCF();
configurePosition();
configureMotor();
setPWMTension(VIT, VIT);

View file

@ -26,6 +26,7 @@ int main(int argc, char* argv[])
initLCD();
configureCF();
configureButtons();
configureMotor();
for (;;) {
clearLCD();

View file

@ -12,5 +12,6 @@ int main(int argc, char* argv[])
(void)argv;
configureCF();
configureMotor();
freewheel();
}

26
chef/src/testPuissance.c Normal file
View file

@ -0,0 +1,26 @@
#include <stdlib.h>
#include <unistd.h>
#include "CF.h"
#include "motor.h"
#include "debug.h"
int main(int argc, char* argv[])
{
(void)argc;
(void)argv;
configureDebug();
configureCF();
configureMotor();
setMoteurTension(24, 24);
sleep(10);
setMoteurTension(0, 0);
sleep(3);
deconfigureMotor();
deconfigureCF();
deconfigureDebug();
}

View file

@ -36,6 +36,7 @@ int main(int argc, char* argv[])
initLCD();
configureCF();
configureButtons();
configureMotor();
for (;;) {
setPWMTensionWrapper(VITL, VITR);

View file

@ -12,6 +12,7 @@ int main(int argc, char* argv[])
(void)argv;
configureCF();
configureMotor();
stop();
}

View file

@ -38,6 +38,7 @@ int main(int argc, char* argv[])
initLCD();
configureCF();
configureButtons();
configureMotor();
for (;;) {
for (int i = 0; i < UP_TIME; i += INTERVAL) {
@ -56,6 +57,22 @@ int main(int argc, char* argv[])
}
changerMoteursWrapper(0, 0);
delay(LOW_TIME);
for (int i = 0; i < UP_TIME; i += INTERVAL) {
float p = (float)i / (float)UP_TIME;
changerMoteursWrapper(-p * MAXI, -p * MAXI);
delay(INTERVAL);
}
changerMoteursWrapper(-MAXI, -MAXI);
delay(HIGH_TIME);
for (int i = 0; i < DOWN_TIME; i += INTERVAL) {
float p = (float)i / (float)DOWN_TIME;
p = 1 - p;
changerMoteursWrapper(-p * MAXI, -p * MAXI);
delay(INTERVAL);
}
changerMoteursWrapper(0, 0);
delay(LOW_TIME);
}
}