Meilleur 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 common debug diagnostics dimensions fpga i2c imu ihm lcd motor movement parcours points position securite
+OBJS=actionneurs buttons CA calibrage 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

@@ -44,3 +44,25 @@ float updatePID(struct PID *pid, float err)
     return pid->KP * err + pid->KI * pid->integErr + pid->KP * derivErr;
 }
 
+void initMovAvg(struct movAvg *movavg, size_t size)
+{
+    movavg->size = size;
+    movavg->actuel = 0;
+    movavg->table = malloc(movavg->size * sizeof(float));
+    for (size_t i = 0; i < movavg->size; i++) {
+        movavg->table[i] = 0.0;
+    }
+    movavg->total = 0.0;
+}
+
+void addMovAvg(struct movAvg *movavg, float val)
+{
+    movavg->total -= movavg->table[movavg->actuel];
+    movavg->table[movavg->actuel] = val;
+    movavg->total += movavg->table[movavg->actuel];
+    movavg->actuel++;
+    movavg->current = movavg->total / (double) movavg->size;
+    if (movavg->actuel >= movavg->size) {
+        movavg->actuel = 0;
+    }
+}

chef/src/common.h

@@ -2,7 +2,12 @@
 #define __COMMON_H_
 
 #include <time.h>
+#include <stdlib.h>
 
+void diffTime(const struct timespec* debut, const struct timespec* fin, struct timespec* ecoule);
+float diffTimeSec(const struct timespec* debut, const struct timespec* fin);
+
+// PID
 struct PID {
     struct timespec lastCalc;
     float KP;
@@ -12,10 +17,21 @@ struct PID {
     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);
 
+// Moving average
+struct movAvg {
+    size_t size;
+    size_t actuel;
+    float* table;
+    double total;
+    float current;
+};
+
+void initMovAvg(struct movAvg *movavg, size_t size);
+void addMovAvg(struct movAvg *movavg, float val);
+
+
 #endif

chef/src/diagnostics.c

@@ -150,13 +150,13 @@ void runDiagnostics()
 
     execDiagnostic("Ouverture loquet", diagJustRun, &diagSetLoquetOuvert);
     execDiagnostic("Fermeture loquet", diagJustRun, &diagSetLoquetFerme);
-    execDiagnostic("Reset barillet", diagJustRun, &barilletReset);
+    /* execDiagnostic("Reset barillet", diagJustRun, &barilletReset); */
-    execDiagnostic("T+1 barillet", diagJustRun, &barilletSuivant);
+    /* execDiagnostic("T+1 barillet", diagJustRun, &barilletSuivant); */
-    execDiagnostic("T+2 barillet", diagJustRun, &barilletSkip);
+    /* execDiagnostic("T+2 barillet", diagJustRun, &barilletSkip); */
-    execDiagnostic("Pousser balle", diagJustRun, &pousserBalle);
+    /* execDiagnostic("Pousser balle", diagJustRun, &pousserBalle); */
-    execDiagnostic("Pos. ejection", diagJustRun, &diagSetPositionBalleEjection);
+    /* execDiagnostic("Pos. ejection", diagJustRun, &diagSetPositionBalleEjection); */
-    execDiagnostic("Pos. evacuation", diagJustRun, &diagSetPositionBalleEvacuation);
+    /* execDiagnostic("Pos. evacuation", diagJustRun, &diagSetPositionBalleEvacuation); */
-    execDiagnostic("Pos. attente", diagJustRun, &diagSetPositionBalleAttente);
+    /* execDiagnostic("Pos. attente", diagJustRun, &diagSetPositionBalleAttente); */
-    execDiagnostic("Propulsion on", diagJustRun, &diagSetPropulsionOff);
+    /* execDiagnostic("Propulsion on", diagJustRun, &diagSetPropulsionOff); */
-    execDiagnostic("Propulsion off", diagJustRun, &diagSetPropulsionOn);
+    /* execDiagnostic("Propulsion off", diagJustRun, &diagSetPropulsionOn); */
 }

chef/src/dimensions.h

@@ -40,27 +40,30 @@
 
 // Constantes asservissement
 
