Réécriture du système de consigne

chef/src/motor.c

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

chef/src/motor.h

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

chef/src/movement.c

@@ -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);
 
-        // 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);
-
-        // 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);
-
-        // 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;
-
-        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;
-
-        // 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);
-        clock_gettime(CLOCK_REALTIME, &pidNow);
-        movDoneBool = !oRetenu && fabs(oEcart) < O_ECART_MAX;
-        if (movDoneBool) {
-            pthread_cond_signal(&movDoneCond);
-        }
-        pthread_mutex_unlock(&movDoneMutex);
-
-        // 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;
-
-        // Si on est en avancement on applique une grande priorité au retour sur la ligne
-        if (!dRetenu && oRetenu) {
-            oErrRev *= O_GAIN;
-        }
-
-        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
+            float oErrRev = oDirEcart * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER;
+            float lVolt = -oErrRev * P;
+            float rVolt = oErrRev * P;
             setMoteurTension(lVolt, rVolt);
 
             nbCalcCons++;
-        pthread_mutex_unlock(&movEnableMutex);
+        } while (fabsf(oDirEcart) > O_DIR_ECART_MIN);
+        brake();
+        usleep(500 * 1000);
+
+        printf("Avance dir\n");
+        // Avance vers direction
+        do {
+
+            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);
+
+#ifdef ENABLE_SECURITE
+            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
+
+            float dErrRev = dErr / WHEEL_PERIMETER;
+            float oErrRev = O_GAIN * oDirEcart * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER;
+
+            lErr = dErrRev - oErrRev;
+            rErr = dErrRev + oErrRev;
+            float lVolt = lErr * P;
+            float rVolt = rErr * P;
+            setMoteurTension(lVolt, rVolt);
+
+            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);
+        }
+
+        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);
-}

chef/src/movement.h

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

chef/src/parcours.c

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

chef/src/test1m.c

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

chef/src/testAvance.c

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

chef/src/testOrange.c

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

chef/src/testRecule.c

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

chef/src/testRot.c

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

chef/src/testSecu.c

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