mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-14 12:26:06 +01:00
Rate limiter
This commit is contained in:
parent
9cdebaa915
commit
4195842e85
|
@ -11,6 +11,7 @@
|
||||||
|
|
||||||
// Variables globales
|
// Variables globales
|
||||||
pthread_t tDebug;
|
pthread_t tDebug;
|
||||||
|
bool debugConfigured = false;
|
||||||
|
|
||||||
struct debugArg* listeDebugArgs = NULL;
|
struct debugArg* listeDebugArgs = NULL;
|
||||||
|
|
||||||
|
@ -117,10 +118,13 @@ void configureDebug()
|
||||||
printf("Writing log in file: %s\n", path);
|
printf("Writing log in file: %s\n", path);
|
||||||
|
|
||||||
fprintf(debugFd, "time");
|
fprintf(debugFd, "time");
|
||||||
|
|
||||||
|
debugConfigured = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void registerDebugVar(char* name, enum debugArgTypes type, void* var)
|
void registerDebugVar(char* name, enum debugArgTypes type, void* var)
|
||||||
{
|
{
|
||||||
|
if (debugConfigured) {
|
||||||
fprintf(debugFd, ",%s", name);
|
fprintf(debugFd, ",%s", name);
|
||||||
|
|
||||||
struct debugArg* arg = NULL;
|
struct debugArg* arg = NULL;
|
||||||
|
@ -135,6 +139,7 @@ void registerDebugVar(char* name, enum debugArgTypes type, void* var)
|
||||||
arg->next = NULL;
|
arg->next = NULL;
|
||||||
|
|
||||||
*addrArg = arg;
|
*addrArg = arg;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void startDebug()
|
void startDebug()
|
||||||
|
@ -144,7 +149,10 @@ void startDebug()
|
||||||
|
|
||||||
void deconfigureDebug()
|
void deconfigureDebug()
|
||||||
{
|
{
|
||||||
|
if (debugConfigured) {
|
||||||
pthread_cancel(tDebug);
|
pthread_cancel(tDebug);
|
||||||
fclose(debugFd);
|
fclose(debugFd);
|
||||||
|
debugConfigured = false;
|
||||||
// TODO Vider la liste des arguments
|
// TODO Vider la liste des arguments
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -97,6 +97,7 @@ bool diagJustRun(void* arg)
|
||||||
{
|
{
|
||||||
void (*fonction)(void) = arg;
|
void (*fonction)(void) = arg;
|
||||||
fonction();
|
fonction();
|
||||||
|
usleep(1000*1000);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -40,7 +40,7 @@
|
||||||
#define D_CONS_THRESOLD 1.0 // mm
|
#define D_CONS_THRESOLD 1.0 // mm
|
||||||
#define O_CONS_THRESOLD (6.0 / 360.0 * 2.0 * M_PI) // rad
|
#define O_CONS_THRESOLD (6.0 / 360.0 * 2.0 * M_PI) // rad
|
||||||
#define O_GAIN 3.0
|
#define O_GAIN 3.0
|
||||||
#define P 3.0
|
#define P 5.0
|
||||||
#define I 0.0
|
#define I 0.0
|
||||||
#define D 0.0
|
#define D 0.0
|
||||||
|
|
||||||
|
|
|
@ -93,7 +93,7 @@ void* TaskIMU(void* pData)
|
||||||
}
|
}
|
||||||
imuStart.tv_sec = imuNow.tv_sec;
|
imuStart.tv_sec = imuNow.tv_sec;
|
||||||
imuStart.tv_nsec = imuNow.tv_nsec;
|
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();
|
gyroNew = readGyro();
|
||||||
gyro.x += gyroNew.x * dt;
|
gyro.x += gyroNew.x * dt;
|
||||||
|
|
160
chef/src/motor.c
160
chef/src/motor.c
|
@ -1,5 +1,35 @@
|
||||||
#include "motor.h"
|
#include <unistd.h>
|
||||||
|
|
||||||
#include "actionneurs.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)
|
uint8_t tensionToPWM(float V)
|
||||||
{
|
{
|
||||||
|
@ -22,17 +52,19 @@ uint8_t moteurTensionToPWM(float V)
|
||||||
return tensionToPWM(V * MOTOR_CONTROLLER_REFERENCE / MOTOR_CONTROLLER_ALIMENTATION);
|
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
|
#ifdef INVERSE_L_MOTOR
|
||||||
lFor = !lFor;
|
lFor = !lFor;
|
||||||
#endif
|
#endif
|
||||||
static struct C2FD_PWMs msg;
|
static struct C2FD_PWMs msg;
|
||||||
msg.in = 0x00;
|
msg.in = 0x00;
|
||||||
if (lVolt > 0) {
|
if (l > 0) {
|
||||||
msg.in |= 1 << (lFor ? IN1 : IN2);
|
msg.in |= 1 << (lFor ? IN1 : IN2);
|
||||||
msg.ena = moteurTensionToPWM(lVolt);
|
msg.ena = moteurTensionToPWM(l);
|
||||||
} else {
|
} else {
|
||||||
// Nothing needs to be changed for this motor controller
|
// 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
|
#ifdef INVERSE_R_MOTOR
|
||||||
rFor = !rFor;
|
rFor = !rFor;
|
||||||
#endif
|
#endif
|
||||||
if (rVolt > 0) {
|
if (r > 0) {
|
||||||
msg.in |= 1 << (rFor ? IN3 : IN4);
|
msg.in |= 1 << (rFor ? IN3 : IN4);
|
||||||
msg.enb = moteurTensionToPWM(rVolt);
|
msg.enb = moteurTensionToPWM(r);
|
||||||
} else {
|
} else {
|
||||||
// Nothing needs to be changed for this motor controller
|
// 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));
|
sendCF(C2FD_PWM, &msg, sizeof(struct C2FD_PWMs));
|
||||||
}
|
}
|
||||||
|
|
||||||
void setMoteurTension(float lVolt, float rVolt)
|
void* TaskMotor(void* pData)
|
||||||
{
|
{
|
||||||
|
(void)pData;
|
||||||
|
|
||||||
// Gauche
|
float debug = 0;
|
||||||
bool lFor = lVolt < 0;
|
|
||||||
lVolt = fabs(lVolt);
|
clock_gettime(CLOCK_REALTIME, &motStart);
|
||||||
if (lVolt < MOTOR_SATURATION_MIN) {
|
for (;;) {
|
||||||
lVolt = 0;
|
// Calcul du temps écoulé depuis la dernière mise à jour des moteurs
|
||||||
} else if (lVolt > MOTOR_SATURATION_MAX) {
|
clock_gettime(CLOCK_REALTIME, &motNow);
|
||||||
lVolt = MOTOR_SATURATION_MAX;
|
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;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Droite
|
if (r < MOTOR_SATURATION_MIN) {
|
||||||
bool rFor = rVolt < 0;
|
r = 0;
|
||||||
rVolt = fabs(rVolt);
|
} else if (r > MOTOR_SATURATION_MAX) {
|
||||||
if (rVolt < MOTOR_SATURATION_MIN) {
|
r = MOTOR_SATURATION_MAX;
|
||||||
rVolt = 0;
|
|
||||||
} else if (rVolt > MOTOR_SATURATION_MAX) {
|
|
||||||
rVolt = MOTOR_SATURATION_MAX;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
setMoteurTensionRaw(lVolt, rVolt, lFor, rFor);
|
// 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);
|
||||||
|
}
|
||||||
|
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
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)
|
void setPWMTension(float lVolt, float rVolt)
|
||||||
|
@ -81,17 +184,20 @@ void setPWMTension(float lVolt, float rVolt)
|
||||||
rVolt * MOTOR_CONTROLLER_ALIMENTATION / MOTOR_CONTROLLER_REFERENCE);
|
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()
|
int brake()
|
||||||
{
|
{
|
||||||
sendCF(C2FD_PWM, &msgBrake, sizeof(struct C2FD_PWMs));
|
pthread_mutex_lock(&motCons);
|
||||||
|
motState = braking;
|
||||||
|
pthread_mutex_unlock(&motCons);
|
||||||
|
rawBrake();
|
||||||
}
|
}
|
||||||
|
|
||||||
int freewheel()
|
int freewheel()
|
||||||
{
|
{
|
||||||
sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
|
pthread_mutex_lock(&motCons);
|
||||||
|
motState = freewheeling;
|
||||||
|
pthread_mutex_unlock(&motCons);
|
||||||
|
rawFreewheel();
|
||||||
}
|
}
|
||||||
|
|
||||||
int stop()
|
int stop()
|
||||||
|
|
|
@ -4,12 +4,16 @@
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
#include "dimensions.h"
|
|
||||||
#include "CF.h"
|
#include "CF.h"
|
||||||
|
#include "dimensions.h"
|
||||||
|
|
||||||
#define INVERSE_L_MOTOR
|
#define INVERSE_L_MOTOR
|
||||||
// #define INVERSE_R_MOTOR
|
// #define INVERSE_R_MOTOR
|
||||||
|
|
||||||
|
// V/s
|
||||||
|
#define RATE_LIMITER_UP 12
|
||||||
|
#define RATE_LIMITER_DOWN 1
|
||||||
|
|
||||||
#define TESTINATOR
|
#define TESTINATOR
|
||||||
// #define TLE5206
|
// #define TLE5206
|
||||||
|
|
||||||
|
@ -18,7 +22,15 @@
|
||||||
#define IN3 2
|
#define IN3 2
|
||||||
#define IN4 3
|
#define IN4 3
|
||||||
|
|
||||||
|
enum motorState {
|
||||||
|
running,
|
||||||
|
braking,
|
||||||
|
freewheeling
|
||||||
|
};
|
||||||
|
|
||||||
// Public
|
// Public
|
||||||
|
void configureMotor();
|
||||||
|
void deconfigureMotor();
|
||||||
void setMoteurTension(float lVolt, float rVolt);
|
void setMoteurTension(float lVolt, float rVolt);
|
||||||
void setPWMTension(float lVolt, float rVolt);
|
void setPWMTension(float lVolt, float rVolt);
|
||||||
int brake();
|
int brake();
|
||||||
|
@ -26,7 +38,10 @@ int freewheel();
|
||||||
int stop();
|
int stop();
|
||||||
|
|
||||||
// Private
|
// Private
|
||||||
|
void* TaskMotor(void* pData);
|
||||||
uint8_t moteurTensionToPWM(float V);
|
uint8_t moteurTensionToPWM(float V);
|
||||||
void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor);
|
void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor);
|
||||||
|
void rawFreewheel();
|
||||||
|
void rawBrake();
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -33,8 +33,6 @@ float lErr;
|
||||||
float rErr;
|
float rErr;
|
||||||
float lErrPrev;
|
float lErrPrev;
|
||||||
float rErrPrev;
|
float rErrPrev;
|
||||||
float lVolt;
|
|
||||||
float rVolt;
|
|
||||||
unsigned int nbCalcCons;
|
unsigned int nbCalcCons;
|
||||||
|
|
||||||
struct timespec pidStart;
|
struct timespec pidStart;
|
||||||
|
@ -45,6 +43,7 @@ void configureMovement()
|
||||||
{
|
{
|
||||||
stop();
|
stop();
|
||||||
|
|
||||||
|
configureMotor();
|
||||||
configureSecurite();
|
configureSecurite();
|
||||||
|
|
||||||
nbCalcCons = 0;
|
nbCalcCons = 0;
|
||||||
|
@ -73,8 +72,6 @@ void configureMovement()
|
||||||
registerDebugVar("oRetenu", d, &oRetenu);
|
registerDebugVar("oRetenu", d, &oRetenu);
|
||||||
registerDebugVar("lErr", f, &lErr);
|
registerDebugVar("lErr", f, &lErr);
|
||||||
registerDebugVar("rErr", f, &rErr);
|
registerDebugVar("rErr", f, &rErr);
|
||||||
registerDebugVar("lVolt", f, &lVolt);
|
|
||||||
registerDebugVar("rVolt", f, &rVolt);
|
|
||||||
registerDebugVar("nbCalcCons", d, &nbCalcCons);
|
registerDebugVar("nbCalcCons", d, &nbCalcCons);
|
||||||
|
|
||||||
disableConsigne();
|
disableConsigne();
|
||||||
|
@ -173,9 +170,11 @@ void* TaskMovement(void* pData)
|
||||||
dErr = dRetenu ? 0 : dDirEcart;
|
dErr = dRetenu ? 0 : dDirEcart;
|
||||||
|
|
||||||
// Limitation par les valeurs des capteurs
|
// Limitation par les valeurs des capteurs
|
||||||
|
#ifdef ENABLE_SECURITE
|
||||||
float avant, arriere;
|
float avant, arriere;
|
||||||
getDistance(&avant, &arriere);
|
getDistance(&avant, &arriere);
|
||||||
dErr = fmaxf(-arriere, fminf(avant, dErr));
|
dErr = fmaxf(-arriere, fminf(avant, dErr));
|
||||||
|
#endif
|
||||||
|
|
||||||
// Calcul si on est arrivé
|
// Calcul si on est arrivé
|
||||||
pthread_mutex_lock(&movDoneMutex);
|
pthread_mutex_lock(&movDoneMutex);
|
||||||
|
@ -213,7 +212,7 @@ void* TaskMovement(void* pData)
|
||||||
// Enregistrement de cette mesure comme la dernière mesure
|
// Enregistrement de cette mesure comme la dernière mesure
|
||||||
pidStart.tv_sec = pidNow.tv_sec;
|
pidStart.tv_sec = pidNow.tv_sec;
|
||||||
pidStart.tv_nsec = pidNow.tv_nsec;
|
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
|
// Calcul des facteurs dérivé et intégrale
|
||||||
lErrInteg += (lErr + lErrPrev) / 2 * timeStep;
|
lErrInteg += (lErr + lErrPrev) / 2 * timeStep;
|
||||||
|
@ -225,8 +224,8 @@ void* TaskMovement(void* pData)
|
||||||
rErrPrev = rErr;
|
rErrPrev = rErr;
|
||||||
|
|
||||||
// Calcul de la commande
|
// Calcul de la commande
|
||||||
lVolt = P * lErr + I * lErrInteg + D * lErrDeriv;
|
float lVolt = P * lErr + I * lErrInteg + D * lErrDeriv;
|
||||||
rVolt = P * rErr + I * rErrInteg + D * rErrDeriv;
|
float rVolt = P * rErr + I * rErrInteg + D * rErrDeriv;
|
||||||
|
|
||||||
// Envoi de la commande
|
// Envoi de la commande
|
||||||
setMoteurTension(lVolt, rVolt);
|
setMoteurTension(lVolt, rVolt);
|
||||||
|
@ -242,6 +241,7 @@ void deconfigureMovement()
|
||||||
stop();
|
stop();
|
||||||
pthread_cancel(tMovement);
|
pthread_cancel(tMovement);
|
||||||
deconfigureSecurite();
|
deconfigureSecurite();
|
||||||
|
deconfigureMotor();
|
||||||
}
|
}
|
||||||
|
|
||||||
void enableConsigne()
|
void enableConsigne()
|
||||||
|
|
|
@ -7,6 +7,8 @@
|
||||||
|
|
||||||
#define ANGLE_INSIGNIFIANT NAN
|
#define ANGLE_INSIGNIFIANT NAN
|
||||||
|
|
||||||
|
// #define ENABLE_SECURITE
|
||||||
|
|
||||||
#include "position.h"
|
#include "position.h"
|
||||||
|
|
||||||
void configureMovement();
|
void configureMovement();
|
||||||
|
|
|
@ -168,3 +168,4 @@ void* TaskParcours(void* pdata)
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -45,5 +45,6 @@ void startParcours();
|
||||||
int updateParcours();
|
int updateParcours();
|
||||||
void stopParcours();
|
void stopParcours();
|
||||||
void* TaskParcours(void* pdata);
|
void* TaskParcours(void* pdata);
|
||||||
|
void gotoPoint(float x, float y, float o);
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -53,7 +53,7 @@ void configureSecurite()
|
||||||
{
|
{
|
||||||
pthread_mutex_init(&secPolling, NULL);
|
pthread_mutex_init(&secPolling, NULL);
|
||||||
pthread_mutex_init(&secData, NULL);
|
pthread_mutex_init(&secData, NULL);
|
||||||
registerRxHandlerCF(F2CI_CODER, onF2CI_CAPT);
|
registerRxHandlerCF(F2CI_CAPT, onF2CI_CAPT);
|
||||||
registerDebugVar("secFront", ld, &secFront);
|
registerDebugVar("secFront", ld, &secFront);
|
||||||
registerDebugVar("secBack", ld, &secBack);
|
registerDebugVar("secBack", ld, &secBack);
|
||||||
pthread_create(&tSecurite, NULL, TaskSecurite, NULL);
|
pthread_create(&tSecurite, NULL, TaskSecurite, NULL);
|
||||||
|
|
68
chef/src/test1m.c
Normal file
68
chef/src/test1m.c
Normal 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;
|
||||||
|
}
|
|
@ -16,7 +16,7 @@
|
||||||
|
|
||||||
pthread_mutex_t sRunning;
|
pthread_mutex_t sRunning;
|
||||||
|
|
||||||
#define VIT 1
|
#define VIT 0
|
||||||
|
|
||||||
void endRunning(int signal)
|
void endRunning(int signal)
|
||||||
{
|
{
|
||||||
|
@ -30,6 +30,7 @@ int main()
|
||||||
configureDebug();
|
configureDebug();
|
||||||
configureCF();
|
configureCF();
|
||||||
configurePosition();
|
configurePosition();
|
||||||
|
configureMotor();
|
||||||
|
|
||||||
setPWMTension(VIT, VIT);
|
setPWMTension(VIT, VIT);
|
||||||
|
|
||||||
|
|
|
@ -26,6 +26,7 @@ int main(int argc, char* argv[])
|
||||||
initLCD();
|
initLCD();
|
||||||
configureCF();
|
configureCF();
|
||||||
configureButtons();
|
configureButtons();
|
||||||
|
configureMotor();
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
clearLCD();
|
clearLCD();
|
||||||
|
|
|
@ -12,5 +12,6 @@ int main(int argc, char* argv[])
|
||||||
(void)argv;
|
(void)argv;
|
||||||
|
|
||||||
configureCF();
|
configureCF();
|
||||||
|
configureMotor();
|
||||||
freewheel();
|
freewheel();
|
||||||
}
|
}
|
||||||
|
|
26
chef/src/testPuissance.c
Normal file
26
chef/src/testPuissance.c
Normal 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();
|
||||||
|
}
|
|
@ -36,6 +36,7 @@ int main(int argc, char* argv[])
|
||||||
initLCD();
|
initLCD();
|
||||||
configureCF();
|
configureCF();
|
||||||
configureButtons();
|
configureButtons();
|
||||||
|
configureMotor();
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
setPWMTensionWrapper(VITL, VITR);
|
setPWMTensionWrapper(VITL, VITR);
|
||||||
|
|
|
@ -12,6 +12,7 @@ int main(int argc, char* argv[])
|
||||||
(void)argv;
|
(void)argv;
|
||||||
|
|
||||||
configureCF();
|
configureCF();
|
||||||
|
configureMotor();
|
||||||
stop();
|
stop();
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -38,6 +38,7 @@ int main(int argc, char* argv[])
|
||||||
initLCD();
|
initLCD();
|
||||||
configureCF();
|
configureCF();
|
||||||
configureButtons();
|
configureButtons();
|
||||||
|
configureMotor();
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
for (int i = 0; i < UP_TIME; i += INTERVAL) {
|
for (int i = 0; i < UP_TIME; i += INTERVAL) {
|
||||||
|
@ -56,6 +57,22 @@ int main(int argc, char* argv[])
|
||||||
}
|
}
|
||||||
changerMoteursWrapper(0, 0);
|
changerMoteursWrapper(0, 0);
|
||||||
delay(LOW_TIME);
|
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);
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in a new issue