+// Asservissement en angle
+#define O_VIT_MIN 0.5 // rad/s
+#define O_TENSION_MIN 1 // V
+#define O_DIR_ECART_MIN (20.0 / 360.0 * 2.0 * M_PI) // rad
+#define O_ECART_MIN (10.0 / 360.0 * 2.0 * M_PI) // rad
+#define O_ECART_MAX (20.0 / 360.0 * 2.0 * M_PI) // rad
+#define DERIV_M_PI (MOTOR_SATURATION_MAX / (WHEEL_PERIMETER * M_PI))
+#define O_KP (3.0 * DERIV_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
 
 // Asservissement en distance
-#define D_DIR_ECART_MIN 30.0 // mm
+#define D_VIT_MIN 10.0 // mm/s
-#define D_DIR_ECART_MAX 50.0 // mm
+#define D_TENSION_MIN 1 // V
-#define D_KP 0.05
+#define D_DIR_ECART_MIN 20.0 // mm
+#define D_DIR_ECART_MAX 70.0 // mm
+#define D_KP 0.1
 #define D_KI 0.0
 #define D_KD 0.0
 #define TARGET_TENSION_RATIO 0.75
 #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/ihm.c

@@ -2,12 +2,13 @@
 #include <signal.h>
 #include <time.h>
 
-#include "position.h"
 #include "buttons.h"
+#include "calibrage.h"
 #include "diagnostics.h"
 #include "ihm.h"
 #include "lcd.h"
 #include "parcours.h"
+#include "position.h"
 
 // Globales
 pthread_t tIHM;
