Gestion correcte de l'asservissement

chef/Makefile

@@ -11,7 +11,7 @@ CFLAGS_CUSTOM += -g
 ## Générateurs de drapeaux pour les bibliothèques
 PKG_CONFIG=pkg-config
 ## Nom des objets communs
-OBJS=actionneurs buttons CA debug diagnostics fpga i2c imu ihm lcd motor movement parcours points position securite
+OBJS=actionneurs buttons CA common debug diagnostics dimensions fpga i2c imu ihm lcd motor movement parcours points position securite
 OBJS_O=$(addprefix obj/,$(addsuffix .o,$(OBJS)))
 
 # VARIABLES AUTOMATIQUES

chef/src/common.c

@@ -0,0 +1,46 @@
+#include "common.h"
+
+void diffTime(const struct timespec* debut, const struct timespec* fin, struct timespec* ecoule)
+{
+    if ((fin->tv_nsec - debut->tv_nsec) < 0) {
+        ecoule->tv_sec = fin->tv_sec - debut->tv_sec - 1;
+        ecoule->tv_nsec = fin->tv_nsec - debut->tv_nsec + 1000000000UL;
+    } else {
+        ecoule->tv_sec = fin->tv_sec - debut->tv_sec;
+        ecoule->tv_nsec = fin->tv_nsec - debut->tv_nsec;
+    }
+}
+
+float diffTimeSec(const struct timespec* debut, const struct timespec* fin)
+{
+    struct timespec ecoule;
+    diffTime(debut, fin, &ecoule);
+    return ecoule.tv_sec + ecoule.tv_nsec * 1E-9;
+}
+
+void resetPID(struct PID *pid)
+{
+    clock_gettime(CLOCK_REALTIME, &pid->lastCalc);
+    pid->prevErr = 0;
+    pid->integErr = 0;
+}
+
+void initPID(struct PID *pid, float KP, float KI, float KD)
+{
+    pid->KP = KP;
+    pid->KI = KI;
+    pid->KD = KD;
+    resetPID(pid);
+}
+
+float updatePID(struct PID *pid, float err)
+{
+    struct timespec now;
+    clock_gettime(CLOCK_REALTIME, &now);
+    float dt = diffTimeSec(&pid->lastCalc, &now);
+    pid->integErr += (err + pid->prevErr) / 2 * dt;
+    float derivErr = (err - pid->prevErr) / dt;
+    pid->prevErr = err;
+    return pid->KP * err + pid->KI * pid->integErr + pid->KP * derivErr;
+}
+

chef/src/common.h

@@ -0,0 +1,21 @@
+#ifndef __COMMON_H_
+#define __COMMON_H_
+
+#include <time.h>
+
+struct PID {
+    struct timespec lastCalc;
+    float KP;
+    float KI;
+    float KD;
+    float prevErr;
+    float integErr;
+};
+
+void diffTime(const struct timespec* debut, const struct timespec* fin, struct timespec* ecoule);
+float diffTimeSec(const struct timespec* debut, const struct timespec* fin);
+void resetPID(struct PID *pid);
+void initPID(struct PID *pid, float KP, float KI, float KD);
+float updatePID(struct PID *pid, float err);
+
+#endif

chef/src/diagnostics.c

@@ -137,7 +137,7 @@ void runDiagnostics()
 {
     execDiagnostic("Lien FPGA", diagFPGA, NULL);
     execDiagnostic("Lien Arduino", diagArduino, NULL);
-    execDiagnostic("Lien IMU", diagIMU, NULL);
+    /* execDiagnostic("Lien IMU", diagIMU, NULL); */
     int i;
     i = 0;
     execDiagnostic("Mot+Cod L AV", diagCodeuse, &i);

chef/src/dimensions.h

@@ -21,7 +21,7 @@
 #define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec)
 #define MOTOR_CONTROLLER_REFERENCE 5.0 // V (from wiring)
 #define MOTOR_SATURATION_MIN 0.0 // V (from random)
-#define MOTOR_SATURATION_MAX 3.0 // V (from testing)
+#define MOTOR_SATURATION_MAX 4.0 // V (from testing)
 #define PWM_MAX 3.3 // V (from FPGA datasheet)
 #define CODER_RESOLUTION 370.0 // cycles/motor rev
 #define CODER_DATA_FACTOR 4.0 // increments/motor cycles
@@ -32,16 +32,35 @@
 #define CODER_FULL_RESOLUTION (CODER_DATA_RESOLUTION / REDUC_RATIO) // cycles / wheel rev
 #define AV_PER_CYCLE (WHEEL_PERIMETER / CODER_FULL_RESOLUTION) // mm
 
+// Pour éviter les pics de codeuse liées à la communication I2C
+#define ABSOLUTE_MAX_VITESSE_ROBOT 10.0 // km/h
+#define ABSOLUTE_MAX_VITESSE_ROBOT_MMP_S (ABSOLUTE_MAX_VITESSE_ROBOT * 10000.0 / 36.0) // mm/s
+#define ABSOLUTE_MAX_VITESSE_ROBOT_REVP_S (ABSOLUTE_MAX_VITESSE_ROBOT_MMP_S / WHEEL_PERIMETER) // rev/s
+#define ABSOLUTE_MAX_VITESSE_ROBOT_CYCP_S (ABSOLUTE_MAX_VITESSE_ROBOT_REVP_S * CODER_FULL_RESOLUTION) // cycle/s
+
 // Constantes asservissement
-#define D_DIR_ECART_MIN 7.0 // mm
+
-#define D_DIR_ECART_MAX 10.0 // mm
+
-#define O_DIR_ECART_MIN (5.0 / 360.0 * 2.0 * M_PI) // rad
+// Asservissement en distance
-#define O_DIR_ECART_MAX (25.0 / 360.0 * 2.0 * M_PI) // rad
+#define D_DIR_ECART_MIN 30.0 // mm
-#define O_ECART_MAX (25.0 / 360.0 * 2.0 * M_PI) // rad
+#define D_DIR_ECART_MAX 50.0 // mm
-#define O_GAIN 5.0
+#define D_KP 0.05
-#define P 5.0
+#define D_KI 0.0
-#define I 0.0
+#define D_KD 0.0
-#define D 0.0
+#define TARGET_TENSION_RATIO 0.75
-#define M 0.0
+#define TARGET_TENSION (TARGET_TENSION_RATIO * MOTOR_SATURATION_MAX) // V
+#define CAROTTE_DISTANCE (TARGET_TENSION / D_KP) // mm
+
+// Asservissement en angle
+#define O_DIR_ECART_MIN (25.0 / 360.0 * 2.0 * M_PI) // rad
+#define O_DIR_ECART_MAX (45.0 / 360.0 * 2.0 * M_PI) // rad
+#define O_ECART_MIN (25.0 / 360.0 * 2.0 * M_PI) // rad
+#define O_ECART_MAX (45.0 / 360.0 * 2.0 * M_PI) // rad
+#define O_KP (MOTOR_SATURATION_MAX / (WHEEL_PERIMETER * M_PI)) // au max peut dérivier de pi
+#define O_KI 0.0
+#define O_KD 0.0
+#define CAROTTE_ANGLE (TARGET_TENSION / O_KP) // mm
+
+#define MARGE_SECURITE 300.0 // mm
 
 #endif

chef/src/i2c.c

@@ -1,8 +1,8 @@
 #include <pthread.h>
+#include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <unistd.h>
-#include <stdbool.h>
 #include <wiringPiI2C.h>
 
 #include "i2c.h"
@@ -34,7 +34,20 @@ int openI2C(uint8_t address)
 uint8_t readI2C(int fd, uint8_t reg)
 {
     lockI2C();
-    uint8_t res = wiringPiI2CReadReg8(fd, reg);
+    int res;
+    int delay = 1;
+    static char errBuffer[1024];
+    for (int i = 0; i < I2C_DRIVEN_HIGH_RETRIES; i++) {
+        while ((res = wiringPiI2CReadReg8(fd, reg)) < 0) {
+            snprintf(errBuffer, 1024, "wiringPiI2CReadReg8 @%3d %2x %9d", fd, reg, delay);
+            perror(errBuffer);
+            usleep(delay);
+            delay *= 2;
+        }
+        if (res != 0xFF) {
+            break;
+        }
+    }
     unlockI2C();
     return res;
 }
@@ -43,8 +56,10 @@ void writeI2C(int fd, uint8_t reg, uint8_t data)
 {
     lockI2C();
     int delay = 1;
+    static char errBuffer[1024];
     while (wiringPiI2CWriteReg8(fd, reg, data) < 0) {
-        perror("wiringPiI2CWriteReg8");
+        snprintf(errBuffer, 1024, "wiringPiI2CWriteReg8 @%3d %2x←%2x %9d", fd, reg, data, delay);
+        perror(errBuffer);
         usleep(delay);
         delay *= 2;
     }

chef/src/i2c.h

@@ -3,6 +3,8 @@
 
 #include <stdint.h>
 
+#define I2C_DRIVEN_HIGH_RETRIES 3
+
 void initI2C();
 int openI2C(uint8_t address);
 uint8_t readI2C(int fd, uint8_t reg);

chef/src/motor.c

@@ -7,6 +7,8 @@
 
 float lVolt;
 float rVolt;
+float lVoltDbg;
+float rVoltDbg;
 float lVoltCons;
 float rVoltCons;
 struct timespec motStart;
@@ -18,8 +20,8 @@ enum motorState motState = braking;
 
 void configureMotor()
 {
-    registerDebugVar("lVolt", f, &lVolt);
+    registerDebugVar("lVolt", f, &lVoltDbg);
-    registerDebugVar("rVolt", f, &rVolt);
+    registerDebugVar("rVolt", f, &rVoltDbg);
     pthread_mutex_init(&motCons, NULL);
     pthread_create(&tMotor, NULL, TaskMotor, NULL);
 }
@@ -54,6 +56,8 @@ void setMoteurTensionRaw(float l, float r, bool lFor, bool rFor)
 {
     lVolt = l;
     rVolt = r;
+    lVoltDbg = (lFor ? -1 : 1) * l;
+    rVoltDbg = (rFor ? -1 : 1) * r;
 
     uint8_t enA = 0;
     uint8_t enB = 0;
@@ -79,9 +83,33 @@ void setMoteurTensionRaw(float l, float r, bool lFor, bool rFor)
         // Stay at 0 : brake mode
     }
 
-    writeI2C(fdFPGA(), MOTOR_IN, in);
+    setIn(in);
-    writeI2C(fdFPGA(), MOTOR_ENA, enA);
+    setEnA(enA);
-    writeI2C(fdFPGA(), MOTOR_ENB, enB);
+    setEnB(enB);
+}
+
+uint8_t enAbuff = 0x80;
+void setEnA(uint8_t val) {
+    if (val != enAbuff) {
+        writeI2C(fdFPGA(), MOTOR_ENA, val);
+        enAbuff = val;
+    }
+}
+
+uint8_t enBbuff = 0x80;
+void setEnB(uint8_t val) {
+    if (val != enBbuff) {
+        writeI2C(fdFPGA(), MOTOR_ENB, val);
+        enBbuff = val;
+    }
+}
+
+uint8_t inbuff = 0xFF;
+void setIn(uint8_t val) {
+    if (val != inbuff) {
+        writeI2C(fdFPGA(), MOTOR_IN, val);
+        inbuff = val;
+    }
 }
 
 void* TaskMotor(void* pData)
@@ -156,7 +184,7 @@ void* TaskMotor(void* pData)
             break;
         }
 
