WIP Contrôle

chef/src/dimensions.h

@@ -33,14 +33,13 @@
 #define AV_PER_CYCLE (WHEEL_PERIMETER / CODER_FULL_RESOLUTION) // mm
 
 // Constantes asservissement
-#define D_DIR_ECART_MIN 1.0 // mm
+#define D_DIR_ECART_MIN 7.0 // mm
-#define D_DIR_ECART_MAX 5.0 // mm
+#define D_DIR_ECART_MAX 10.0 // mm
-#define O_DIR_ECART_MIN (6.0 / 360.0 * 2.0 * M_PI) // rad
+#define O_DIR_ECART_MIN (5.0 / 360.0 * 2.0 * M_PI) // rad
-#define O_DIR_ECART_MAX (45.0 / 360.0 * 2.0 * M_PI) // rad
+#define O_DIR_ECART_MAX (25.0 / 360.0 * 2.0 * M_PI) // rad
-#define D_CONS_THRESOLD 1.0 // mm
+#define O_ECART_MAX (25.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 P 5.0
+#define P 7.0
 #define I 0.0
 #define D 0.0
 

chef/src/local.c

@@ -1,52 +1,16 @@
-#include <pthread.h>
+#include <math.h>
-#include <signal.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <time.h>   // random seed
-#include <unistd.h> // sleep
 
-pthread_mutex_t posConnu;
+float angleGap(float target, float actual)
-pthread_cond_t newPos;
-pthread_t tPosition;
-pthread_t tMovement;
-
-void* TaskPosition(void* pData)
 {
-
+    return remainderf(target - actual + M_PI, 2 * M_PI) - M_PI;
-    (void)pData;
-    for (;;) {
-        printf("Begin position\n");
-        sleep(1);
-
-        pthread_mutex_lock(&posConnu);
-        printf("Sending position\n");
-        pthread_cond_signal(&newPos);
-        pthread_mutex_unlock(&posConnu);
-    }
-    return NULL;
-}
-
-void* TaskMovement(void* pData)
-{
-    (void)pData;
-    for (;;) {
-        printf("Begin Movement\n");
-        sleep(3);
-
-        pthread_mutex_lock(&posConnu);
-        printf("Waiting movement\n");
-        pthread_cond_wait(&newPos, &posConnu);
-        pthread_mutex_unlock(&posConnu);
-
-    }
-    return NULL;
 }
 
 int main()
 {
-    pthread_mutex_init(&posConnu, NULL);
+    float cons = 2;
-    pthread_cond_init(&newPos, NULL);
+    for (float conn = 0; conn < 10.0; conn += 0.1) {
-    pthread_create(&tPosition, NULL, TaskPosition, NULL);
+        printf("%6f\n", angleGap(cons, conn));
-    pthread_create(&tMovement, NULL, TaskMovement, NULL);
+    }
-    sleep(300);
 }

chef/src/motor.c

@@ -161,10 +161,14 @@ void* TaskMotor(void* pData)
 
 void rawBrake()
 {
+    lVolt = 0;
+    rVolt = 0;
     sendCF(C2FD_PWM, &msgBrake, sizeof(struct C2FD_PWMs));
 }
 void rawFreewheel()
 {
+    lVolt = 0;
+    rVolt = 0;
     sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
 }
 
@@ -189,6 +193,7 @@ int brake()
     pthread_mutex_lock(&motCons);
     motState = braking;
     pthread_mutex_unlock(&motCons);
+    printf("192 Brake\n");
     rawBrake();
 }
 

chef/src/motor.h

@@ -11,7 +11,7 @@
 // #define INVERSE_R_MOTOR
 
 // V/s
-#define RATE_LIMITER_UP 12
+#define RATE_LIMITER_UP 6
 #define RATE_LIMITER_DOWN 1
 
 #define TESTINATOR

chef/src/movement.c