@@ -116,7 +117,7 @@ void* TaskIHM(void* pdata)
             if (bout == rouge) {
                 clearLCD();
                 printToLCD(LCD_LINE_1, "Calibrage...");
-                resetPosition();
+                calibrer(isOrange);
                 clock_gettime(CLOCK_REALTIME, &calibrageLast);
             } else if (bout == jaune) {
                 break;
@@ -191,7 +192,7 @@ void* TaskIHM(void* pdata)
             if (bout == rouge) {
                 clearLCD();
                 printToLCD(LCD_LINE_1, "Remise a zero...");
-                delay(3000); // TODO
+                resetPosition();
             } else if (bout == jaune) {
                 break;
             }

chef/src/movement.c

@@ -33,8 +33,12 @@ float dVolt;
 float oVolt;
 float lErr;
 float rErr;
+enum movStates etat;
 unsigned int nbCalcCons;
 
+bool secuAv = true;
+bool secuAr = true;
+
 void configureMovement()
 {
     stop();
@@ -66,6 +70,7 @@ void configureMovement()
     registerDebugVar("oConsEcart", f, &oConsEcart);
     registerDebugVar("lErr", f, &lErr);
     registerDebugVar("rErr", f, &rErr);
+    registerDebugVar("etat", d, &etat);
     registerDebugVar("nbCalcCons", d, &nbCalcCons);
 }
 
@@ -96,10 +101,9 @@ void* TaskMovement(void* pData)
     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;
+    bool obstacle;
+    etat = quelconque;
 
     for (;;) {
 
@@ -109,35 +113,102 @@ void* TaskMovement(void* pData)
         yDiff = cons.y - connu.y;
         dDirEcart = hypotf(xDiff, yDiff);
         oDirEcart = angleMod(atan2(yDiff, xDiff) - connu.o);
-        oConsEcart = angleMod(cons.o - connu.o);
+        oConsEcart = isnan(cons.o) ? 0 : angleMod(cons.o - connu.o);
 
         if ((reverse = fabsf(oDirEcart) > M_PI_2)) {
             dDirEcart = -dDirEcart;
-            oDirEcart = angleMod(oDirEcart + M_PI);
+            oDirEcart = angleMod(atan2(yDiff, xDiff) - connu.o + M_PI);
         }
 
-        if (fabsf(oDirEcart) < O_DIR_ECART_MIN) {
+        // Selection de l'état suivant
-            orienteDestination = true;
+        switch (etat) {
-        } else if (fabsf(oDirEcart) > O_DIR_ECART_MAX) {
+        case quelconque:
-            orienteDestination = false;
+            if (fabs(oDirEcart) < O_DIR_ECART_MIN) {
+                etat = direction;
+            }
+            break;
+        case direction:
+            if (fabs(getAnglVitesse()) < O_VIT_MIN && oVolt < O_TENSION_MIN) {
+                etat = approche;
+            }
+            break;
+        case approche:
+            if (fabs(dDirEcart) < D_DIR_ECART_MIN) {
+                etat = arret;
+            }
+            break;
+        case arret:
+            if (fabs(dDirEcart) > D_DIR_ECART_MAX) {
+                etat = quelconque;
+            } else if (fabs(getAbsVitesse()) < D_VIT_MIN && dVolt < D_TENSION_MIN) {
+                etat = orientation;
+            }
+            break;
+        case orientation:
+            if (fabs(dDirEcart) > D_DIR_ECART_MAX) {
+                etat = quelconque;
+            } else if (fabs(oConsEcart) < O_ECART_MIN) {
+                etat = oriente;
+            }
+            break;
+        case oriente:
+            if (fabs(dDirEcart) > D_DIR_ECART_MAX) {
+                etat = quelconque;
+            } else if (fabs(oConsEcart) > O_ECART_MAX) {
+                etat = orientation;
+            } else if (fabs(getAnglVitesse()) < O_VIT_MIN && oVolt < O_TENSION_MIN) {
+                etat = fini;
+            }
+            break;
+        case fini:
+            if (fabs(dDirEcart) > D_DIR_ECART_MAX) {
+                etat = quelconque;
+            } else if (fabs(oConsEcart) > O_ECART_MAX) {
+                etat = orientation;
+            }
+            break;
         }
 
-        if (fabsf(dDirEcart) < D_DIR_ECART_MIN) {
+        // Application des directives d'état
-            procheDestination = true;
+        switch (etat) {
-        } else if (fabsf(dDirEcart) > D_DIR_ECART_MAX) {
+        case quelconque:
-            procheDestination = false;
+        case direction:
+            oEcart = oDirEcart;
+            dEcart = 0;
+            break;
+        case approche:
+            oEcart = oDirEcart;
+            dEcart = dDirEcart;
+            break;
+        case arret:
+            oEcart = 0;
+            dEcart = dDirEcart;
+            break;
+        case orientation:
+        case oriente:
+            oEcart = oConsEcart;
+            dEcart = 0;
+            break;
+        case fini:
+            oEcart = 0;
+            dEcart = 0;
+            break;
         }
 
-        if (fabsf(oConsEcart) < O_ECART_MIN) {
+#ifdef ENABLE_SECURITE
-            orienteConsigne = true;
+        float av, ar;
-        } else if (fabsf(oConsEcart) > O_ECART_MAX) {
+        getDistance(&av, &ar);
-            orienteConsigne = false;
+        if (!reverse) {
+            obstacle = secuAv && av < MARGE_SECURITE;
+            /* dEcart = fmax(av, dEcart); */
+        } else {
+            obstacle = secuAr && ar < MARGE_SECURITE;
+            /* dEcart = fmin(-ar, dEcart); */
         }
+#endif
 
         // 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);
@@ -148,11 +219,12 @@ void* TaskMovement(void* pData)
 
         pthread_mutex_lock(&movInstructionMutex);
         if (movInstructionBool) {
-            if (procheDestination && orienteConsigne) {
+            if (obstacle || etat == fini) {
                 brake();
             } else {
                 setMoteurTension(lVolt, rVolt);
             }
+            pthread_cond_signal(&movInstructionCond);
         }
         pthread_mutex_unlock(&movInstructionMutex);
 
@@ -173,22 +245,32 @@ void disableAsservissement()
 {
     pthread_mutex_lock(&movInstructionMutex);
     movInstructionBool = false;
+    etat = quelconque;
     pthread_mutex_unlock(&movInstructionMutex);
 }
 
 void setDestination(struct position* pos)
 {
     pthread_mutex_lock(&movInstructionMutex);
+    etat = quelconque;
     memcpy(&cons, pos, sizeof(struct position));
     movInstructionBool = true;
     pthread_cond_signal(&movInstructionCond);
     pthread_mutex_unlock(&movInstructionMutex);
 }
 
+void setSecurite(bool av, bool ar)
+{
+    pthread_mutex_lock(&movInstructionMutex);
+    secuAv = av;
+    secuAr = ar;
+    pthread_mutex_unlock(&movInstructionMutex);
+}
+
 void waitDestination()
 {
     pthread_mutex_lock(&movInstructionMutex);
-    while (movInstructionBool) {
+    while (etat != fini) {
         pthread_cond_wait(&movInstructionCond, &movInstructionMutex);
     }
     pthread_mutex_unlock(&movInstructionMutex);

chef/src/movement.h

@@ -9,8 +9,20 @@
 
 // #define ENABLE_SECURITE
 
+#include <stdbool.h>
+
 #include "position.h"
 
+enum movStates {
+    quelconque,  // 0
+    direction,   // 1
+    approche,    // 2
+    arret,       // 3
+    orientation, // 4
+    oriente,     // 5
+    fini         // 6
+};
+
 // Public
 void configureMovement();
 void deconfigureMovement();
@@ -18,6 +30,7 @@ void setDestination(struct position* pos);
 void waitDestination();
 void enableAsservissement();
 void disableAsservissement();
+void setSecurite(bool av, bool ar);
 
 // Private
 void* TaskMovement(void* pData);

chef/src/parcours.c

@@ -73,6 +73,7 @@ int updateParcours()
 void stopParcours()
 {
     pthread_cancel(tParcours);
+    disableAsservissement();
     stop();
 
     resetLCD();
@@ -90,12 +91,15 @@ void gotoPoint(float x, float y, float o)
     /*         o = M_PI - o; */
     /*     } */
     /* } */
-    if (!isOrange) {
+    if (isOrange) {
         o = -o;
+        y = -y;
     }
     struct position pos = { x, y, o };
     setDestination(&pos);
+    printf("New dest : %f %f %f\n", pos.x, pos.y, pos.o);
     waitDestination();
+    printf("Done.\n");
     brake();
 }
 
@@ -112,14 +116,23 @@ void* TaskParcours(void* pdata)
 {
     (void)pdata;
 
-    /* gotoPoint(350, 0, 1.05*M_PI/3.0); */
+    float x = 306 + (isOrange ? 170 : 0);
-    gotoPoint(500, 0, 0);
+
-    waitDestination();
+    setSecurite(true, false);
-    for (int i = 0; i < 5; i++) {
+    gotoPoint(x, 200, -M_PI_2);
-        setLoquet(false);
+    setSecurite(false, true);
+    gotoPoint(x, 20, -M_PI_2);
+    brake();
+    for (int i = 0; i < 3; i++) {
         setLoquet(true);
+        setLoquet(false);
     }
-    gotoPoint(0, 0, 0);
+    addPoints(10);
+    gotoPoint(x, 200, ANGLE_INSIGNIFIANT);
+    setSecurite(true, false);
+    gotoPoint(600, 50, ANGLE_INSIGNIFIANT);
+    disableAsservissement();
+    stop();
     return NULL;
 }
 

chef/src/position.c

@@ -21,6 +21,10 @@ pthread_mutex_t posConnu;
 pthread_cond_t newPos;
 pthread_t tPosition;
 
+struct movAvg xVit;
+struct movAvg yVit;
+struct movAvg oVit;
+
 // Globales
 unsigned int nbCalcPos;
 long lCodTot, rCodTot;
@@ -28,30 +32,37 @@ long lCodTot, rCodTot;
 uint16_t oldL, oldR;
 uint16_t newL, newR;
 int16_t deltaL, deltaR;
+float deltaT;
 int newLdbg, newRdbg;
 
 struct timespec lastCoderRead;
 
 void updateDelta()
 {
+    // Récupération des valeurs
     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)) & 0xFFFF;
     newLdbg = newL;
     newRdbg = newR;
+
+    // Calcul des deltaL et deltaR
     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
+    // Calcul de deltaT
     struct timespec now;
     clock_gettime(CLOCK_REALTIME, &now);
-    float maxDelta = diffTimeSec(&lastCoderRead, &now) * ABSOLUTE_MAX_VITESSE_ROBOT_CYCP_S;
+    deltaT = diffTimeSec(&lastCoderRead, &now);
+    lastCoderRead = now;
+
+    // Verification de valeur abbérante
+    float maxDelta = deltaT * ABSOLUTE_MAX_VITESSE_ROBOT_CYCP_S;
     if (abs(deltaL) > maxDelta) {
         deltaL = 0;
     }
     if (abs(deltaR) > maxDelta) {
         deltaR = 0;
     }
-    lastCoderRead = now;
 
     oldL = newL;
     oldR = newR;
@@ -82,18 +93,28 @@ void* TaskPosition(void* pData)
         lCodTot += deltaL;
         rCodTot += deltaR;
 
+        // Calcul delta en mm
         float dR = deltaR * AV_PER_CYCLE;
         float dL = deltaL * AV_PER_CYCLE;
 
+        // Calcul delta en angle et en distance
         float deltaO = atan2(dR - dL, DISTANCE_BETWEEN_WHEELS);
         float deltaD = (dL + dR) / 2;
 
         pthread_mutex_lock(&posConnu);
+
+        // Modification des valeurs de position actuelle
         connu.o += deltaO;
         float deltaX = deltaD * cos(connu.o);
         float deltaY = deltaD * sin(connu.o);
         connu.x += deltaX;
         connu.y += deltaY;
+
+        // Modification des valeurs de vitesse
+        addMovAvg(&xVit, deltaX / deltaT);
+        addMovAvg(&yVit, deltaY / deltaT);
+        addMovAvg(&oVit, deltaO / deltaT);
+
         nbCalcPos++;
         pthread_cond_signal(&newPos);
         pthread_mutex_unlock(&posConnu);
@@ -106,6 +127,9 @@ void* TaskPosition(void* pData)
 
 void configurePosition()
 {
+    initMovAvg(&xVit, VIT_MOVAVG_SIZE);
+    initMovAvg(&yVit, VIT_MOVAVG_SIZE);
+    initMovAvg(&oVit, VIT_MOVAVG_SIZE);
     resetPosition();
     registerDebugVar("lCodTot", ld, &lCodTot);
     registerDebugVar("rCodTot", ld, &rCodTot);
@@ -114,6 +138,9 @@ void configurePosition()
     registerDebugVar("xConnu", f, &connu.x);
     registerDebugVar("yConnu", f, &connu.y);
     registerDebugVar("oConnu", f, &connu.o);
+    registerDebugVar("xVit", f, &xVit.current);
+    registerDebugVar("yVit", f, &yVit.current);
+    registerDebugVar("oVit", f, &oVit.current);
     registerDebugVar("nbCalcPos", d, &nbCalcPos);
     pthread_mutex_init(&posConnu, NULL);
     pthread_cond_init(&newPos, NULL);
@@ -140,6 +167,16 @@ unsigned int getPosition(struct position* pos)
     return nb;
 }
 
+float getAbsVitesse()
+{
+    return hypotf(xVit.current, yVit.current);
+}
+
+float getAnglVitesse()
+{
+    return oVit.current;
+}
+
 unsigned int getPositionNewer(struct position* pos, unsigned int lastCalc)
 {
     pthread_mutex_lock(&posConnu);

chef/src/position.h

@@ -10,8 +10,11 @@
 
 #define POSITION_INTERVAL 10
 
+#define VIT_MOVAVG_TIME 100
+#define VIT_MOVAVG_SIZE (VIT_MOVAVG_TIME / POSITION_INTERVAL)
+
 // Structures
-struct __attribute__ ((packed)) position {
+struct __attribute__((packed)) position {
     float x;
     float y;
     float o;
@@ -25,6 +28,7 @@ unsigned int getPositionNewer(struct position* pos, unsigned int lastCalc);
 unsigned int getPosition(struct position* pos);
 void setPosition(struct position* pos);
 void resetPosition();
+float getAnglVitesse();
+float getAbsVitesse();
 
 #endif
-

chef/src/testRetour.c

@@ -39,8 +39,11 @@ int main()
     disableAsservissement();
     freewheel();
 
+    enum boutons but = pressedButton(BUT_BLOCK);
+    clearLCD();
+
     for (;;) {
-        switch (pressedButton(BUT_BLOCK)) {
+        switch (but) {
         case jaune:
             isFree = !isFree;
             if (isFree) {
@@ -56,12 +59,13 @@ int main()
         default:
             break;
         }
-        clearLCD();
+
-        if (isFree) {
+        struct position pos;
-            printToLCD(LCD_LINE_1, "Freewheel");
+        getPosition(&pos);
-        } else {
+
-            printToLCD(LCD_LINE_1, "Asservi");
+        printfToLCD(LCD_LINE_1, "X% 4g Y% 4g ", pos.x, pos.y);
-        }
+        printfToLCD(LCD_LINE_2, "O% 10g %s", pos.o, (isFree ? "FREE" : "ASRV"));
+        but = pressedButton(100);
     }
 
     deconfigureMovement();

simu/simu.m

@@ -3,7 +3,7 @@ SIMULATION = 0;
 
 % Paramètres de lecture
 DIRNAME = "/home/geoffrey/CdF/cdf2018-principal/log/";
-FILENAME = "last.csv";
+FILENAME = "001418.csv";
 PATH = DIRNAME + FILENAME;
 
 % Paramètres de simulation
@@ -51,33 +51,37 @@ absoluteMaxVitesseRobotRevpS = (absoluteMaxVitesseRobotMMpS / wheelPerimeter); %
 absoluteMaxVitesseRobotCycpS = (absoluteMaxVitesseRobotRevpS * coderFullResolution); % cycle/s
 
 % Constantes asservissement
-global dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oEcartMin oEcartMax targetTensionRatio targetTension carotteDistance dKP dKI dKD oKP oKI oKD margeSecurite;
+global oTensionMin dTensionMin oVitMin dVitMin dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oEcartMin oEcartMax targetTensionRatio targetTension carotteDistance carotteAngle dKP dKI dKD oKP oKI oKD margeSecurite;
 
+% Asservissement en angle
+oVitMin = 0.5; % rad/s
+oTensionMin = 1; % V
+oDirEcartMin = (20.0 / 360.0 * 2.0 * pi); % rad
+oEcartMin = (10.0 / 360.0 * 2.0 * pi); % rad
+oEcartMax = (20.0 / 360.0 * 2.0 * pi); % rad
+derivPi = (motorSaturationMax / (wheelPerimeter * pi));
+oKP = (3.0 * derivPi); % au max peut dérivier de pi
+oKI = 0.0;
+oKD = 0.0;
+carotteAngle = (targetTension / oKP); % mm
 
 % Asservissement en distance
-dDirEcartMin = 30.0; % mm
+dVitMin = 10.0; % mm/s
-dDirEcartMax = 50.0; % mm
+dTensionMin = 1; % V
-dKP = 0.05;
+dDirEcartMin = 20.0; % mm
+dDirEcartMax = 70.0; % mm
+dKP = 0.1;
 dKI = 0.0;
 dKD = 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
 
+
 global s;
 if SIMULATION == 1
     % Génération de la consigne
@@ -128,80 +132,27 @@ end
 
 % Graphes
 
-clf
+global curGraph graphWidth graphHeight;
-global p;
+curGraph = 1;
+graphWidth = 3;
+graphHeight = 2;
+
+clf
+graphSpatiale();
+graphRoues();
+graphCodeuses();
+graphDistance();
+graphRotation();
+graphSecurite();
+%graphVitesseDist();
+%graphVitesseAngl();
+%graphEtat();
 
-% Évolution spatiale
-subplot(2, 3, 1);
-initGraph
 updateToTime(SIMULATION_DT);
 
-% Codeuses
+% FONCTIONS
-p = subplot(2, 3, 3);
-hold on;
-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");
 
-
+% Données
-% Roues
-p = subplot(2, 3, 2);
-hold on;
-timeGraph(["lVolt", "rVolt", "dVolt", "oVolt"]);
-addLimitline(p, -motorSaturationMin);
-addLimitline(p, -motorSaturationMax);
-addLimitline(p, motorSaturationMin);
-addLimitline(p, motorSaturationMax);
-addLimitline(p, 0);
-title("Roues");
-xlabel("Temps (s)");
-ylabel("Tension (V)");
-legend("Tension gauche", "Tension droite", "Dont distance", "Dont direction");
-
-% Distance
-p = subplot(2, 3, 4);
-hold on;
-timeGraph(["dDirEcart", "dEcart", "oErr"]);
-addLimitline(p, 0);
-addLimitline(p, dDirEcartMin);
-addLimitline(p, dDirEcartMax);
-addLimitline(p, -dDirEcartMin);
-addLimitline(p, -dDirEcartMax);
-title("Distance");
-xlabel("Temps (s)");
-ylabel("Distance (mm)");
-legend("Err. distance", "Err. retenue", "Err rotation");
-
-% Rotation
-p = subplot(2, 3, 5);
-hold on;
-timeGraph(["oDirEcart", "oConsEcart", "oEcart"]);
-addLimitline(p, oDirEcartMax);
-addLimitline(p, oDirEcartMin);
-addLimitline(p, -oDirEcartMax);
-addLimitline(p, -oDirEcartMin);
-title("Rotation");
-xlabel("Temps (s)");
-ylabel("Angle (rad)");
-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
 
 function ts = getTS(name)
     global SIMULATION s;
@@ -228,6 +179,33 @@ function pt = getTimePoints()
     end
 end
 
+function d = getTSData(name, i)
+    ts = getTS(name);
+    if isempty(ts.Data)
+        d = 0;
+    else
+        d = ts.Data(i);
+    end
+end
+
+% Dessin
+
+function [x, y] = pointArround(xC, yC, xD, yD, o)
+    D = xD + yD * 1i;
+    F = abs(D) .* exp(1i * (angle(D) + o - pi/2));
+    x = xC + real(F);
+    y = yC + imag(F);
+end
+
+function drawRect(p, x, y, o, w, h)
+    [x1, y1] = pointArround(x, y, - w/2, + h/2, o);
+    [x2, y2] = pointArround(x, y, + w/2, + h/2, o);
+    [x3, y3] = pointArround(x, y, + w/2, - h/2, o);
+    [x4, y4] = pointArround(x, y, - w/2, - h/2, o);
+    p.XData = [x1, x2, x3, x4, x1];
+    p.YData = [y1, y2, y3, y4, y1];
+end
+
 function timeGraph(series)
     global SIMULATION_TIME p;
     m = inf;
@@ -259,55 +237,18 @@ function addTimeline(p)
     timelines = [timelines timeline];
 end
 
-function play()
+% Graphiques
-    global SIMULATION_TIME speed t playing;
+
-    if playing == 1
+function p = newGraph()
-        return
+    global curGraph graphWidth graphHeight p;
-    end
+    fprintf("Graphe %d/%d\n", curGraph, graphWidth * graphHeight);
-    startCpu=cputime;
+    p = subplot(graphHeight, graphWidth, curGraph);
-    startT=t;
+    hold on;
-    n=0;
+    curGraph = curGraph + 1;
-    playing=1;
-    while t<SIMULATION_TIME && playing == 1
-        updateToTime((cputime-startCpu)*speed + startT);
-        drawnow limitrate;
-        n = n + 1;
-    end
-    playing=0;
-    fprintf("Refresh rate : %f Hz\n", n/(cputime-startCpu));
 end
 
-function sliderCallback(hObject, ~)
+function graphSpatiale()
-    updateToTime(get(hObject, 'Value'));
+    p = newGraph();
-end
-
-function playCallback(~, ~)
-    play();
-end
-
-function pauseCallback(~, ~)
-    global playing;
-    playing=0;
-end
-
-function [x, y] = pointArround(xC, yC, xD, yD, o)
-    D = xD + yD * 1i;
-    F = abs(D) .* exp(1i * (angle(D) + o - pi/2));
-    x = xC + real(F);
-    y = yC + imag(F);
-end
-
-function drawRect(p, x, y, o, w, h)
-    [x1, y1] = pointArround(x, y, - w/2, + h/2, o);
-    [x2, y2] = pointArround(x, y, + w/2, + h/2, o);
-    [x3, y3] = pointArround(x, y, + w/2, - h/2, o);
-    [x4, y4] = pointArround(x, y, - w/2, - h/2, o);
-    p.XData = [x1, x2, x3, x4, x1];
-    p.YData = [y1, y2, y3, y4, y1];
-end
-
-function initGraph()
-    cla;
     global SIMULATION_TIME;
     global t speed playing timelines;
     t = 0;
@@ -356,13 +297,133 @@ function initGraph()
     ylabel("Y (mm)");
 end
 
-function d = getTSData(name, i)
+function graphRoues()
-    ts = getTS(name);
+    p = newGraph();
-    if isempty(ts.Data)
+    timeGraph(["lVolt", "rVolt", "dVolt", "oVolt"]);
-        d = 0;
+    global motorSaturationMin motorSaturationMax;
-    else
+    addLimitline(p, -motorSaturationMin);
-        d = ts.Data(i);
+    addLimitline(p, -motorSaturationMax);
+    addLimitline(p, motorSaturationMin);
+    addLimitline(p, motorSaturationMax);
+    addLimitline(p, 0);
+    title("Roues");
+    xlabel("Temps (s)");
+    ylabel("Tension (V)");
+    legend("Tension gauche", "Tension droite", "Dont distance", "Dont direction");
+end
+
+function graphCodeuses()
+    p = newGraph();
+    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");
+end
+
+function graphDistance()
+    p = newGraph();
+    timeGraph(["dDirEcart", "dEcart", "oErr"]);
+    global dDirEcartMin dDirEcartMax;
+    addLimitline(p, 0);
+    addLimitline(p, dDirEcartMin);
+    addLimitline(p, dDirEcartMax);
+    addLimitline(p, -dDirEcartMin);
+    addLimitline(p, -dDirEcartMax);
+    title("Distance");
+    xlabel("Temps (s)");
+    ylabel("Distance (mm)");
+    legend("Err. distance", "Err. retenue", "Err rotation");
+end
+
+function graphRotation()
+    p = newGraph();
+    timeGraph(["oDirEcart", "oConsEcart", "oEcart"]);
+    global oDirEcartMin;
+    addLimitline(p, oDirEcartMin);
+    addLimitline(p, -oDirEcartMin);
+    title("Rotation");
+    xlabel("Temps (s)");
+    ylabel("Angle (rad)");
+    legend("Err. direction", "Err. consigne", "Err. retenue");
+end
+
+function graphSecurite()
+    p = newGraph();
+    timeGraph(["dDirEcart", "secFrontL", "secFrontR", "secBackL", "secBackR", "dErr"]);
+    global margeSecurite;
+    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");
+end
+
+function graphVitesseDist()
+    p = newGraph();
+    timeGraph(["xVit", "yVit"]);
+    addLimitline(p, 0);
+    title("Vitesse");
+    xlabel("Temps (s)");
+    ylabel("Vitesse (mm/s)");
+    legend("X", "Y");
+end
+
+function graphEtat()
+    p = newGraph();
+    timeGraph(["etat"]);
+    addLimitline(p, 0);
+    title("Etat");
+    xlabel("Temps (s)");
+    %ylabel("Vitesse (mm/s)");
+    %legend("X", "Y");
+end
+
+function graphVitesseAngl()
+    p = newGraph();
+    timeGraph(["oVit"]);
+    addLimitline(p, 0);
+    title("Vitesse");
+    xlabel("Temps (s)");
+    ylabel("Vitesse (rad/s)");
+    legend("O");
+end
+
+% Playback
+
+function play()
+    global SIMULATION_TIME speed t playing;
+    if playing == 1
+        return
     end
+    startCpu=cputime;
+    startT=t;
+    n=0;
+    playing=1;
+    while t<SIMULATION_TIME && playing == 1
+        updateToTime((cputime-startCpu)*speed + startT);
+        drawnow limitrate;
+        n = n + 1;
+    end
+    playing=0;
+    fprintf("Refresh rate : %f Hz\n", n/(cputime-startCpu));
+end
+
+function sliderCallback(hObject, ~)
+    updateToTime(get(hObject, 'Value'));
+end
+
+function playCallback(~, ~)
+    play();
+end
+
+function pauseCallback(~, ~)
+    global playing;
+    playing=0;
 end
 
 function updateToTime(newT)