-        usleep(1000);
+        usleep(MOTOR_INTERVAL * 1000);
     }
 
     return NULL;
@@ -166,17 +194,21 @@ void rawBrake()
 {
     lVolt = 0;
     rVolt = 0;
-    writeI2C(fdFPGA(), MOTOR_IN, 0);
+    lVoltDbg = 0;
-    writeI2C(fdFPGA(), MOTOR_ENA, UINT8_MAX);
+    rVoltDbg = 0;
-    writeI2C(fdFPGA(), MOTOR_ENB, UINT8_MAX);
+    setIn(0);
+    setEnA(UINT8_MAX);
+    setEnB(UINT8_MAX);
 }
 void rawFreewheel()
 {
     lVolt = 0;
     rVolt = 0;
-    writeI2C(fdFPGA(), MOTOR_IN, (1 << IN1) | (1 << IN2) | (1 << IN3) | (1 << IN4));
+    lVoltDbg = 0;
-    writeI2C(fdFPGA(), MOTOR_ENA, 0);
+    rVoltDbg = 0;
-    writeI2C(fdFPGA(), MOTOR_ENA, 0);
+    setIn((1 << IN1) | (1 << IN2) | (1 << IN3) | (1 << IN4));
+    setEnA(0);
+    setEnB(0);
 }
 
 void setMoteurTension(float l, float r)

chef/src/motor.h

@@ -10,7 +10,9 @@
 #define INVERSE_L_MOTOR
 // #define INVERSE_R_MOTOR
 
-#define ENABLE_RATE_LIMITER
+// #define ENABLE_RATE_LIMITER
+
+#define MOTOR_INTERVAL 10
 
 // V/s
 #define RATE_LIMITER_UP 6
@@ -45,5 +47,8 @@ uint8_t moteurTensionToPWM(float V);
 void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor);
 void rawFreewheel();
 void rawBrake();
+void setEnA(uint8_t val);
+void setEnB(uint8_t val);
+void setIn(uint8_t val);
 
 #endif

chef/src/movement.c

@@ -5,7 +5,9 @@
 #include <string.h>
 #include <unistd.h>
 
+#include "common.h"
 #include "debug.h"
+#include "dimensions.h"
 #include "motor.h"
 #include "movement.h"
 #include "securite.h"
@@ -20,17 +22,17 @@ bool movInstructionBool;
 
 float xDiff;
 float yDiff;
+float dEcart;
 float oEcart;
-float dDirEcart;
 float oDirEcart;
+float dDirEcart;
+float oConsEcart;
 float dErr;
 float oErr;
-bool oRetenu;
+float dVolt;
-bool dRetenu;
+float oVolt;
 float lErr;
 float rErr;
-float lErrPrev;
-float rErrPrev;
 unsigned int nbCalcCons;
 
 void configureMovement()
@@ -38,9 +40,7 @@ void configureMovement()
     stop();
 
     configureMotor();
-#ifdef ENABLE_SECURITE
     configureSecurite();
-#endif
 
     nbCalcCons = 0;
 
@@ -55,18 +55,127 @@ void configureMovement()
     registerDebugVar("oCons", f, &cons.o);
     registerDebugVar("xDiff", f, &xDiff);
     registerDebugVar("yDiff", f, &yDiff);
-    registerDebugVar("oEcart", f, &oEcart);
     registerDebugVar("dErr", f, &dErr);
     registerDebugVar("oErr", f, &oErr);
+    registerDebugVar("dVolt", f, &dVolt);
+    registerDebugVar("oVolt", f, &oVolt);
     registerDebugVar("dDirEcart", f, &dDirEcart);
     registerDebugVar("oDirEcart", f, &oDirEcart);
-    registerDebugVar("dRetenu", d, &dRetenu);
+    registerDebugVar("dEcart", f, &dEcart);
-    registerDebugVar("oRetenu", d, &oRetenu);
+    registerDebugVar("oEcart", f, &oEcart);
+    registerDebugVar("oConsEcart", f, &oConsEcart);
     registerDebugVar("lErr", f, &lErr);
     registerDebugVar("rErr", f, &rErr);
     registerDebugVar("nbCalcCons", d, &nbCalcCons);
 }
 
