Rate limiter

chef/src/debug.c

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

chef/src/diagnostics.c

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

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
 

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;

chef/src/motor.c

@@ -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;
             }
 
-    // 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;
+            if (r < MOTOR_SATURATION_MIN) {
+                r = 0;
+            } else if (r > MOTOR_SATURATION_MAX) {
+                r = 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)
@@ -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()

chef/src/motor.h

@@ -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

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()

chef/src/movement.h

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

chef/src/parcours.c

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

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

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);

chef/src/test1m.c

@@ -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;
+}

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);
 

chef/src/testForward.c

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

chef/src/testFree.c

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

chef/src/testPuissance.c

@@ -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();
+}

chef/src/testSlow.c

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

chef/src/testStop.c

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

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);
     }
 
 }