From d0d3e7f244bdbb05d24b95e1a6b9593d60653dbd Mon Sep 17 00:00:00 2001 From: Geoffrey Frogeye Date: Wed, 16 May 2018 07:58:23 +0200 Subject: [PATCH] Meilleur asservissement --- chef/Makefile | 2 +- chef/src/common.c | 22 +++ chef/src/common.h | 20 ++- chef/src/diagnostics.c | 18 +-- chef/src/dimensions.h | 29 ++-- chef/src/ihm.c | 7 +- chef/src/movement.c | 124 ++++++++++++--- chef/src/movement.h | 13 ++ chef/src/parcours.c | 27 +++- chef/src/position.c | 43 +++++- chef/src/position.h | 8 +- chef/src/testRetour.c | 18 ++- simu/simu.m | 337 ++++++++++++++++++++++++----------------- 13 files changed, 462 insertions(+), 206 deletions(-) diff --git a/chef/Makefile b/chef/Makefile index 4a230bd..9d32eea 100644 --- a/chef/Makefile +++ b/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 diff --git a/chef/src/common.c b/chef/src/common.c index 083142e..1f909bc 100644 --- a/chef/src/common.c +++ b/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; + } +} diff --git a/chef/src/common.h b/chef/src/common.h index 5986d0f..59124ab 100644 --- a/chef/src/common.h +++ b/chef/src/common.h @@ -2,7 +2,12 @@ #define __COMMON_H_ #include +#include +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 diff --git a/chef/src/diagnostics.c b/chef/src/diagnostics.c index c9ff1b4..91004f3 100644 --- a/chef/src/diagnostics.c +++ b/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("T+1 barillet", diagJustRun, &barilletSuivant); - execDiagnostic("T+2 barillet", diagJustRun, &barilletSkip); - execDiagnostic("Pousser balle", diagJustRun, &pousserBalle); - execDiagnostic("Pos. ejection", diagJustRun, &diagSetPositionBalleEjection); - execDiagnostic("Pos. evacuation", diagJustRun, &diagSetPositionBalleEvacuation); - execDiagnostic("Pos. attente", diagJustRun, &diagSetPositionBalleAttente); - execDiagnostic("Propulsion on", diagJustRun, &diagSetPropulsionOff); - execDiagnostic("Propulsion off", diagJustRun, &diagSetPropulsionOn); + /* execDiagnostic("Reset barillet", diagJustRun, &barilletReset); */ + /* execDiagnostic("T+1 barillet", diagJustRun, &barilletSuivant); */ + /* execDiagnostic("T+2 barillet", diagJustRun, &barilletSkip); */ + /* execDiagnostic("Pousser balle", diagJustRun, &pousserBalle); */ + /* execDiagnostic("Pos. ejection", diagJustRun, &diagSetPositionBalleEjection); */ + /* execDiagnostic("Pos. evacuation", diagJustRun, &diagSetPositionBalleEvacuation); */ + /* execDiagnostic("Pos. attente", diagJustRun, &diagSetPositionBalleAttente); */ + /* execDiagnostic("Propulsion on", diagJustRun, &diagSetPropulsionOff); */ + /* execDiagnostic("Propulsion off", diagJustRun, &diagSetPropulsionOn); */ } diff --git a/chef/src/dimensions.h b/chef/src/dimensions.h index c5321eb..d9dee9a 100644 --- a/chef/src/dimensions.h +++ b/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_DIR_ECART_MAX 50.0 // mm -#define D_KP 0.05 +#define D_VIT_MIN 10.0 // mm/s +#define D_TENSION_MIN 1 // V +#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 diff --git a/chef/src/ihm.c b/chef/src/ihm.c index 583ea40..9974c1e 100644 --- a/chef/src/ihm.c +++ b/chef/src/ihm.c @@ -2,12 +2,13 @@ #include #include -#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; } diff --git a/chef/src/movement.c b/chef/src/movement.c index 86b01f7..441b55c 100644 --- a/chef/src/movement.c +++ b/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) { - orienteDestination = true; - } else if (fabsf(oDirEcart) > O_DIR_ECART_MAX) { - orienteDestination = false; + // Selection de l'état suivant + switch (etat) { + case quelconque: + 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) { - procheDestination = true; - } else if (fabsf(dDirEcart) > D_DIR_ECART_MAX) { - procheDestination = false; + // Application des directives d'état + switch (etat) { + case quelconque: + 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) { - orienteConsigne = true; - } else if (fabsf(oConsEcart) > O_ECART_MAX) { - orienteConsigne = false; +#ifdef ENABLE_SECURITE + float av, ar; + getDistance(&av, &ar); + 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); diff --git a/chef/src/movement.h b/chef/src/movement.h index 85226f9..b35756a 100644 --- a/chef/src/movement.h +++ b/chef/src/movement.h @@ -9,8 +9,20 @@ // #define ENABLE_SECURITE +#include + #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); diff --git a/chef/src/parcours.c b/chef/src/parcours.c index e3b6111..21c4168 100644 --- a/chef/src/parcours.c +++ b/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); */ - gotoPoint(500, 0, 0); - waitDestination(); - for (int i = 0; i < 5; i++) { - setLoquet(false); + float x = 306 + (isOrange ? 170 : 0); + + setSecurite(true, false); + gotoPoint(x, 200, -M_PI_2); + 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; } diff --git a/chef/src/position.c b/chef/src/position.c index ddc6840..3b58f74 100644 --- a/chef/src/position.c +++ b/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); diff --git a/chef/src/position.h b/chef/src/position.h index 6f91e79..5fba769 100644 --- a/chef/src/position.h +++ b/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 - diff --git a/chef/src/testRetour.c b/chef/src/testRetour.c index e5b6a5c..32c7491 100644 --- a/chef/src/testRetour.c +++ b/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) { - printToLCD(LCD_LINE_1, "Freewheel"); - } else { - printToLCD(LCD_LINE_1, "Asservi"); - } + + struct position pos; + getPosition(&pos); + + 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(); diff --git a/simu/simu.m b/simu/simu.m index b9017db..f48c602 100644 --- a/simu/simu.m +++ b/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 -dDirEcartMax = 50.0; % mm -dKP = 0.05; +dVitMin = 10.0; % mm/s +dTensionMin = 1; % V +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 p; +global curGraph graphWidth graphHeight; +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 -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"); +% FONCTIONS - -% 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 +% Données 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() - global SIMULATION_TIME speed t playing; - if playing == 1 - return - end - startCpu=cputime; - startT=t; - n=0; - playing=1; - while t