+float angleMod(float angle)
+{
+    return fmodf(angle + M_PI, 2 * M_PI) - M_PI;
+}
+
+float fcap(float value, float cap)
+{
+    if (value > 0) {
+        return fmin(value, cap);
+    } else {
+        return fmax(value, -cap);
+    }
+}
+
+void* TaskMovement(void* pData)
+{
+    (void)pData;
+
+    unsigned int lastPosCalc = 0;
+    struct position connu;
+
+    struct PID dPid;
+    struct PID oPid;
+
+    initPID(&dPid, D_KP, D_KI, D_KD);
+    initPID(&oPid, O_KP, O_KI, O_KD);
+
+    bool orienteDestination = false;
+    bool procheDestination = false;
+    bool orienteConsigne = false;
+    bool reverse;
+
+    for (;;) {
+
+        lastPosCalc = getPositionNewer(&connu, lastPosCalc);
+
+        xDiff = cons.x - connu.x;
+        yDiff = cons.y - connu.y;
+        dDirEcart = hypotf(xDiff, yDiff);
+        oDirEcart = angleMod(atan2(yDiff, xDiff) - connu.o);
+        oConsEcart = angleMod(cons.o - connu.o);
+
+        if ((reverse = fabsf(oDirEcart) > M_PI_2)) {
+            dDirEcart = -dDirEcart;
+            oDirEcart = angleMod(oDirEcart + M_PI);
+        }
+
+        if (fabsf(oDirEcart) < O_DIR_ECART_MIN) {
+            orienteDestination = true;
+        } else if (fabsf(oDirEcart) > O_DIR_ECART_MAX) {
+            orienteDestination = false;
+        }
+
+        if (fabsf(dDirEcart) < D_DIR_ECART_MIN) {
+            procheDestination = true;
+        } else if (fabsf(dDirEcart) > D_DIR_ECART_MAX) {
+            procheDestination = false;
+        }
+
+        if (fabsf(oConsEcart) < O_ECART_MIN) {
+            orienteConsigne = true;
+        } else if (fabsf(oConsEcart) > O_ECART_MAX) {
+            orienteConsigne = false;
+        }
+
+        // Carotte
+        dEcart = (orienteDestination && !procheDestination) ? dDirEcart : 0;
+        dErr = fcap(dEcart, CAROTTE_DISTANCE);
+        oEcart = procheDestination ? oConsEcart : oDirEcart;
+        oErr = fcap(oEcart * DISTANCE_BETWEEN_WHEELS, CAROTTE_ANGLE);
+
+        dVolt = updatePID(&dPid, dErr);
+        oVolt = updatePID(&oPid, oErr);
+
+        float lVolt = dVolt - oVolt;
+        float rVolt = dVolt + oVolt;
+
+        pthread_mutex_lock(&movInstructionMutex);
+        if (movInstructionBool) {
+            if (procheDestination && orienteConsigne) {
+                brake();
+            } else {
+                setMoteurTension(lVolt, rVolt);
+            }
+        }
+        pthread_mutex_unlock(&movInstructionMutex);
+
+        nbCalcCons++;
+    }
+
+    return NULL;
+}
+
+void enableAsservissement()
+{
+    pthread_mutex_lock(&movInstructionMutex);
+    movInstructionBool = true;
+    pthread_mutex_unlock(&movInstructionMutex);
+}
+
+void disableAsservissement()
+{
+    pthread_mutex_lock(&movInstructionMutex);
+    movInstructionBool = false;
+    pthread_mutex_unlock(&movInstructionMutex);
+}
+
 void setDestination(struct position* pos)
 {
     pthread_mutex_lock(&movInstructionMutex);
@@ -85,116 +194,6 @@ void waitDestination()
     pthread_mutex_unlock(&movInstructionMutex);
 }
 