@@ -95,15 +95,16 @@ void waitDestination()
 
 float angleGap(float target, float actual)
 {
-    return fmod(target - actual + M_PI, 2 * M_PI) - M_PI;
+    float ret = fmodf(target - actual + M_PI, 2 * M_PI) - M_PI;
+    return ret;
 }
 
 float angleGap180(float target, float actual, float* dist)
 {
-    if (fabs(fmod(target - actual + M_PI, 2 * M_PI) - M_PI) > M_PI_2) {
+    if (fabs(fmodf(target - actual + M_PI, 2 * M_PI) - M_PI) > M_PI_2) {
         *dist = -*dist;
     }
-    return fmod(target - actual + M_PI_2, M_PI) - M_PI_2;
+    return fmodf(target - actual + M_PI_2, M_PI) - M_PI_2;
 }
 
 void* TaskMovement(void* pData)
@@ -128,7 +129,6 @@ void* TaskMovement(void* pData)
         while (!movEnableBool) {
             pthread_cond_wait(&movEnableCond, &movEnableMutex);
         }
-        pthread_mutex_unlock(&movEnableMutex);
 
         // Wait for new calculation if not calculated yet
         lastPosCalc = getPositionNewer(&connu, lastPosCalc);
@@ -145,19 +145,21 @@ void* TaskMovement(void* pData)
         // É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 = 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
-        if (dDirEcart > D_DIR_ECART_MAX) {
+        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 (dDirEcart < D_DIR_ECART_MIN) {
+        } else if (dDirEcartAbs < D_DIR_ECART_MIN) {
             oRetenu = false;
         }
         oErr = oRetenu ? oDirEcart : oEcart;
 
-        float oDirEcartAbs = fabs(oDirEcart);
+        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) {
@@ -179,7 +181,7 @@ void* TaskMovement(void* pData)
         // Calcul si on est arrivé
         pthread_mutex_lock(&movDoneMutex);
         clock_gettime(CLOCK_REALTIME, &pidNow);
-        movDoneBool = !dRetenu && !oRetenu && dDirEcart < D_CONS_THRESOLD && oEcart < O_CONS_THRESOLD;
+        movDoneBool = !oRetenu && fabs(oEcart) < O_ECART_MAX;
         if (movDoneBool) {
             pthread_cond_signal(&movDoneCond);
         }
@@ -231,6 +233,7 @@ void* TaskMovement(void* pData)
         setMoteurTension(lVolt, rVolt);
 
         nbCalcCons++;
+        pthread_mutex_unlock(&movEnableMutex);
     }
 
     return NULL;
@@ -246,6 +249,7 @@ void deconfigureMovement()
 
 void enableConsigne()
 {
+    printf("251 enableConsigne\n");
     pthread_mutex_lock(&movEnableMutex);
     clock_gettime(CLOCK_REALTIME, &pidNow);
     movEnableBool = true;
@@ -255,6 +259,7 @@ void enableConsigne()
 
 void disableConsigne()
 {
+    printf("261 disableConsigne\n");
     pthread_mutex_lock(&movEnableMutex);
     movEnableBool = false;
     // No signal here, will be disabled on next run

chef/src/parcours.c

@@ -73,8 +73,8 @@ int updateParcours()
 void stopParcours()
 {
     pthread_cancel(tParcours);
-    stop();
     disableConsigne();
+    stop();
 
     resetLCD();
     updateTimeDisplay();
@@ -95,8 +95,8 @@ void gotoPoint(float x, float y, float o)
     struct position pos = { x, y, o };
     setDestination(&pos);
     waitDestination();
-    brake();
     disableConsigne();
+    brake();
 }
 
 void recuperBalles()

chef/src/position.c

@@ -23,6 +23,23 @@ pthread_t tPosition;
 unsigned int nbCalcPos;
 long lCodTot, rCodTot;
 
+void onF2CI_CODER()
+{
+    readCF(&deltaCoders, sizeof(struct F2CI_CODERs));
+    pthread_mutex_unlock(&posPolling);
+}
+
+void updateDelta()
+{
+    // Sending
+    pthread_mutex_lock(&posPolling);
+    sendCF(F2CI_CODER, NULL, 0);
+
+    // Waiting for reception
+    pthread_mutex_lock(&posPolling);
+    pthread_mutex_unlock(&posPolling);
+}
+
 void* TaskPosition(void* pData)
 {
     (void)pData;
@@ -32,15 +49,11 @@ void* TaskPosition(void* pData)
     lCodTot = 0;
     rCodTot = 0;
 
+    updateDelta();
+
     for (;;) {
 
-        // Sending
+        updateDelta();
-        pthread_mutex_lock(&posPolling);
-        sendCF(F2CI_CODER, NULL, 0);
-
-        // Waiting for reception
-        pthread_mutex_lock(&posPolling);
-        pthread_mutex_unlock(&posPolling);
 
         // Calculation
 #ifdef INVERSE_L_CODER
@@ -67,19 +80,11 @@ void* TaskPosition(void* pData)
         nbCalcPos++;
         pthread_cond_signal(&newPos);
         pthread_mutex_unlock(&posConnu);
-
-
     }
 
     return NULL;
 }
 
-void onF2CI_CODER()
-{
-    readCF(&deltaCoders, sizeof(struct F2CI_CODERs));
-    pthread_mutex_unlock(&posPolling);
-}
-
 void configurePosition()
 {
     pthread_mutex_init(&posPolling, NULL);

chef/src/test1m.c

@@ -37,18 +37,29 @@ 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 = { 100, 0, 0 };
+    struct position pos = { 0, -100, -M_PI };
     setDestination(&pos);
+    printf("50\n");
     waitDestination();
-    brake();
+    printf("52\n");
     disableConsigne();
+    printf("54\n");
+    brake();
+    printf("Done\n");
 
     // Bloque jusqu'à l'arrivée d'un signal
     pthread_mutex_init(&sRunning, NULL);

simu/simu.m

@@ -3,7 +3,7 @@ SIMULATION = 0;
 
 % Paramètres de lecture
 DIRNAME = "/home/geoffrey/CdF/cdf2018-principal/log/";
-FILENAME = "000303.csv";
+FILENAME = "000471.csv";
 PATH = DIRNAME + FILENAME;
 
 % Paramètres de simulation
@@ -45,15 +45,14 @@ coderFullResolution = (coderDataResolution / reducRatio); % cycles / wheel rev
 avPerCycle = (wheelPerimeter / coderFullResolution); % mm
 
 % Constantes asservissement
-global dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oGain dConsThresold oConsThresold;
+global dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oGain oEcartMax;
-dDirEcartMin = 1.0; % mm
+dDirEcartMin = 7.0; % mm
-dDirEcartMax = 5.0; % mm
+dDirEcartMax = 10.0; % mm
-oDirEcartMin = (6.0 / 360.0 * 2.0 * pi); % rad
+oDirEcartMin = (5.0 / 360.0 * 2.0 * pi); % rad
-oDirEcartMax = (45.0 / 360.0 * 2.0 * pi); % rad
+oDirEcartMax = (25.0 / 360.0 * 2.0 * pi); % rad
-dConsThresold = 1.0; % mm
+oEcartMax = (25.0 / 360.0 * 2.0 * pi); % rad
-oConsThresold = (6.0 / 360.0 * 2.0 * pi); % rad
 oGain = 3.0;
-P = 3.0;
+P = 7.0;
 I = 0.0;
 D = 0.0;
 
@@ -195,7 +194,7 @@ function timeGraph(series)
     end
     xlim([0 SIMULATION_TIME]);
     if (abs(m) ~= inf) && (abs(M) ~= inf)
-        ylim([m M]);
+        ylim([m M+1E-9]);
     end
     addTimeline(p);
 end