Movement

chef/Makefile

@@ -40,7 +40,7 @@ bin/premier: obj/premier.o $(OBJS_O)
 bin/test%: obj/test%.o $(OBJS_O)
 
 # Programme de test sur PC, n'embarquant pas wiringPi
-bin/local: obj/local.o obj/CF.o obj/debug.o obj/position.o
+bin/local: obj/local.o
 	$(CC) $(CFLAGS) $(CFLAGS_CUSTOM) -lpthread -lm $^ -o $@
 
 # Génération des fichiers objets

chef/src/CF.h

@@ -10,7 +10,7 @@
 
 #define FPGA_PORTNAME "/dev/ttyUSB0"
 #define CF_BAUDRATE B115200
-#define PRINTRAWDATA
+// #define PRINTRAWDATA
 
 int fpga;
 pthread_mutex_t sSendCF;

chef/src/debug.h

@@ -6,8 +6,8 @@
 #include <stdbool.h>
 
 // Constantes
-#define DEBUG_INTERVAL_IDLE 50
+#define DEBUG_INTERVAL_IDLE 1000
-#define DEBUG_INTERVAL_ACTIVE 1000
+#define DEBUG_INTERVAL_ACTIVE 50
 
 // Structures
 enum debugArgTypes {

chef/src/diagnostics.c

@@ -1,13 +1,13 @@
-#include <unistd.h>
 #include <stdio.h>
+#include <unistd.h>
 
-#include "diagnostics.h"
 #include "buttons.h"
+#include "diagnostics.h"
 #include "lcd.h"
 
 #include "CF.h"
-#include "motor.h"
 #include "imu.h"
+#include "motor.h"
 #include "position.h"
 
 bool recu;
@@ -50,24 +50,21 @@ bool diagCodeuse(void* arg)
     if (i % 2 == 1) { // Arrière
         tension = -tension;
     }
-    printf("49 %f\n", tension);
     if (i < 2) {
-        setPWMTension(tension, 0);
+        setMoteurTension(tension, 0);
     } else {
-        setPWMTension(0, tension);
+        setMoteurTension(0, tension);
     }
-    usleep(500*1000);
+    usleep(DIAGNOSTIC_TEMPS_ROTATION * 1000);
     brake();
     long lCodn, rCodn;
     getCoders(&lCodn, &rCodn);
     long diff;
-    printf("60 %ld %ld %ld %ld\n", lCod, lCodn, rCod, rCodn);
     if (i < 2) {
         diff = lCodn - lCod;
     } else {
         diff = rCodn - rCod;
     }
-    printf("65 %ld\n", diff);
     if (i % 2 == 0) { // Avant
         return (diff > DIAGNOSTIC_CODEUSES_DIFF_MIN);
     } else { // Arrière
@@ -102,9 +99,12 @@ void runDiagnostics()
     /* execDiagnostic("Lien Arduino", diagArduino); */
     execDiagnostic("Lien IMU", diagIMU, NULL);
     int i;
-    i = 0; execDiagnostic("Mot+Cod L AV", diagCodeuse, &i);
+    i = 0;
-    i = 1; execDiagnostic("Mot+Cod L AR", diagCodeuse, &i);
+    execDiagnostic("Mot+Cod L AV", diagCodeuse, &i);
-    i = 2; execDiagnostic("Mot+Cod R AV", diagCodeuse, &i);
+    i = 1;
-    i = 3; execDiagnostic("Mot+Cod R AR", diagCodeuse, &i);
+    execDiagnostic("Mot+Cod L AR", diagCodeuse, &i);
+    i = 2;
+    execDiagnostic("Mot+Cod R AV", diagCodeuse, &i);
+    i = 3;
+    execDiagnostic("Mot+Cod R AR", diagCodeuse, &i);
 }
-

chef/src/diagnostics.h

@@ -4,12 +4,13 @@
 #include <stdbool.h>
 
 // Constantes
-#define DIAGNOSTIC_INTERVAL 500
+#define DIAGNOSTIC_INTERVAL 100
 #define DIAGNOSTIC_POLL_INTERVAL 100
 #define DIAGNOSTIC_SERIAL_TIMEOUT 10000
 
-#define DIAGNOSTIC_TENSION_TEST 1
+#define DIAGNOSTIC_TENSION_TEST 3
 #define DIAGNOSTIC_CODEUSES_DIFF_MIN 1000
+#define DIAGNOSTIC_TEMPS_ROTATION 250
 
 // Public
 void runDiagnostics();

chef/src/dimensions.h

@@ -20,9 +20,9 @@
 #define MOTOR_NOMINAL_TENSION 24.0 // V (from datasheet)
 #define MOTOR_CONTROLLER_ALIMENTATION 24.0 // V (from elec)
 #define MOTOR_CONTROLLER_REFERENCE 5 // V (from wiring)
-#define MOTOR_SATURATION_MIN 1.0 // V (from random)
+#define MOTOR_SATURATION_MIN 0.1 // V (from random)
 #define MOTOR_SATURATION_MAX 12.0 // V (from testing)
-#define PWM_MAX 3.5 // V (from FPGA datasheet)
+#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
 #define CODER_DATA_RESOLUTION (CODER_RESOLUTION * CODER_DATA_FACTOR) // cycles/motor rev
@@ -33,10 +33,11 @@
 #define AV_PER_CYCLE (WHEEL_PERIMETER / CODER_FULL_RESOLUTION) // mm
 
 // Constantes asservissement
-#define D_DIR_ECART_MIN 1 // mm
+#define D_DIR_ECART_MIN 1.0 // mm
-#define D_DIR_ECART_MAX 5 // mm
+#define D_DIR_ECART_MAX 5.0 // mm
-#define O_DIR_ECART_MIN 1 / 360 * 2 * M_PI // rad
+#define O_DIR_ECART_MIN (2.5 / 360.0 * 2.0 * M_PI) // rad
-#define O_DIR_ECART_MAX 3 / 360 * 2 * M_PI // rad
+#define O_DIR_ECART_MAX (7.5 / 360.0 * 2.0 * M_PI) // rad
+#define O_GAIN 1
 #define P 2
 #define I 0
 #define D 0

chef/src/ihm.c

@@ -2,6 +2,7 @@
 #include <signal.h>
 #include <time.h>
 
+#include "position.h"
 #include "buttons.h"
 #include "diagnostics.h"
 #include "ihm.h"
@@ -115,7 +116,7 @@ void* TaskIHM(void* pdata)
             if (bout == rouge) {
                 clearLCD();
                 printToLCD(LCD_LINE_1, "Calibrage...");
-                delay(3000); // TODO
+                resetPosition();
                 clock_gettime(CLOCK_REALTIME, &calibrageLast);
             } else if (bout == jaune) {
                 break;

chef/src/local.c

@@ -5,31 +5,48 @@
 #include <time.h>   // random seed
 #include <unistd.h> // sleep
 
-#include "CF.h"
+pthread_mutex_t posConnu;
-#include "debug.h"
+pthread_cond_t newPos;
-#include "movement.h"
+pthread_t tPosition;
+pthread_t tMovement;
 
-pthread_mutex_t sRunning;
+void* TaskPosition(void* pData)
-
-void endRunning(int signal)
 {
-    (void)signal;
+
-    pthread_mutex_unlock(&sRunning);
+    (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);
-    configureDebug();
+    pthread_cond_init(&newPos, NULL);
-    configureCF();
+    pthread_create(&tPosition, NULL, TaskPosition, NULL);
-    configurePosition();
+    pthread_create(&tMovement, NULL, TaskMovement, NULL);
-
+    sleep(300);
-    /* long lCod, rCod; */
-    for (;;) {
-        sleep(1);
-        /* getCoders(&lCod, &rCod); */
-        /* printf("%ld %ld\n", lCod, rCod); */
-        /* usleep(100*1000); */
-    }
-
 }

chef/src/motor.c

@@ -2,20 +2,17 @@
 
 uint8_t tensionToPWM(float V)
 {
-    printf("PWM tension %f\n", V);
     if (V >= PWM_MAX) {
         return UINT8_MAX;
     } else if (V <= 0) {
         return 0;
     } else {
-        printf("PWM value %d\n", (uint8_t) (V * UINT8_MAX / PWM_MAX));
         return V * UINT8_MAX / PWM_MAX;
     }
 }
 
 uint8_t moteurTensionToPWM(float V)
 {
-    printf("Moteur tension %f\n", V);
     if (V >= MOTOR_CONTROLLER_ALIMENTATION) {
         V = MOTOR_CONTROLLER_ALIMENTATION;
     } else if (V <= 0) {
@@ -44,7 +41,7 @@ void setMoteurTensionRaw(float lVolt, float rVolt, bool lFor, bool rFor)
 #endif
     if (rVolt > 0) {
         msg.in |= 1 << (rFor ? IN3 : IN4);
-        msg.enb = moteurTensionToPWM(lVolt);
+        msg.enb = moteurTensionToPWM(rVolt);
     } else {
         // Nothing needs to be changed for this motor controller
     }

chef/src/movement.c

@@ -1,21 +1,160 @@
 #include <math.h>
-#include <stdbool.h>
+#include <pthread.h>
-#include <stdio.h>
 #include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
 
-#include "CF.h"
+#include "debug.h"
-#include "dimensions.h"
 #include "motor.h"
 #include "movement.h"
 
+pthread_t tMovement;
+struct position cons;
+pthread_mutex_t movCons;
+pthread_mutex_t movEnableMutex;
+pthread_cond_t movEnableCond;
+bool movEnableBool;
+
+float xDiff;
+float yDiff;
+float oEcart;
+float dDirEcart;
+float oDirEcart;
+float dErr;
+float oErr;
+bool oRetenu;
+bool dRetenu;
+float lErr;
+float rErr;
+unsigned int nbCalcCons;
+
 void configureMovement()
 {
     stop();
+
+    nbCalcCons = 0;
+    pthread_mutex_init(&movCons, NULL);
+
+    pthread_mutex_init(&movEnableMutex, NULL);
+    pthread_cond_init(&movEnableCond, NULL);
+    movEnableBool = false;
+
+    pthread_create(&tMovement, NULL, TaskMovement, NULL);
+
+    registerDebugVar("xCons", f, &cons.x);
+    registerDebugVar("yCons", f, &cons.y);
+    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("dDirEcart", f, &dDirEcart);
+    registerDebugVar("oDirEcart", f, &oDirEcart);
+    registerDebugVar("dRetenu", d, &dRetenu);
+    registerDebugVar("oRetenu", d, &oRetenu);
+    registerDebugVar("lErr", f, &lErr);
+    registerDebugVar("rErr", f, &rErr);
+    registerDebugVar("nbCalcCons", d, &nbCalcCons);
+
+    disableConsigne();
 }
 
-void aller(struct position* pos);
+void setDestination(struct position* pos)
+{
+    pthread_mutex_lock(&movCons);
+    memcpy(&cons, pos, sizeof(struct position));
+    pthread_mutex_unlock(&movCons);
+}
+
+float angleGap(float target, float actual)
+{
+    return fmod(target - actual + M_PI, 2 * M_PI) - M_PI;
+}
+
+void* TaskMovement(void* pData)
+{
+    (void)pData;
+
+    unsigned int lastPosCalc = 0;
+    struct position connu;
+
+    oRetenu = true;
+    dRetenu = true;
+
+    for (;;) {
+
+        // Test if enabled
+        pthread_mutex_lock(&movEnableMutex);
+        while (!movEnableBool) {
+            pthread_cond_wait(&movEnableCond, &movEnableMutex);
+        }
+        pthread_mutex_unlock(&movEnableMutex);
+
+        lastPosCalc = getPositionNewer(&connu, lastPosCalc);
+        // Destination → ordre
+        pthread_mutex_lock(&movCons);
+        xDiff = cons.x - connu.x;
+        yDiff = cons.y - connu.y;
+        oEcart = angleGap(cons.o, connu.o);
+
+        dDirEcart = hypotf(xDiff, yDiff);
+        oDirEcart = angleGap(atan2(yDiff, xDiff), connu.o);
+        pthread_mutex_unlock(&movCons);
+
+        if (dDirEcart > D_DIR_ECART_MAX) {
+            oRetenu = true;
+        } else if (dDirEcart < D_DIR_ECART_MIN) {
+            oRetenu = false;
+        }
+        oErr = oRetenu ? oDirEcart : oEcart;
+
+        float oDirEcartAbs = fabs(oDirEcart);
+        if (oDirEcartAbs > O_DIR_ECART_MAX) {
+            dRetenu = true;
+        } else if (oDirEcartAbs < O_DIR_ECART_MIN) {
+            dRetenu = false;
+        }
+        dErr = dRetenu ? 0 : dDirEcart;
+
+        // Ordre → Volt
+        float dErrRev = dErr / WHEEL_PERIMETER;
+        float oErrRev = O_GAIN * oErr * DISTANCE_BETWEEN_WHEELS / WHEEL_PERIMETER;
+
+        lErr = dErrRev - oErrRev;
+        rErr = dErrRev + oErrRev;
+
+        // PID
+        float lVoltCons = P * lErr;
+        float rVoltCons = P * rErr;
+
+        setMoteurTension(lVoltCons, rVoltCons);
+
+        nbCalcCons++;
+    }
+
+    return NULL;
+}
 
 void deconfigureMovement()
 {
     stop();
+    pthread_cancel(tMovement);
+}
+
+void enableConsigne()
+{
+    pthread_mutex_lock(&movEnableMutex);
+    movEnableBool = true;
+    pthread_cond_signal(&movEnableCond);
+    pthread_mutex_unlock(&movEnableMutex);
+}
+
+void disableConsigne()
+{
+    pthread_mutex_lock(&movEnableMutex);
+    movEnableBool = false;
+    // No signal here, will be disabled on next run
+    pthread_mutex_unlock(&movEnableMutex);
 }

chef/src/movement.h

@@ -9,5 +9,9 @@
 
 void configureMovement();
 void deconfigureMovement();
+void setDestination(struct position* pos);
+void* TaskMovement(void* pData);
+void enableConsigne();
+void disableConsigne();
 
 #endif

chef/src/parcours.c

@@ -32,6 +32,7 @@ void prepareParcours(bool orange)
     resetPoints();
     showPoints();
     printRightLCD(LCD_LINE_2, isOrange ? "Org" : "Vrt");
+    enableConsigne();
 }
 
 void startParcours()
@@ -71,6 +72,7 @@ void stopParcours()
 {
     pthread_cancel(tParcours);
     stop();
+    disableConsigne();
 
     updateTimeDisplay();
     printRightLCD(LCD_LINE_1, "FIN");
@@ -78,42 +80,24 @@ void stopParcours()
     debugSetActive(false);
 }
 
-#define UP_TIME 1000
-#define HIGH_TIME 3000
-#define DOWN_TIME 1000
-#define LOW_TIME 2000
-#define MAX_VIT 1
-
 void* TaskParcours(void* pdata)
 {
     (void)pdata;
 
-    for (;;) {
+    struct position dest1 = {0, 0, 0};
-        setPWMTension(MAX_VIT, MAX_VIT);
+    setDestination(&dest1);
-    }
 
-    for (;;) {
+    sleep(1);
-        addPoints(1);
-        for (int i = 0; i < UP_TIME; i++) {
-            float p = (float)i / (float)UP_TIME;
-            setPWMTension(p * MAX_VIT, p * MAX_VIT);
-            usleep(1000 * 1);
-        }
-        addPoints(1);
-        setPWMTension(MAX_VIT, MAX_VIT);
-        usleep(1000 * HIGH_TIME);
 
-        addPoints(1);
+    struct position dest2 = {300, -300, 0};
-        for (int i = 0; i < DOWN_TIME; i++) {
+    setDestination(&dest2);
-            float p = (float)i / (float)DOWN_TIME;
+
-            p = 1 - p;
+    sleep(5);
-            setPWMTension(p * MAX_VIT, p * MAX_VIT);
+    /*  */
-            usleep(1000 * 1);
+    /* struct position dest3 = {1000, 1000, 0}; */
-        }
+    /* setDestination(&dest3); */
-        addPoints(1);
+    /*  */
-        setPWMTension(0, 0);
+    /* sleep(5); */
-        usleep(1000 * LOW_TIME);
-    }
 
     return NULL;
 }

chef/src/position.c

@@ -2,17 +2,21 @@
  * Fonctions de calcul de la position du robot
  */
 
-#include <stdio.h>
 #include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
 
 #include "debug.h"
-#include "position.h"
 #include "dimensions.h"
+#include "position.h"
 
 // Globales
 struct position connu;
 struct F2CI_CODERs deltaCoders;
 pthread_mutex_t posPolling;
+pthread_mutex_t posConnu;
+pthread_cond_t newPos;
 pthread_t tPosition;
 
 // Globales
@@ -23,12 +27,11 @@ void* TaskPosition(void* pData)
 {
     (void)pData;
 
+    resetPosition();
     nbCalcPos = 0;
     lCodTot = 0;
     rCodTot = 0;
 
-    pthread_mutex_init(&posPolling, NULL);
-
     for (;;) {
 
         // Sending
@@ -40,7 +43,6 @@ void* TaskPosition(void* pData)
         pthread_mutex_unlock(&posPolling);
 
         // Calculation
-        nbCalcPos++;
 #ifdef INVERSE_L_CODER
         deltaCoders.dL = -deltaCoders.dL;
 #endif
@@ -56,11 +58,17 @@ void* TaskPosition(void* pData)
         float deltaO = atan2(dR - dL, DISTANCE_BETWEEN_WHEELS);
         float deltaD = (dL + dR) / 2;
 
+        pthread_mutex_lock(&posConnu);
         connu.o += deltaO;
         float deltaX = deltaD * cos(connu.o);
         float deltaY = deltaD * sin(connu.o);
         connu.x += deltaX;
         connu.y += deltaY;
+        nbCalcPos++;
+        pthread_cond_signal(&newPos);
+        pthread_mutex_unlock(&posConnu);
+
+
     }
 
     return NULL;
@@ -74,6 +82,9 @@ void onF2CI_CODER()
 
 void configurePosition()
 {
+    pthread_mutex_init(&posPolling, NULL);
+    pthread_mutex_init(&posConnu, NULL);
+    pthread_cond_init(&newPos, NULL);
     registerRxHandler(F2CI_CODER, onF2CI_CODER);
     registerDebugVar("lCodTot", ld, &lCodTot);
     registerDebugVar("rCodTot", ld, &rCodTot);
@@ -96,5 +107,36 @@ void getCoders(long* l, long* r)
 {
     *l = lCodTot;
     *r = rCodTot;
-
+}
+
+unsigned int getPosition(struct position* pos)
+{
+    pthread_mutex_lock(&posConnu);
+    unsigned int nb = nbCalcPos;
+    memcpy(pos, &connu, sizeof(struct position));
+    pthread_mutex_unlock(&posConnu);
+    return nb;
+}
+
+unsigned int getPositionNewer(struct position* pos, unsigned int lastCalc)
+{
+    pthread_mutex_lock(&posConnu);
+    while (nbCalcPos <= lastCalc) {
+        pthread_cond_wait(&newPos, &posConnu);
+    }
+    pthread_mutex_unlock(&posConnu);
+    return getPosition(pos);
+}
+
+void setPosition(struct position* pos)
+{
+    pthread_mutex_lock(&posConnu);
+    memcpy(&connu, pos, sizeof(struct position));
+    pthread_mutex_unlock(&posConnu);
+}
+
+void resetPosition()
+{
+    struct position pos = {0, 0, 0};
+    setPosition(&pos);
 }

chef/src/position.h

@@ -22,6 +22,10 @@ struct __attribute__ ((packed)) position {
 void configurePosition();
 void deconfigurePosition();
 void getCoders(long* l, long* r);
+unsigned int getPositionNewer(struct position* pos, unsigned int lastCalc);
+unsigned int getPosition(struct position* pos);
+void setPosition(struct position* pos);
+void resetPosition();
 
 #endif
 

chef/src/premier.c

@@ -36,8 +36,8 @@ int main()
     configureIHM();
     configureCF();
     configureIMU();
-    configureMovement();
     configurePosition();
+    configureMovement();
     startDebug();
     startIHM();
 
@@ -49,8 +49,8 @@ int main()
     pthread_mutex_lock(&sRunning);
     pthread_mutex_lock(&sRunning);
 
-    deconfigurePosition();
     deconfigureMovement();
+    deconfigurePosition();
     deconfigureIMU();
     deconfigureCF();
     deconfigureIHM();

chef/src/testFree.c

@@ -0,0 +1,16 @@
+/* Teste si une broche est connecté à une autre */
+
+#include <stdlib.h>
+
+#include "CF.h"
+#include "motor.h"
+
+int main(int argc, char* argv[])
+{
+
+    (void)argc;
+    (void)argv;
+
+    configureCF();
+    freewheel();
+}

chef/src/testParcours.c

@@ -0,0 +1,55 @@
+#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 "debug.h"
+#include "i2c.h"
+#include "imu.h"
+#include "movement.h"
+#include "buttons.h"
+#include "parcours.h"
+#include "position.h"
+
+pthread_mutex_t sRunning;
+
+int main()
+{
+
+    if (wiringPiSetup() < 0) {
+        fprintf(stderr, "Impossible d'initialiser WiringPi\n");
+        exit(EXIT_FAILURE);
+    }
+    initI2C();
+    srand(time(NULL));
+
+    configureDebug();
+    configureCF();
+    configureIMU();
+    configurePosition();
+    configureMovement();
+    startDebug();
+
+    configureParcours();
+    prepareParcours(false);
+    startParcours();
+
+    int toWait;
+    while ((toWait = updateParcours()) >= 0) {
+        if (pressedButton(toWait) != none) {
+            break;
+        }
+    }
+    stopParcours();
+
+    deconfigureMovement();
+    deconfigurePosition();
+    deconfigureIMU();
+    deconfigureCF();
+    deconfigureDebug();
+    return EXIT_SUCCESS;
+}

chef/src/testSlow.c

@@ -14,7 +14,7 @@
 
 #define VIT 1
 #define VITL VIT
-#define VITR 0
+#define VITR VIT
 
 
 void setPWMTensionWrapper(float l, float r) {

raspberrypi/board/robotech/cdfprincipal/rootfs_overlay/root/.profile

@@ -21,6 +21,12 @@ s()
     /opt/chef/bin/testStop
 }
 
+f()
+{
+    /etc/init.d/S50chef stop
+    /opt/chef/bin/testFree
+}
+
 l()
 {
     tail -f $(find /opt/chef/log | sort | tail -1)