-float angleGap(float target, float actual)
-{
-    float ret = fmodf(target - actual + M_PI, 2 * M_PI) - M_PI;
-    return ret;
-}
-
-float angleGap180(float target, float actual, float* dist)
-{
-    if (fabs(fmodf(target - actual + M_PI, 2 * M_PI) - M_PI) > M_PI_2) {
-        *dist = -*dist;
-    }
-    return fmodf(target - actual + M_PI_2, M_PI) - M_PI_2;
-}
-
-void* TaskMovement(void* pData)
-{
-    (void)pData;
-
-    unsigned int lastPosCalc = 0;
-    struct position connu;
-
-    for (;;) {
-        // Attend instruction
-        printf("Wait instruction\n");
-        pthread_mutex_lock(&movInstructionMutex);
-        while (!movInstructionBool) {
-            pthread_cond_wait(&movInstructionCond, &movInstructionMutex);
-        } // Début de l'instruction
-
-        printf("Oriente dir\n");
-        // Oriente vers direction
-        // TODO Marche arrière
-        do {
-
-            lastPosCalc = getPositionNewer(&connu, lastPosCalc);
-
-            xDiff = cons.x - connu.x;
-            yDiff = cons.y - connu.y;
-            oDirEcart = angleGap(atan2(yDiff, xDiff), connu.o);
-            float oErrRev = oDirEcart * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER;
-            float lVolt = -oErrRev * P + (oErrRev > 0 ? -M : M);
-            float rVolt = oErrRev * P + (oErrRev > 0 ? M : -M);
-            setMoteurTension(lVolt, rVolt);
-
-            nbCalcCons++;
-        } 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 + (lErr > 0 ? M : -M);
-            float rVolt = rErr * P + (rErr > 0 ? M : -M);
-            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 + (oErrRev > 0 ? -M : M);
-                float rVolt = oErrRev * P + (oErrRev > 0 ? M : -M);
-                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;
-}
-
 void deconfigureMovement()
 {
     stop();

chef/src/movement.h

@@ -7,17 +7,19 @@
 
 #define ANGLE_INSIGNIFIANT NAN
 
-#define ENABLE_SECURITE
+// #define ENABLE_SECURITE
-
-#define SECURITE_MARGE 300
-
 
 #include "position.h"
 
+// Public
 void configureMovement();
 void deconfigureMovement();
 void setDestination(struct position* pos);
-void* TaskMovement(void* pData);
 void waitDestination();
+void enableAsservissement();
+void disableAsservissement();
+
+// Private
+void* TaskMovement(void* pData);
 
 #endif

chef/src/position.c

@@ -13,6 +13,7 @@
 #include "dimensions.h"
 #include "fpga.h"
 #include "position.h"
+#include "common.h"
 
 // Globales
 struct position connu;
@@ -24,16 +25,34 @@ pthread_t tPosition;
 unsigned int nbCalcPos;
 long lCodTot, rCodTot;
 
-int16_t oldL, oldR;
+uint16_t oldL, oldR;
-int16_t newL, newR;
+uint16_t newL, newR;
 int16_t deltaL, deltaR;
+int newLdbg, newRdbg;
+
+struct timespec lastCoderRead;
 
 void updateDelta()
 {
-    newL = (readI2C(fdFPGA(), CODER_LEFT_H) << 8 | readI2C(fdFPGA(), CODER_LEFT_L));
+    newL = (readI2C(fdFPGA(), CODER_LEFT_H) << 8 | readI2C(fdFPGA(), CODER_LEFT_L)) & 0xFFFF;
-    newR = (readI2C(fdFPGA(), CODER_RIGHT_H) << 8 | readI2C(fdFPGA(), CODER_RIGHT_L));
+    newR = (readI2C(fdFPGA(), CODER_RIGHT_H) << 8 | readI2C(fdFPGA(), CODER_RIGHT_L)) & 0xFFFF;
-    deltaL = (abs(oldL - newL) < UINT16_MAX/2) ? newL - oldL : UINT16_MAX - oldL + newL;
+    newLdbg = newL;
-    deltaR = (abs(oldR - newR) < UINT16_MAX/2) ? newR - oldR : UINT16_MAX - oldR + newR;
+    newRdbg = newR;
+    deltaL = (abs(oldL - newL) < UINT16_MAX / 2) ? newL - oldL : UINT16_MAX - oldL + newL;
+    deltaR = (abs(oldR - newR) < UINT16_MAX / 2) ? newR - oldR : UINT16_MAX - oldR + newR;
+
+    // Verification de valeur abbérante
+    struct timespec now;
+    clock_gettime(CLOCK_REALTIME, &now);
+    float maxDelta = diffTimeSec(&lastCoderRead, &now) * ABSOLUTE_MAX_VITESSE_ROBOT_CYCP_S;
+    if (abs(deltaL) > maxDelta) {
+        deltaL = 0;
+    }
+    if (abs(deltaR) > maxDelta) {
+        deltaR = 0;
+    }
+    lastCoderRead = now;
+
     oldL = newL;
     oldR = newR;
 }
@@ -46,8 +65,6 @@ void* TaskPosition(void* pData)
     nbCalcPos = 0;
     lCodTot = 0;
     rCodTot = 0;
-    oldL = 0;
-    oldR = 0;
 
     updateDelta();
 
@@ -80,6 +97,8 @@ void* TaskPosition(void* pData)
         nbCalcPos++;
         pthread_cond_signal(&newPos);
         pthread_mutex_unlock(&posConnu);
+
+        usleep(POSITION_INTERVAL * 1000);
     }
 
     return NULL;
@@ -87,19 +106,17 @@ void* TaskPosition(void* pData)
 
 void configurePosition()
 {
-    pthread_mutex_init(&posConnu, NULL);
+    resetPosition();
-    pthread_cond_init(&newPos, NULL);
     registerDebugVar("lCodTot", ld, &lCodTot);
     registerDebugVar("rCodTot", ld, &rCodTot);
-    registerDebugVar("newL", ld, &newL);
+    registerDebugVar("newL", d, &newLdbg);
-    registerDebugVar("newR", ld, &newR);
+    registerDebugVar("newR", d, &newRdbg);
-    connu.x = 0;
-    connu.y = 0;
-    connu.o = 0;
     registerDebugVar("xConnu", f, &connu.x);
     registerDebugVar("yConnu", f, &connu.y);
     registerDebugVar("oConnu", f, &connu.o);
     registerDebugVar("nbCalcPos", d, &nbCalcPos);
+    pthread_mutex_init(&posConnu, NULL);
+    pthread_cond_init(&newPos, NULL);
     pthread_create(&tPosition, NULL, TaskPosition, NULL);
 }
 

chef/src/position.h

@@ -8,6 +8,8 @@
 // #define INVERSE_L_CODER
 #define INVERSE_R_CODER
 
+#define POSITION_INTERVAL 10
+
 // Structures
 struct __attribute__ ((packed)) position {
     float x;

chef/src/premier.c

@@ -10,7 +10,6 @@
 #include "debug.h"
 #include "i2c.h"
 #include "ihm.h"
-#include "imu.h"
 #include "movement.h"
 #include "position.h"
 
@@ -34,7 +33,6 @@ int main()
 
     configureDebug();
     configureIHM();
-    configureIMU();
     configureActionneurs();
     configurePosition();
     configureMovement();
@@ -52,7 +50,6 @@ int main()
     deconfigureMovement();
     deconfigurePosition();
     deconfigureActionneurs();
-    deconfigureIMU();
     deconfigureIHM();
     deconfigureDebug();
     return EXIT_SUCCESS;

chef/src/test1m.c

@@ -1,67 +0,0 @@
-#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 "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();
-    configureIMU();
-    configureActionneurs();
-    configurePosition();
-    configureMovement();
-    startDebug();
-
-    debugSetActive(true);
-    struct position pos = {1000, 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();
-    deconfigureDebug();
-    return EXIT_SUCCESS;
-}

chef/src/testAvance.c

@@ -10,9 +10,8 @@
 #include "debug.h"
 #include "i2c.h"
 #include "ihm.h"
-#include "imu.h"
-#include "movement.h"
 #include "motor.h"
+#include "movement.h"
 #include "position.h"
 
 pthread_mutex_t sRunning;
@@ -23,7 +22,7 @@ void endRunning(int signal)
     pthread_mutex_unlock(&sRunning);
 }
 
-int main()
+int main(int argc, char* argv[])
 {
 
     if (wiringPiSetup() < 0) {
@@ -33,15 +32,29 @@ int main()
     initI2C();
     srand(time(NULL));
 
+    float x = 0;
+    float y = 0;
+    float o = 0;
+    if (argc >= 2) {
+        sscanf(argv[1], "%f", &x);
+        if (argc >= 3) {
+            sscanf(argv[2], "%f", &y);
+            if (argc >= 4) {
+                sscanf(argv[3], "%f", &o);
+            }
+        }
+    }
+
     configureDebug();
     configurePosition();
     configureMovement();
+    debugSetActive(true);
     startDebug();
 
-    debugSetActive(true);
+    struct position pos = { x, y, o };
-    sleep(1);
+    printf("Go\n");
-    struct position pos = {100000, 0, 0 };
     setDestination(&pos);
+    enableAsservissement();
     waitDestination();
     brake();
     printf("Done\n");

chef/src/testRetour.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 "actionneurs.h"
+#include "buttons.h"
+#include "debug.h"
+#include "i2c.h"
+#include "lcd.h"
+#include "motor.h"
+#include "movement.h"
+#include "position.h"
+
+int main()
+{
+    if (wiringPiSetup() < 0) {
+        fprintf(stderr, "Impossible d'initialiser WiringPi\n");
+        exit(EXIT_FAILURE);
+    }
+    initI2C();
+    srand(time(NULL));
+
+    configureDebug();
+    configurePosition();
+    configureMovement();
+    configureButtons();
+    initLCD();
+    debugSetActive(true);
+    startDebug();
+
+    printToLCD(LCD_LINE_1, "RGE: Set origin");
+    printToLCD(LCD_LINE_2, "JNE: Toggle free");
+
+    bool isFree = true;
+    disableAsservissement();
+    freewheel();
+
+    for (;;) {
+        switch (pressedButton(BUT_BLOCK)) {
+        case jaune:
+            isFree = !isFree;
+            if (isFree) {
+                disableAsservissement();
+                freewheel();
+            } else {
+                enableAsservissement();
+            }
+            break;
+        case rouge:
+            resetPosition();
+            break;
+        default:
+            break;
+        }
+        clearLCD();
+        if (isFree) {
+            printToLCD(LCD_LINE_1, "Freewheel");
+        } else {
+            printToLCD(LCD_LINE_1, "Asservi");
+        }
+    }
+
+    deconfigureMovement();
+    deconfigurePosition();
+    deconfigureDebug();
+    return EXIT_SUCCESS;
+}

chef/src/testUpDown.c

@@ -7,10 +7,12 @@
 #include <wiringPi.h>
 #include <wiringPiI2C.h>
 
+#include "buttons.h"
+#include "debug.h"
+#include "dimensions.h"
 #include "lcd.h"
 #include "motor.h"
-#include "buttons.h"
+#include "position.h"
-#include "dimensions.h"
 
 #define UP_TIME 1000
 #define HIGH_TIME 3000
@@ -19,7 +21,8 @@
 #define MAXI MOTOR_SATURATION_MAX
 #define INTERVAL 10
 
-void changerMoteursWrapper(float l, float r) {
+void changerMoteursWrapper(float l, float r)
+{
     /* clearLCD(); */
     printfToLCD(LCD_LINE_1, "L: %f", l);
     printfToLCD(LCD_LINE_2, "R: %f", r);
@@ -36,8 +39,12 @@ int main(int argc, char* argv[])
 
     initI2C();
     initLCD();
+    configureDebug();
     configureButtons();
     configureMotor();
+    configurePosition();
+    startDebug();
+    debugSetActive(true);
 
     for (;;) {
         for (int i = 0; i < UP_TIME; i += INTERVAL) {
@@ -73,5 +80,4 @@ int main(int argc, char* argv[])
         changerMoteursWrapper(0, 0);
         delay(LOW_TIME);
     }
-
 }

fpga/Principal.vhd

@@ -5,14 +5,13 @@ use IEEE.NUMERIC_STD.ALL;
 
 entity Principal is
     Generic(
-               fFpga : INTEGER := 50_000_000;
+               fFpga : INTEGER := 50_000_000
-               fBaud : INTEGER := 115200
            );
     Port (
              CLK : in  std_logic;
              BTN: in std_logic;
-             RX: in std_logic;
+             SDA: inout std_logic;
-             TX: out std_logic;
+             SCL: inout std_logic;
              LEFTCHA: in std_logic;
              LEFTCHB: in std_logic;
              RIGHTCHA: in std_logic;
@@ -25,11 +24,11 @@ entity Principal is
              BACKRECHO: in std_logic;
              ENAREF: out std_logic;
              ENA: out std_logic;
-             IN1ENC: out std_logic;
+             IN1: out std_logic;
              IN2: out std_logic;
              ENBREF: out std_logic;
              ENB: out std_logic;
-             IN3END: out std_logic;
+             IN3: out std_logic;
              IN4: out std_logic
          );
 end Principal;
@@ -41,7 +40,6 @@ architecture Behavioral of Principal is
     -- Encoder
     signal left : integer;
     signal right : integer;
-    signal zerocoder : std_logic;
 
     component hedm is
         Port (
@@ -101,11 +99,9 @@ architecture Behavioral of Principal is
 
     -- Motor controller
     signal enAd : std_logic_vector(7 downto 0);
-    signal in1enCd : std_logic_vector(7 downto 0);
-    signal in2d : std_logic;
     signal enBd : std_logic_vector(7 downto 0);
-    signal in3enDd : std_logic_vector(7 downto 0);
+    signal ind : std_logic_vector(7 downto 0);
-    signal in4d : std_logic;
+
     component PWM is
         port (
                  clk : in std_logic;
@@ -115,54 +111,46 @@ architecture Behavioral of Principal is
     end component;
 
     -- CF
-    component uart is
+    component I2CSLAVE
-        generic (
+        generic(
-                    baud                : positive := fBaud;
+                   DEVICE: std_logic_vector(7 downto 0) := x"42"
-                    clock_frequency     : positive := fFpga
+               );
-                );
+        port(
-        port (
+                MCLK        : in    std_logic;
-                 clock               :   in  std_logic;
+                nRST        : in    std_logic;
-                 reset               :   in  std_logic;
+                SDA_IN      : in    std_logic;
-                 data_stream_in      :   in  std_logic_vector(7 downto 0);
+                SCL_IN      : in    std_logic;
-                 data_stream_in_stb  :   in  std_logic;
+                SDA_OUT     : out   std_logic;
-                 data_stream_in_ack  :   out std_logic;
+                SCL_OUT     : out   std_logic;
-                 data_stream_out     :   out std_logic_vector(7 downto 0);
+                ADDRESS     : out   std_logic_vector(7 downto 0);
-                 data_stream_out_stb :   out std_logic;
+                DATA_OUT    : out   std_logic_vector(7 downto 0);
-                 tx                  :   out std_logic;
+                DATA_IN     : in    std_logic_vector(7 downto 0);
-                 rx                  :   in  std_logic
+                WR          : out   std_logic;
-             );
+                RD          : out   std_logic
+            );
     end component;
+    signal sdaIn : std_logic;
+    signal sclIn : std_logic;
+    signal sdaOut : std_logic;
+    signal sclOut : std_logic;
+    signal address : std_logic_vector(7 downto 0);
+    signal dataOut : std_logic_vector(7 downto 0);
+    signal dataIn : std_logic_vector(7 downto 0);
+    signal wr : std_logic;
+    signal rd : std_logic;
+    signal rdP : std_logic;
 
-    signal txData : std_logic_vector(7 downto 0);
-    signal txStb : std_logic := '0';
-    signal txAck : std_logic := '0';
 
-    signal rxData : std_logic_vector(7 downto 0);
+    signal leftB : std_logic_vector(15 downto 0);
-    signal rxStb : std_logic := '0';
+    signal rightB : std_logic_vector(15 downto 0);
-
+    signal frontLRawB : std_logic_vector(15 downto 0);
-    -- Handling
+    signal frontRRawB : std_logic_vector(15 downto 0);
-    component communication is
+    signal backLRawB : std_logic_vector(15 downto 0);
-        Port (
+    signal backRRawB : std_logic_vector(15 downto 0);
-                 clock : in  std_logic;
+    signal frontLB : std_logic_vector(15 downto 0);
-                 reset : in std_logic;
+    signal frontRB : std_logic_vector(15 downto 0);
-                 left : in integer;
+    signal backLB : std_logic_vector(15 downto 0);
-                 right : in integer;
+    signal backRB : std_logic_vector(15 downto 0);
-                 zerocoder : out std_logic;
-                 front : in integer;
-                 back : in integer;
-                 txData : out std_logic_vector(7 downto 0);
-                 txStb : out std_logic;
-                 txAck : in std_logic;
-                 rxData : in std_logic_vector(7 downto 0);
-                 rxStb : in std_logic;
-                 enA : out std_logic_vector(7 downto 0);
-                 in1enC : out std_logic_vector(7 downto 0);
-                 in2 : out std_logic;
-                 enB : out std_logic_vector(7 downto 0);
-                 in3enD : out std_logic_vector(7 downto 0);
-                 in4 : out std_logic
-             );
-    end component;
 
 begin
 
@@ -184,7 +172,7 @@ begin
                                  chA => LEFTCHA,
                                  chB => LEFTCHB,
                                  reset => reset,
-                                 zero => zerocoder,
+                                 zero => '0',
                                  counts => left
                              );
 
@@ -193,80 +181,80 @@ begin
                                   chA => RIGHTCHA,
                                   chB => RIGHTCHB,
                                   reset => reset,
-                                  zero => zerocoder,
+                                  zero => '0',
                                   counts => right
                               );
 
     frontLCapt: hcsr04 port map (
-                                   clk => CLK,
+                                    clk => CLK,
-                                   reset => reset,
+                                    reset => reset,
-                                   echo => FRONTLECHO,
+                                    echo => FRONTLECHO,
-                                   distance => frontLRaw,
+                                    distance => frontLRaw,
-                                   trigger => FRONTTRIGGER,
+                                    trigger => FRONTTRIGGER,
-                                   start => '1',
+                                    start => '1',
-                                   finished => frontLFinished
+                                    finished => frontLFinished
-                               );
+                                );
     frontLFilter : FIR port map (
-                                   clock     => CLK,
+                                    clock     => CLK,
-                                   reset     => reset,
+                                    reset     => reset,
-                                   signalIn  => frontLRaw,
+                                    signalIn  => frontLRaw,
-                                   signalOut => frontL,
+                                    signalOut => frontL,
-                                   start     => frontLFinished
+                                    start     => frontLFinished
-                               -- done      => done
+                                -- done      => done
-                               );
+                                );
 
     frontRCapt: hcsr04 port map (
-                                   clk => CLK,
+                                    clk => CLK,
-                                   reset => reset,
+                                    reset => reset,
-                                   echo => FRONTRECHO,
+                                    echo => FRONTRECHO,
-                                   distance => frontRRaw,
+                                    distance => frontRRaw,
-                                   -- trigger => FRONTTRIGGER,
+                                    -- trigger => FRONTTRIGGER,
-                                   start => '1',
+                                    start => '1',
-                                   finished => frontRFinished
+                                    finished => frontRFinished
-                               );
+                                );
     frontRFilter : FIR port map (
-                                   clock     => CLK,
+                                    clock     => CLK,
-                                   reset     => reset,
+                                    reset     => reset,
-                                   signalIn  => frontRRaw,
+                                    signalIn  => frontRRaw,
-                                   signalOut => frontR,
+                                    signalOut => frontR,
-                                   start     => frontRFinished
+                                    start     => frontRFinished
-                               -- done      => done
+                                -- done      => done
-                               );
+                                );
 
     backLCapt: hcsr04 port map (
-                                  clk => CLK,
+                                   clk => CLK,
-                                  reset => reset,
+                                   reset => reset,
-                                  echo => BACKLECHO,
+                                   echo => BACKLECHO,
-                                  distance => backLRaw,
+                                   distance => backLRaw,
-                                  trigger => BACKTRIGGER,
+                                   trigger => BACKTRIGGER,
-                                  start => '1',
+                                   start => '1',
-                                  finished => backLFinished
+                                   finished => backLFinished
-                              );
+                               );
     backLFilter : FIR port map (
-                                  clock     => CLK,
+                                   clock     => CLK,
-                                  reset     => reset,
+                                   reset     => reset,
-                                  signalIn  => backLRaw,
+                                   signalIn  => backLRaw,
-                                  signalOut => backL,
+                                   signalOut => backL,
-                                  start     => backLFinished
+                                   start     => backLFinished
-                              -- done      => done
+                               -- done      => done
-                              );
+                               );
     backRCapt: hcsr04 port map (
-                                  clk => CLK,
+                                   clk => CLK,
-                                  reset => reset,
+                                   reset => reset,
-                                  echo => BACKRECHO,
+                                   echo => BACKRECHO,
-                                  distance => backRRaw,
+                                   distance => backRRaw,
-                                  -- trigger => BACKTRIGGER,
+                                   -- trigger => BACKTRIGGER,
-                                  start => '1',
+                                   start => '1',
-                                  finished => backRFinished
+                                   finished => backRFinished
-                              );
+                               );
     backRFilter : FIR port map (
-                                  clock     => CLK,
+                                   clock     => CLK,
-                                  reset     => reset,
+                                   reset     => reset,
-                                  signalIn  => backRRaw,
+                                   signalIn  => backRRaw,
-                                  signalOut => backR,
+                                   signalOut => backR,
-                                  start     => backRFinished
+                                   start     => backRFinished
-                              -- done      => done
+                               -- done      => done
-                              );
+                               );
     enAp : PWM port map (
                             clk => pwmClk,
                             data => enAd,
@@ -274,12 +262,8 @@ begin
                         );
     ENAREF <= '1';
 
