From 4195842e8574b42b9e498de675f29018255cab33 Mon Sep 17 00:00:00 2001 From: Geoffrey Frogeye Date: Wed, 9 May 2018 09:57:11 +0200 Subject: [PATCH] Rate limiter --- chef/src/debug.c | 38 +++++---- chef/src/diagnostics.c | 1 + chef/src/dimensions.h | 2 +- chef/src/imu.c | 2 +- chef/src/motor.c | 162 ++++++++++++++++++++++++++++++++------- chef/src/motor.h | 17 +++- chef/src/movement.c | 14 ++-- chef/src/movement.h | 2 + chef/src/parcours.c | 1 + chef/src/parcours.h | 1 + chef/src/securite.c | 2 +- chef/src/test1m.c | 68 ++++++++++++++++ chef/src/testCodeuse.c | 3 +- chef/src/testForward.c | 1 + chef/src/testFree.c | 1 + chef/src/testPuissance.c | 26 +++++++ chef/src/testSlow.c | 1 + chef/src/testStop.c | 1 + chef/src/testUpDown.c | 17 ++++ 19 files changed, 305 insertions(+), 55 deletions(-) create mode 100644 chef/src/test1m.c create mode 100644 chef/src/testPuissance.c diff --git a/chef/src/debug.c b/chef/src/debug.c index ccb5677..3f12933 100644 --- a/chef/src/debug.c +++ b/chef/src/debug.c @@ -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 + } } diff --git a/chef/src/diagnostics.c b/chef/src/diagnostics.c index c339b79..042b8c8 100644 --- a/chef/src/diagnostics.c +++ b/chef/src/diagnostics.c @@ -97,6 +97,7 @@ bool diagJustRun(void* arg) { void (*fonction)(void) = arg; fonction(); + usleep(1000*1000); return true; } diff --git a/chef/src/dimensions.h b/chef/src/dimensions.h index 4ff3a82..31a3fc7 100644 --- a/chef/src/dimensions.h +++ b/chef/src/dimensions.h @@ -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 diff --git a/chef/src/imu.c b/chef/src/imu.c index 915706a..3a2eddc 100644 --- a/chef/src/imu.c +++ b/chef/src/imu.c @@ -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; diff --git a/chef/src/motor.c b/chef/src/motor.c index 01d3f6c..7ee0162 100644 --- a/chef/src/motor.c +++ b/chef/src/motor.c @@ -1,5 +1,35 @@ -#include "motor.h" +#include + #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() diff --git a/chef/src/motor.h b/chef/src/motor.h index fe07247..da04afd 100644 --- a/chef/src/motor.h +++ b/chef/src/motor.h @@ -4,12 +4,16 @@ #include #include -#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 diff --git a/chef/src/movement.c b/chef/src/movement.c index 8f90dab..44572b1 100644 --- a/chef/src/movement.c +++ b/chef/src/movement.c @@ -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() diff --git a/chef/src/movement.h b/chef/src/movement.h index 1183d8b..8f48d6c 100644 --- a/chef/src/movement.h +++ b/chef/src/movement.h @@ -7,6 +7,8 @@ #define ANGLE_INSIGNIFIANT NAN +// #define ENABLE_SECURITE + #include "position.h" void configureMovement(); diff --git a/chef/src/parcours.c b/chef/src/parcours.c index 613043f..28e2776 100644 --- a/chef/src/parcours.c +++ b/chef/src/parcours.c @@ -168,3 +168,4 @@ void* TaskParcours(void* pdata) return NULL; } + diff --git a/chef/src/parcours.h b/chef/src/parcours.h index 3978810..5879d2e 100644 --- a/chef/src/parcours.h +++ b/chef/src/parcours.h @@ -45,5 +45,6 @@ void startParcours(); int updateParcours(); void stopParcours(); void* TaskParcours(void* pdata); +void gotoPoint(float x, float y, float o); #endif diff --git a/chef/src/securite.c b/chef/src/securite.c index 09d174e..2db5fa8 100644 --- a/chef/src/securite.c +++ b/chef/src/securite.c @@ -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); diff --git a/chef/src/test1m.c b/chef/src/test1m.c new file mode 100644 index 0000000..0eaf330 --- /dev/null +++ b/chef/src/test1m.c @@ -0,0 +1,68 @@ +#include +#include +#include +#include +#include // random seed +#include // sleep +#include + +#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; +} diff --git a/chef/src/testCodeuse.c b/chef/src/testCodeuse.c index 60a9dba..8e1bd1e 100644 --- a/chef/src/testCodeuse.c +++ b/chef/src/testCodeuse.c @@ -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); diff --git a/chef/src/testForward.c b/chef/src/testForward.c index dd72b99..ed7c734 100644 --- a/chef/src/testForward.c +++ b/chef/src/testForward.c @@ -26,6 +26,7 @@ int main(int argc, char* argv[]) initLCD(); configureCF(); configureButtons(); + configureMotor(); for (;;) { clearLCD(); diff --git a/chef/src/testFree.c b/chef/src/testFree.c index 9bec114..2430937 100644 --- a/chef/src/testFree.c +++ b/chef/src/testFree.c @@ -12,5 +12,6 @@ int main(int argc, char* argv[]) (void)argv; configureCF(); + configureMotor(); freewheel(); } diff --git a/chef/src/testPuissance.c b/chef/src/testPuissance.c new file mode 100644 index 0000000..7a347ba --- /dev/null +++ b/chef/src/testPuissance.c @@ -0,0 +1,26 @@ +#include +#include + +#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(); +} diff --git a/chef/src/testSlow.c b/chef/src/testSlow.c index 94e31eb..9807f17 100644 --- a/chef/src/testSlow.c +++ b/chef/src/testSlow.c @@ -36,6 +36,7 @@ int main(int argc, char* argv[]) initLCD(); configureCF(); configureButtons(); + configureMotor(); for (;;) { setPWMTensionWrapper(VITL, VITR); diff --git a/chef/src/testStop.c b/chef/src/testStop.c index 2d2a878..0bc7782 100644 --- a/chef/src/testStop.c +++ b/chef/src/testStop.c @@ -12,6 +12,7 @@ int main(int argc, char* argv[]) (void)argv; configureCF(); + configureMotor(); stop(); } diff --git a/chef/src/testUpDown.c b/chef/src/testUpDown.c index 5e60580..4ed3700 100644 --- a/chef/src/testUpDown.c +++ b/chef/src/testUpDown.c @@ -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); } }