-    in1enCp : PWM port map (
+    IN1 <= ind(0);
-                               clk => pwmClk,
+    IN2 <= ind(1);
-                               data => in1enCd,
-                               pulse => IN1ENC
-                           );
-    IN2 <= in2d;
 
     enBp : PWM port map (
                             clk => pwmClk,
@@ -288,48 +272,69 @@ begin
                         );
     ENBREF <= '1';
 
-    in3enDp : PWM port map (
+    IN3 <= ind(2);
-                               clk => pwmClk,
+    IN4 <= ind(3);
-                               data => in3enDd,
+
-                               pulse => IN3END
+    FA : i2cslave port map (
+                               MCLK        => clk,
+                               nRST        => not reset,
+                               SDA_IN      => sdaIn,
+                               SCL_IN      => sclIn,
+                               SDA_OUT     => sdaOut,
+                               SCL_OUT     => sclOut,
+                               ADDRESS     => address,
+                               DATA_OUT    => dataOut,
+                               DATA_IN     => dataIn,
+                               WR          => wr,
+                               RD          => rd
                            );
-    IN4 <= in4d;
+
+    SCL <= 'Z' when sclOut = '1' else '0';
+    sclIn <= to_UX01(SCL);
+    SDA <= 'Z' when sdaOut = '1' else '0';
+    sdaIn <= to_UX01(SDA);
+
+    leftB <= std_logic_vector(to_signed(left, 16));
+    rightB <= std_logic_vector(to_signed(right, 16));
+    frontLRawB <= std_logic_vector(to_unsigned(frontLRaw, 16));
+    frontRRawB <= std_logic_vector(to_unsigned(frontRRaw, 16));
+    backLRawB <= std_logic_vector(to_unsigned(backLRaw, 16));
+    backRRawB <= std_logic_vector(to_unsigned(backRRaw, 16));
+    frontLB <= std_logic_vector(to_unsigned(frontL, 16));
+    frontRB <= std_logic_vector(to_unsigned(frontR, 16));
+    backLB <= std_logic_vector(to_unsigned(backL, 16));
+    backRB <= std_logic_vector(to_unsigned(backR, 16));
+
+    dataIn <=  x"50" when address = x"00" else
+               leftB(15 downto 8) when address = x"10" else
+               leftB(7 downto 0) when address = x"11" else
+               rightB(15 downto 8) when address = x"12" else
+               rightB(7 downto 0) when address = x"13" else
+               frontLRawB(15 downto 8) when address = x"20" else
+               frontLRawB(7 downto 0) when address = x"21" else
+               frontRRawB(15 downto 8) when address = x"22" else
+               frontRRawB(7 downto 0) when address = x"23" else
+               backLRawB(15 downto 8) when address = x"24" else
+               backLRawB(7 downto 0) when address = x"25" else
+               backRRawB(15 downto 8) when address = x"26" else
+               backRRawB(7 downto 0) when address = x"27" else
+               frontLB(15 downto 8) when address = x"30" else
+               frontLB(7 downto 0) when address = x"31" else
+               frontRB(15 downto 8) when address = x"32" else
+               frontRB(7 downto 0) when address = x"33" else
+               backLB(15 downto 8) when address = x"34" else
+               backLB(7 downto 0) when address = x"35" else
+               backRB(15 downto 8) when address = x"36" else
+               backRB(7 downto 0) when address = x"37" else
+               ind when address = x"60" else
+               enAd when address = x"61" else
+               enBd when address = x"62" else
+               (others => '0');
+
+    ind <= dataOut when (address = x"60" and wr = '1') else ind;
+    enAd <= dataOut when (address = x"61" and wr = '1') else enAd;
+    enBd <= dataOut when (address = x"62" and wr = '1') else enBd;
 
 
-    FA: uart port map(
-                         clock               => CLK,
-                         reset               => reset,
-                         data_stream_in      => txData,
-                         data_stream_in_stb  => txStb,
-                         data_stream_in_ack  => txAck,
-                         data_stream_out     => rxData,
-                         data_stream_out_stb => rxStb,
-                         tx                  => TX,
-                         rx                  => RX
-                     );
-
-    frontMin <= frontLRaw when frontLRaw < frontRRaw else frontRRaw;
-    backMin <= backLRaw when backLRaw < backRRaw else backRRaw;
-
-    com: communication port map(
-                                   clock => CLK,
-                                   reset  => reset,
-                                   left  => left,
-                                   right  => right,
-                                   zerocoder  => zerocoder,
-                                   front  => frontMin,
-                                   back  => backMin,
-                                   txData  => txData,
-                                   txStb  => txStb,
-                                   txAck  => txAck,
-                                   rxData  => rxData,
-                                   rxStb  => rxStb,
-                                   enA    => enAd,
-                                   in1enC => in1enCd,
-                                   in2    => in2d,
-                                   enB    => enBd,
-                                   in3enD => in3enDd,
-                                   in4    => in4d
-                               );
 end Behavioral;
 

fpga/communication.vhd

@@ -97,6 +97,12 @@ begin
             txStb <= '0';
             zerocoder <= '0';
             txData <= x"00";
+            enA <= (others => '0');
+            in1enC <= (others => '0');
+            enB <= (others => '0');
+            in3enD <= (others => '0');
+            in2 <= '0';
+            in4 <= '0';
         else
             if rising_edge(clock) then
                 zerocoder <= '0';

fpga/fir.vhd

@@ -23,7 +23,7 @@ architecture Behavioral of fir is
     constant N : INTEGER := 4; -- Nombre de coefficients
     constant M : INTEGER := 2**6; -- Facteur multiplicatif
     type INT_ARRAY is array (N-1 downto 0) of integer;
-    constant coefficients : INT_ARRAY := (16,16,16,16);
+    constant coefficients : INT_ARRAY := (32,16,8,8);
     -- ↑ Coefficients du fir multipliés par M
 
     signal echantillons : INT_ARRAY := (others => 0); -- stockage des entrées retardées

fpga/i2c.vhd

@@ -0,0 +1,283 @@
+--###############################
+--# Project Name : I2C slave
+--# File         : i2cslave.vhd
+--# Project      : i2c slave for FPGA
+--# Engineer     : Philippe THIRION
+--# Modification History
+--###############################
+
+
+--	copyright Philippe Thirion
+--	github.com/tirfil
+--
+--    Copyright 2016 Philippe THIRION
+--
+--    This program is free software: you can redistribute it and/or modify
+--    it under the terms of the GNU General Public License as published by
+--    the Free Software Foundation, either version 3 of the License, or
+--    (at your option) any later version.
+
+--    This program is distributed in the hope that it will be useful,
+--    but WITHOUT ANY WARRANTY; without even the implied warranty of
+--    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+--    GNU General Public License for more details.
+
+--    You should have received a copy of the GNU General Public License
+--    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+
+library IEEE;
+use IEEE.std_logic_1164.all;
+use IEEE.numeric_std.all;
+
+entity I2CSLAVE is
+	generic(
+		DEVICE 		: std_logic_vector(7 downto 0) := x"38"
+	);
+	port(
+		MCLK		: in	std_logic;
+		nRST		: in	std_logic;
+		SDA_IN		: in	std_logic;
+		SCL_IN		: in	std_logic;
+		SDA_OUT		: out	std_logic;
+		SCL_OUT		: out	std_logic;
+		ADDRESS		: out	std_logic_vector(7 downto 0);
+		DATA_OUT	: out	std_logic_vector(7 downto 0);
+		DATA_IN		: in	std_logic_vector(7 downto 0);
+		WR			: out	std_logic;
+		RD			: out	std_logic
+	);
+end I2CSLAVE;
+
+architecture rtl of I2CSLAVE is
+
+	type tstate is ( S_IDLE, S_START, S_SHIFTIN, S_RW, S_SENDACK, S_SENDACK2, S_SENDNACK,
+		S_ADDRESS, S_WRITE, S_SHIFTOUT, S_READ, S_WAITACK
+	);
+
+	type toperation is (OP_READ, OP_WRITE);
+	
+	signal state : tstate;
+	signal next_state : tstate;
+	signal operation : toperation;
+
+	signal rising_scl, falling_scl : std_logic;
+	signal address_i : std_logic_vector(7 downto 0);
+	signal next_address : std_logic_vector(7 downto 0);
+	signal counter : integer range 0 to 7;
+	signal start_cond : std_logic;
+	signal stop_cond  : std_logic;
+	signal sda_q, sda_qq, sda_qqq : std_logic;
+	signal scl_q, scl_qq, scl_qqq : std_logic;
+	signal shiftreg : std_logic_vector(7 downto 0);
+	signal sda: std_logic;
+	signal address_incr : std_logic;
+	signal rd_d : std_logic;
+begin
+
+	ADDRESS <= address_i;
+	
+	next_address <= (others=>'0') when (address_i = x"FF") else
+		std_logic_vector(to_unsigned(to_integer(unsigned( address_i )) + 1, 8));
+	
+	S_RSY: process(MCLK,nRST)
+	begin
+		if (nRST = '0') then
+			sda_q <= '1';
+			sda_qq <= '1';
+			sda_qqq <= '1';
+			scl_q <= '1';
+			scl_qq <= '1';
+			scl_qqq <= '1';
+		elsif (MCLK'event and MCLK='1') then
+			sda_q <= SDA_IN;
+			sda_qq <= sda_q;
+			sda_qqq <= sda_qq;
+			scl_q <= SCL_IN;
+			scl_qq <= scl_q;
+			scl_qqq <= scl_qq;
+		end if;
+	end process S_RSY;
+
+	rising_scl <= scl_qq and not scl_qqq;
+	falling_scl <= not scl_qq and scl_qqq;
+		
+	START_BIT: process(MCLK,nRST)
+	begin
+		if (nRST = '0') then
+			start_cond <= '0';
+		elsif (MCLK'event and MCLK='1') then
+			if (sda_qqq = '1' and sda_qq = '0' and scl_qq = '1') then
+				start_cond <= '1';
+			else	
+				start_cond <= '0';
+			end if;
+		end if;
+	end process START_BIT;
+	
+	STOP_BIT: process(MCLK,nRST)
+	begin
+		if (nRST = '0') then
+			stop_cond <= '0';
+		elsif (MCLK'event and MCLK='1') then
+			if (sda_qqq = '0' and sda_qq = '1' and scl_qq = '1') then
+				stop_cond <= '1';
+			else	
+				stop_cond <= '0';
+			end if;
+		end if;
+	end process STOP_BIT;
+	
+	sda <= sda_qq;
+	
+	RD_DELAY: process(MCLK, nRST)
+	begin
+		if (nRST = '0') then
+			RD <= '0';
+		elsif (MCLK'event and MCLK='1') then
+			RD <= rd_d;
+		end if;
+	end process RD_DELAY;
+
+	OTO: process(MCLK, nRST)
+	begin
+		if (nRST = '0') then
+			state <= S_IDLE;
+			SDA_OUT <= '1';
+			SCL_OUT <= '1';
+			WR <= '0';
+			rd_d <= '0';
+			address_i <= (others=>'0');
+			DATA_OUT <= (others=>'0');
+			shiftreg <= (others=>'0');
+		elsif (MCLK'event and MCLK='1') then
+			if (stop_cond = '1') then
+				state <= S_IDLE;
+				SDA_OUT <= '1';
+				SCL_OUT <= '1';
+				operation <= OP_READ;
+				WR <= '0';
+				rd_d <= '0';
+				address_incr <= '0';
+			elsif(start_cond = '1') then
+				state <= S_START;
+				SDA_OUT <= '1';
+				SCL_OUT <= '1';
+				operation <= OP_READ;
+				WR <= '0';
+				rd_d <= '0';
+				address_incr <= '0';
+			elsif(state = S_IDLE) then
+				state <= S_IDLE;
+				SDA_OUT <= '1';
+				SCL_OUT <= '1';
+				operation <= OP_READ;
+				WR <= '0';
+				rd_d <= '0';
+				address_incr <= '0';
+			elsif(state = S_START) then
+				shiftreg <= (others=>'0');
+				state <= S_SHIFTIN;
+				next_state <= S_RW;
+				counter <= 6;
+			elsif(state = S_SHIFTIN) then
+				if (rising_scl = '1') then
+					shiftreg(7 downto 1) <= shiftreg(6 downto 0);
+					shiftreg(0) <= sda;
+					if (counter = 0) then
+						state <= next_state;
+						counter <= 7;
+					else
+						counter <= counter - 1;
+					end if;
+				end if;
+			elsif(state = S_RW) then
+				if (rising_scl = '1') then
+					if (shiftreg = DEVICE) then
+						state <= S_SENDACK;
+						if (sda = '1') then
+							operation <= OP_READ;
+							-- next_state <= S_READ; -- no needed
+							rd_d <= '1';
+						else
+							operation <= OP_WRITE;
+							next_state <= S_ADDRESS;
+							address_incr <= '0';
+						end if;
+					else
+						state <= S_SENDNACK;
+					end if;
+				end if;
+			elsif(state = S_SENDACK) then
+				WR <= '0';
+				rd_d <= '0';
+				if (falling_scl = '1') then
+					SDA_OUT <= '0';
+					counter <= 7;
+					if (operation= OP_WRITE) then
+						state <= S_SENDACK2;
+					else -- OP_READ
+						state <= S_SHIFTOUT;
+						shiftreg <= DATA_IN;
+					end if;
+				end if;
+			elsif(state = S_SENDACK2) then
+				if (falling_scl = '1') then
+					SDA_OUT <= '1';
+					state <= S_SHIFTIN;
+					shiftreg <= (others=>'0');
+					if (address_incr = '1') then
+						address_i <= next_address;
+					end if;
+				end if;
+			elsif(state = S_SENDNACK) then
+				if (falling_scl = '1') then
+					SDA_OUT <= '1';
+					state <= S_IDLE;
+				end if;
+			elsif(state = S_ADDRESS) then
+				address_i <= shiftreg;
+				next_state <= S_WRITE;
+				state <= S_SENDACK;
+				address_incr <= '0';
+			elsif(state = S_WRITE) then
+				DATA_OUT <= shiftreg;
+				next_state <= S_WRITE;
+				state <= S_SENDACK;
+				WR <= '1';
+				address_incr <= '1';
+			elsif(state = S_SHIFTOUT) then
+				if (falling_scl = '1') then
+					SDA_OUT <= shiftreg(7);
+					shiftreg(7 downto 1) <= shiftreg(6 downto 0);
+					shiftreg(0) <= '1';
+					if (counter = 0) then
+						state <= S_READ;
+						address_i <= next_address;
+						rd_d <= '1';
+					else
+						counter <= counter - 1;
+					end if;
+				end if;
+			elsif(state = S_READ) then
+				rd_d <= '0';
+				if (falling_scl = '1') then
+					SDA_OUT <= '1';
+					state <= S_WAITACK;
+				end if;
+			elsif(state = S_WAITACK) then
+				if (rising_scl = '1') then
+					if (sda = '0') then
+						state <= S_SHIFTOUT;
+						counter <= 7;
+						shiftreg <= DATA_IN;
+					else
+						state <= S_IDLE;
+					end if;
+				end if;
+			end if;
+		end if;
+	end process OTO;
+					
+
+end rtl;

fpga/principal.ucf

@@ -8,10 +8,10 @@ TIMESPEC "TS_CLK" = PERIOD "CLK" 20 ns HIGH 50 %;
 NET "BTN" LOC = "P41" | IOSTANDARD = LVTTL ;
 
 # IO<10>
-NET "RX" LOC = "P85" | IOSTANDARD = LVTTL ;
+NET "SCL" LOC = "P85" | IOSTANDARD = LVTTL ;
 
 # IO<11>
-NET "TX" LOC = "P84" | IOSTANDARD = LVTTL ;
+NET "SDA" LOC = "P84" | IOSTANDARD = LVTTL ;
 
 # IO<12>
 NET "LEFTCHA" LOC = "P83" | IOSTANDARD = LVTTL ;
@@ -44,7 +44,7 @@ NET "ENAREF" LOC = "P5" | IOSTANDARD = LVTTL ;
 NET "ENA" LOC = "P4" | IOSTANDARD = LVTTL ;
 
 # IO<22>
-NET "IN1ENC" LOC = "P6" | IOSTANDARD = LVTTL ;
+NET "IN1" LOC = "P6" | IOSTANDARD = LVTTL ;
 
 # IO<23>
 NET "IN2" LOC = "P98" | IOSTANDARD = LVTTL ;
@@ -56,7 +56,7 @@ NET "ENBREF" LOC = "P94" | IOSTANDARD = LVTTL ;
 NET "ENB" LOC = "P93" | IOSTANDARD = LVTTL ;
 
 # IO<26>
-NET "IN3END" LOC = "P90" | IOSTANDARD = LVTTL ;
+NET "IN3" LOC = "P90" | IOSTANDARD = LVTTL ;
 
 # IO<27>
 NET "IN4" LOC = "P89" | IOSTANDARD = LVTTL ;

fpga/project.cfg

@@ -5,5 +5,5 @@ PROGRAMMER = mercpcl
 
 TOPLEVEL = Principal
 # Prod
-VHDSOURCE = $(TOPLEVEL).vhd communication.vhd uart.vhd hedm.vhd hcsr04.vhd fir.vhd pwm.vhd
+VHDSOURCE = $(TOPLEVEL).vhd i2c.vhd hedm.vhd hcsr04.vhd fir.vhd pwm.vhd
 CONSTRAINTS = principal.ucf

raspberrypi/Makefile

@@ -130,6 +130,7 @@ restart:
 getlogs:
 	ssh -F sshconf principal true
 	rsync --rsh 'ssh -F sshconf' --archive principal:/opt/chef/log/* ../log/
+	rm -f ../log/last.csv; ln -s $$(ls ../log/ | tail -1) ../log/last.csv
 
 gdbcommands:
 	echo "set sysroot $(TARGET_DIR)" > "$@"

simu/simu.m

@@ -3,7 +3,7 @@ SIMULATION = 0;
 
 % Paramètres de lecture
 DIRNAME = "/home/geoffrey/CdF/cdf2018-principal/log/";
-FILENAME = "000655.csv";
+FILENAME = "last.csv";
 PATH = DIRNAME + FILENAME;
 
 % Paramètres de simulation
@@ -31,9 +31,9 @@ motorSpeedGainRPMpV = 233.0; % rpm/V (from datasheet)
 motorSpeedGain = (motorSpeedGainRPMpV / 60.0); % motor rev/s/V
 motorNominalTension = 24.0; % V (from datasheet)
 motorControllerAlimentation = 24.0; % V (from elec)
-motorControllerReference = 5; % V (from wiring)
+motorControllerReference = 5.0; % V (from wiring)
-motorSaturationMin = 0; % V (from random)
+motorSaturationMin = 0.0; % V (from random)
-motorSaturationMax = 3.0; % V (from testing)
+motorSaturationMax = 4.0; % V (from testing)
 pwmMax = 3.3; % V (from FPGA datasheet)
 coderResolution = 370.0; % cycles/motor rev
 coderDataFactor = 4.0; % increments/motor cycles
@@ -44,17 +44,37 @@ reducRatio = (cranReducIn / cranReducOut); % reduction ratio
 coderFullResolution = (coderDataResolution / reducRatio); % cycles / wheel rev
 avPerCycle = (wheelPerimeter / coderFullResolution); % mm
 
+% Pour éviter les pics de codeuse liées à la communication I2C
+absoluteMaxVitesseRobot = 10.0; % km/h
+absoluteMaxVitesseRobotMMpS = (absoluteMaxVitesseRobot * 10000.0 / 36.0); % mm/s
+absoluteMaxVitesseRobotRevpS = (absoluteMaxVitesseRobotMMpS / wheelPerimeter); % rev/s
+absoluteMaxVitesseRobotCycpS = (absoluteMaxVitesseRobotRevpS * coderFullResolution); % cycle/s
+
 % Constantes asservissement
-global dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oGain oEcartMax;
+global dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oEcartMin oEcartMax targetTensionRatio targetTension carotteDistance dKP dKI dKD oKP oKI oKD margeSecurite;
-dDirEcartMin = 7.0; % mm
+
-dDirEcartMax = 10.0; % mm
+
-oDirEcartMin = (5.0 / 360.0 * 2.0 * pi); % rad
+% Asservissement en distance
-oDirEcartMax = (25.0 / 360.0 * 2.0 * pi); % rad
+dDirEcartMin = 30.0; % mm
-oEcartMax = (25.0 / 360.0 * 2.0 * pi); % rad
+dDirEcartMax = 50.0; % mm
-oGain = 5.0;
+dKP = 0.05;
-P = 5.0;
+dKI = 0.0;
-I = 0.0;
+dKD = 0.0;
-D = 0.0;
+targetTensionRatio = 0.75;
+targetTension = (targetTensionRatio * motorSaturationMax); % V
+carotteDistance = (targetTension / dKP); % mm
+
+% Asservissement en angle
+oDirEcartMin = (25.0 / 360.0 * 2.0 * pi); % rad
+oDirEcartMax = (45.0 / 360.0 * 2.0 * pi); % rad
+oEcartMin = (25.0 / 360.0 * 2.0 * pi); % rad
+oEcartMax = (45.0 / 360.0 * 2.0 * pi); % rad
+oKP = (motorSaturationMax / (wheelPerimeter * pi)); % au max peut dérivier de pi
+oKI = 0.0;
+oKD = 0.0;
+carotteAngle = (targetTension / oKP); % mm
+
+margeSecurite = 300.0; % mm
 
 %END DIMENSIONS
 
@@ -89,6 +109,9 @@ else
     T.y = T.yConnu;
     T.o = T.oConnu;
     
+    T.secBackL = -T.secBackL;
+    T.secBackR = -T.secBackR;
+
     disp("Enregistrement des données");
     s = containers.Map;
     for name=T.Properties.VariableNames
@@ -109,14 +132,26 @@ clf
 global p;
 
 % Évolution spatiale
-subplot(2, 2, 1);
+subplot(2, 3, 1);
 initGraph
 updateToTime(SIMULATION_DT);
 
-% Roues
+% Codeuses
-p = subplot(2, 2, 2);
+p = subplot(2, 3, 3);
 hold on;
-timeGraph(["lVolt", "rVolt", "lErr", "rErr"]);
+timeGraph(["lCodTot", "rCodTot", "newL", "newR"]);
+addLimitline(p, 2^16-1);
+addLimitline(p, 0);
+title("Codeuses");
+xlabel("Temps (s)");
+ylabel("Crans");
+legend("Total gauche", "Total droite", "Brut gauche", "Brut droite");
+
+
+% Roues
+p = subplot(2, 3, 2);
+hold on;
+timeGraph(["lVolt", "rVolt", "dVolt", "oVolt"]);
 addLimitline(p, -motorSaturationMin);
 addLimitline(p, -motorSaturationMax);
 addLimitline(p, motorSaturationMin);
@@ -125,25 +160,26 @@ addLimitline(p, 0);
 title("Roues");
 xlabel("Temps (s)");
 ylabel("Tension (V)");
-legend("Gauche", "Droite", "Err. gauche", "Err. droite");
+legend("Tension gauche", "Tension droite", "Dont distance", "Dont direction");
 
 % Distance
-p = subplot(2, 2, 3);
+p = subplot(2, 3, 4);
 hold on;
-%timeGraph(["dDirEcart", "dErr"]);
+timeGraph(["dDirEcart", "dEcart", "oErr"]);
-timeGraph(["dDirEcart", "dErr", "secFront", "secBack"]);
+addLimitline(p, 0);
 addLimitline(p, dDirEcartMin);
 addLimitline(p, dDirEcartMax);
+addLimitline(p, -dDirEcartMin);
+addLimitline(p, -dDirEcartMax);
 title("Distance");
 xlabel("Temps (s)");
 ylabel("Distance (mm)");
-%legend("Ecart direction", "Err. retenue");
+legend("Err. distance", "Err. retenue", "Err rotation");
-legend("Ecart direction", "Err. retenue", "Distance avant", "Distance arrière");
 
 % Rotation
-p = subplot(2, 2, 4);
+p = subplot(2, 3, 5);
 hold on;
-timeGraph(["oDirEcart", "oEcart", "oErr"]);
+timeGraph(["oDirEcart", "oConsEcart", "oEcart"]);
 addLimitline(p, oDirEcartMax);
 addLimitline(p, oDirEcartMin);
 addLimitline(p, -oDirEcartMax);
@@ -151,7 +187,19 @@ addLimitline(p, -oDirEcartMin);
 title("Rotation");
 xlabel("Temps (s)");
 ylabel("Angle (rad)");
-legend("Ecart direction", "Ecart orientation", "Err. retenue");
+legend("Err. direction", "Err. consigne", "Err. retenue");
+
+% Securité
+p = subplot(2, 3, 6);
+hold on;
+timeGraph(["dDirEcart", "secFrontL", "secFrontR", "secBackL", "secBackR", "dErr"]);
+addLimitline(p, 0);
+addLimitline(p, margeSecurite);
+addLimitline(p, -margeSecurite);
+title("Distances de sécurité");
+xlabel("Temps (s)");
+ylabel("Distance (mm)");
+legend("Err. distance", "Avant gauche", "Avant droite", "Arrière gauche", "Arrière droite", "Err. retenue");
 
 % Fonctions
 
@@ -202,7 +250,7 @@ function timeGraph(series)
 end
 
 function addLimitline(p, x)
-    line(p.XLim, [x x], 'Color', [0.8 0.8 0.8]);
+    line(p.XLim, [x x], 'Color', [0.8 0.8 0.8], 'LineStyle', '--');
 end
 
 function addTimeline(p)
@@ -287,9 +335,9 @@ function initGraph()
     global robotPath;
     robotPath = plot(0, 0, 'b');
     global dirQuiver;
-    dirQuiver = quiver(0, 0, 0, 0, 'Color', 'Red', 'MaxHeadSize', height/4);
+    dirQuiver = quiver(0, 0, 0, 0, 'Color', 'Red', 'MaxHeadSize', height/2);
     global consQuiver;
-    consQuiver = quiver(0, 0, 0, 0, 'Color', 'Green', 'MaxHeadSize', height/4);
+    consQuiver = quiver(0, 0, 0, 0, 'Color', 'Green', 'MaxHeadSize', height/2);
     
     % Draw track
     global pisteWidth;