mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-21 15:46:06 +01:00
Meilleur asservissement
This commit is contained in:
parent
aa519e33bf
commit
d0d3e7f244
|
@ -11,7 +11,7 @@ CFLAGS_CUSTOM += -g
|
||||||
## Générateurs de drapeaux pour les bibliothèques
|
## Générateurs de drapeaux pour les bibliothèques
|
||||||
PKG_CONFIG=pkg-config
|
PKG_CONFIG=pkg-config
|
||||||
## Nom des objets communs
|
## 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)))
|
OBJS_O=$(addprefix obj/,$(addsuffix .o,$(OBJS)))
|
||||||
|
|
||||||
# VARIABLES AUTOMATIQUES
|
# VARIABLES AUTOMATIQUES
|
||||||
|
|
|
@ -44,3 +44,25 @@ float updatePID(struct PID *pid, float err)
|
||||||
return pid->KP * err + pid->KI * pid->integErr + pid->KP * derivErr;
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -2,7 +2,12 @@
|
||||||
#define __COMMON_H_
|
#define __COMMON_H_
|
||||||
|
|
||||||
#include <time.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 PID {
|
||||||
struct timespec lastCalc;
|
struct timespec lastCalc;
|
||||||
float KP;
|
float KP;
|
||||||
|
@ -12,10 +17,21 @@ struct PID {
|
||||||
float integErr;
|
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 resetPID(struct PID *pid);
|
||||||
void initPID(struct PID *pid, float KP, float KI, float KD);
|
void initPID(struct PID *pid, float KP, float KI, float KD);
|
||||||
float updatePID(struct PID *pid, float err);
|
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
|
#endif
|
||||||
|
|
|
@ -150,13 +150,13 @@ void runDiagnostics()
|
||||||
|
|
||||||
execDiagnostic("Ouverture loquet", diagJustRun, &diagSetLoquetOuvert);
|
execDiagnostic("Ouverture loquet", diagJustRun, &diagSetLoquetOuvert);
|
||||||
execDiagnostic("Fermeture loquet", diagJustRun, &diagSetLoquetFerme);
|
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); */
|
||||||
}
|
}
|
||||||
|
|
|
@ -40,27 +40,30 @@
|
||||||
|
|
||||||
// Constantes asservissement
|
// 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
|
// 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_KI 0.0
|
||||||
#define D_KD 0.0
|
#define D_KD 0.0
|
||||||
#define TARGET_TENSION_RATIO 0.75
|
#define TARGET_TENSION_RATIO 0.75
|
||||||
#define TARGET_TENSION (TARGET_TENSION_RATIO * MOTOR_SATURATION_MAX) // V
|
#define TARGET_TENSION (TARGET_TENSION_RATIO * MOTOR_SATURATION_MAX) // V
|
||||||
#define CAROTTE_DISTANCE (TARGET_TENSION / D_KP) // mm
|
#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
|
#define MARGE_SECURITE 300.0 // mm
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -2,12 +2,13 @@
|
||||||
#include <signal.h>
|
#include <signal.h>
|
||||||
#include <time.h>
|
#include <time.h>
|
||||||
|
|
||||||
#include "position.h"
|
|
||||||
#include "buttons.h"
|
#include "buttons.h"
|
||||||
|
#include "calibrage.h"
|
||||||
#include "diagnostics.h"
|
#include "diagnostics.h"
|
||||||
#include "ihm.h"
|
#include "ihm.h"
|
||||||
#include "lcd.h"
|
#include "lcd.h"
|
||||||
#include "parcours.h"
|
#include "parcours.h"
|
||||||
|
#include "position.h"
|
||||||
|
|
||||||
// Globales
|
// Globales
|
||||||
pthread_t tIHM;
|
pthread_t tIHM;
|
||||||
|
@ -116,7 +117,7 @@ void* TaskIHM(void* pdata)
|
||||||
if (bout == rouge) {
|
if (bout == rouge) {
|
||||||
clearLCD();
|
clearLCD();
|
||||||
printToLCD(LCD_LINE_1, "Calibrage...");
|
printToLCD(LCD_LINE_1, "Calibrage...");
|
||||||
resetPosition();
|
calibrer(isOrange);
|
||||||
clock_gettime(CLOCK_REALTIME, &calibrageLast);
|
clock_gettime(CLOCK_REALTIME, &calibrageLast);
|
||||||
} else if (bout == jaune) {
|
} else if (bout == jaune) {
|
||||||
break;
|
break;
|
||||||
|
@ -191,7 +192,7 @@ void* TaskIHM(void* pdata)
|
||||||
if (bout == rouge) {
|
if (bout == rouge) {
|
||||||
clearLCD();
|
clearLCD();
|
||||||
printToLCD(LCD_LINE_1, "Remise a zero...");
|
printToLCD(LCD_LINE_1, "Remise a zero...");
|
||||||
delay(3000); // TODO
|
resetPosition();
|
||||||
} else if (bout == jaune) {
|
} else if (bout == jaune) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
|
@ -33,8 +33,12 @@ float dVolt;
|
||||||
float oVolt;
|
float oVolt;
|
||||||
float lErr;
|
float lErr;
|
||||||
float rErr;
|
float rErr;
|
||||||
|
enum movStates etat;
|
||||||
unsigned int nbCalcCons;
|
unsigned int nbCalcCons;
|
||||||
|
|
||||||
|
bool secuAv = true;
|
||||||
|
bool secuAr = true;
|
||||||
|
|
||||||
void configureMovement()
|
void configureMovement()
|
||||||
{
|
{
|
||||||
stop();
|
stop();
|
||||||
|
@ -66,6 +70,7 @@ void configureMovement()
|
||||||
registerDebugVar("oConsEcart", f, &oConsEcart);
|
registerDebugVar("oConsEcart", f, &oConsEcart);
|
||||||
registerDebugVar("lErr", f, &lErr);
|
registerDebugVar("lErr", f, &lErr);
|
||||||
registerDebugVar("rErr", f, &rErr);
|
registerDebugVar("rErr", f, &rErr);
|
||||||
|
registerDebugVar("etat", d, &etat);
|
||||||
registerDebugVar("nbCalcCons", d, &nbCalcCons);
|
registerDebugVar("nbCalcCons", d, &nbCalcCons);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -96,10 +101,9 @@ void* TaskMovement(void* pData)
|
||||||
initPID(&dPid, D_KP, D_KI, D_KD);
|
initPID(&dPid, D_KP, D_KI, D_KD);
|
||||||
initPID(&oPid, O_KP, O_KI, O_KD);
|
initPID(&oPid, O_KP, O_KI, O_KD);
|
||||||
|
|
||||||
bool orienteDestination = false;
|
|
||||||
bool procheDestination = false;
|
|
||||||
bool orienteConsigne = false;
|
|
||||||
bool reverse;
|
bool reverse;
|
||||||
|
bool obstacle;
|
||||||
|
etat = quelconque;
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
|
|
||||||
|
@ -109,35 +113,102 @@ void* TaskMovement(void* pData)
|
||||||
yDiff = cons.y - connu.y;
|
yDiff = cons.y - connu.y;
|
||||||
dDirEcart = hypotf(xDiff, yDiff);
|
dDirEcart = hypotf(xDiff, yDiff);
|
||||||
oDirEcart = angleMod(atan2(yDiff, xDiff) - connu.o);
|
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)) {
|
if ((reverse = fabsf(oDirEcart) > M_PI_2)) {
|
||||||
dDirEcart = -dDirEcart;
|
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
|
// Carotte
|
||||||
dEcart = (orienteDestination && !procheDestination) ? dDirEcart : 0;
|
|
||||||
dErr = fcap(dEcart, CAROTTE_DISTANCE);
|
dErr = fcap(dEcart, CAROTTE_DISTANCE);
|
||||||
oEcart = procheDestination ? oConsEcart : oDirEcart;
|
|
||||||
oErr = fcap(oEcart * DISTANCE_BETWEEN_WHEELS, CAROTTE_ANGLE);
|
oErr = fcap(oEcart * DISTANCE_BETWEEN_WHEELS, CAROTTE_ANGLE);
|
||||||
|
|
||||||
dVolt = updatePID(&dPid, dErr);
|
dVolt = updatePID(&dPid, dErr);
|
||||||
|
@ -148,11 +219,12 @@ void* TaskMovement(void* pData)
|
||||||
|
|
||||||
pthread_mutex_lock(&movInstructionMutex);
|
pthread_mutex_lock(&movInstructionMutex);
|
||||||
if (movInstructionBool) {
|
if (movInstructionBool) {
|
||||||
if (procheDestination && orienteConsigne) {
|
if (obstacle || etat == fini) {
|
||||||
brake();
|
brake();
|
||||||
} else {
|
} else {
|
||||||
setMoteurTension(lVolt, rVolt);
|
setMoteurTension(lVolt, rVolt);
|
||||||
}
|
}
|
||||||
|
pthread_cond_signal(&movInstructionCond);
|
||||||
}
|
}
|
||||||
pthread_mutex_unlock(&movInstructionMutex);
|
pthread_mutex_unlock(&movInstructionMutex);
|
||||||
|
|
||||||
|
@ -173,22 +245,32 @@ void disableAsservissement()
|
||||||
{
|
{
|
||||||
pthread_mutex_lock(&movInstructionMutex);
|
pthread_mutex_lock(&movInstructionMutex);
|
||||||
movInstructionBool = false;
|
movInstructionBool = false;
|
||||||
|
etat = quelconque;
|
||||||
pthread_mutex_unlock(&movInstructionMutex);
|
pthread_mutex_unlock(&movInstructionMutex);
|
||||||
}
|
}
|
||||||
|
|
||||||
void setDestination(struct position* pos)
|
void setDestination(struct position* pos)
|
||||||
{
|
{
|
||||||
pthread_mutex_lock(&movInstructionMutex);
|
pthread_mutex_lock(&movInstructionMutex);
|
||||||
|
etat = quelconque;
|
||||||
memcpy(&cons, pos, sizeof(struct position));
|
memcpy(&cons, pos, sizeof(struct position));
|
||||||
movInstructionBool = true;
|
movInstructionBool = true;
|
||||||
pthread_cond_signal(&movInstructionCond);
|
pthread_cond_signal(&movInstructionCond);
|
||||||
pthread_mutex_unlock(&movInstructionMutex);
|
pthread_mutex_unlock(&movInstructionMutex);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void setSecurite(bool av, bool ar)
|
||||||
|
{
|
||||||
|
pthread_mutex_lock(&movInstructionMutex);
|
||||||
|
secuAv = av;
|
||||||
|
secuAr = ar;
|
||||||
|
pthread_mutex_unlock(&movInstructionMutex);
|
||||||
|
}
|
||||||
|
|
||||||
void waitDestination()
|
void waitDestination()
|
||||||
{
|
{
|
||||||
pthread_mutex_lock(&movInstructionMutex);
|
pthread_mutex_lock(&movInstructionMutex);
|
||||||
while (movInstructionBool) {
|
while (etat != fini) {
|
||||||
pthread_cond_wait(&movInstructionCond, &movInstructionMutex);
|
pthread_cond_wait(&movInstructionCond, &movInstructionMutex);
|
||||||
}
|
}
|
||||||
pthread_mutex_unlock(&movInstructionMutex);
|
pthread_mutex_unlock(&movInstructionMutex);
|
||||||
|
|
|
@ -9,8 +9,20 @@
|
||||||
|
|
||||||
// #define ENABLE_SECURITE
|
// #define ENABLE_SECURITE
|
||||||
|
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
#include "position.h"
|
#include "position.h"
|
||||||
|
|
||||||
|
enum movStates {
|
||||||
|
quelconque, // 0
|
||||||
|
direction, // 1
|
||||||
|
approche, // 2
|
||||||
|
arret, // 3
|
||||||
|
orientation, // 4
|
||||||
|
oriente, // 5
|
||||||
|
fini // 6
|
||||||
|
};
|
||||||
|
|
||||||
// Public
|
// Public
|
||||||
void configureMovement();
|
void configureMovement();
|
||||||
void deconfigureMovement();
|
void deconfigureMovement();
|
||||||
|
@ -18,6 +30,7 @@ void setDestination(struct position* pos);
|
||||||
void waitDestination();
|
void waitDestination();
|
||||||
void enableAsservissement();
|
void enableAsservissement();
|
||||||
void disableAsservissement();
|
void disableAsservissement();
|
||||||
|
void setSecurite(bool av, bool ar);
|
||||||
|
|
||||||
// Private
|
// Private
|
||||||
void* TaskMovement(void* pData);
|
void* TaskMovement(void* pData);
|
||||||
|
|
|
@ -73,6 +73,7 @@ int updateParcours()
|
||||||
void stopParcours()
|
void stopParcours()
|
||||||
{
|
{
|
||||||
pthread_cancel(tParcours);
|
pthread_cancel(tParcours);
|
||||||
|
disableAsservissement();
|
||||||
stop();
|
stop();
|
||||||
|
|
||||||
resetLCD();
|
resetLCD();
|
||||||
|
@ -90,12 +91,15 @@ void gotoPoint(float x, float y, float o)
|
||||||
/* o = M_PI - o; */
|
/* o = M_PI - o; */
|
||||||
/* } */
|
/* } */
|
||||||
/* } */
|
/* } */
|
||||||
if (!isOrange) {
|
if (isOrange) {
|
||||||
o = -o;
|
o = -o;
|
||||||
|
y = -y;
|
||||||
}
|
}
|
||||||
struct position pos = { x, y, o };
|
struct position pos = { x, y, o };
|
||||||
setDestination(&pos);
|
setDestination(&pos);
|
||||||
|
printf("New dest : %f %f %f\n", pos.x, pos.y, pos.o);
|
||||||
waitDestination();
|
waitDestination();
|
||||||
|
printf("Done.\n");
|
||||||
brake();
|
brake();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -112,14 +116,23 @@ void* TaskParcours(void* pdata)
|
||||||
{
|
{
|
||||||
(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(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;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -21,6 +21,10 @@ pthread_mutex_t posConnu;
|
||||||
pthread_cond_t newPos;
|
pthread_cond_t newPos;
|
||||||
pthread_t tPosition;
|
pthread_t tPosition;
|
||||||
|
|
||||||
|
struct movAvg xVit;
|
||||||
|
struct movAvg yVit;
|
||||||
|
struct movAvg oVit;
|
||||||
|
|
||||||
// Globales
|
// Globales
|
||||||
unsigned int nbCalcPos;
|
unsigned int nbCalcPos;
|
||||||
long lCodTot, rCodTot;
|
long lCodTot, rCodTot;
|
||||||
|
@ -28,30 +32,37 @@ long lCodTot, rCodTot;
|
||||||
uint16_t oldL, oldR;
|
uint16_t oldL, oldR;
|
||||||
uint16_t newL, newR;
|
uint16_t newL, newR;
|
||||||
int16_t deltaL, deltaR;
|
int16_t deltaL, deltaR;
|
||||||
|
float deltaT;
|
||||||
int newLdbg, newRdbg;
|
int newLdbg, newRdbg;
|
||||||
|
|
||||||
struct timespec lastCoderRead;
|
struct timespec lastCoderRead;
|
||||||
|
|
||||||
void updateDelta()
|
void updateDelta()
|
||||||
{
|
{
|
||||||
|
// Récupération des valeurs
|
||||||
newL = (readI2C(fdFPGA(), CODER_LEFT_H) << 8 | readI2C(fdFPGA(), CODER_LEFT_L)) & 0xFFFF;
|
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;
|
newR = (readI2C(fdFPGA(), CODER_RIGHT_H) << 8 | readI2C(fdFPGA(), CODER_RIGHT_L)) & 0xFFFF;
|
||||||
newLdbg = newL;
|
newLdbg = newL;
|
||||||
newRdbg = newR;
|
newRdbg = newR;
|
||||||
|
|
||||||
|
// Calcul des deltaL et deltaR
|
||||||
deltaL = (abs(oldL - newL) < UINT16_MAX / 2) ? newL - oldL : UINT16_MAX - oldL + newL;
|
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;
|
deltaR = (abs(oldR - newR) < UINT16_MAX / 2) ? newR - oldR : UINT16_MAX - oldR + newR;
|
||||||
|
|
||||||
// Verification de valeur abbérante
|
// Calcul de deltaT
|
||||||
struct timespec now;
|
struct timespec now;
|
||||||
clock_gettime(CLOCK_REALTIME, &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) {
|
if (abs(deltaL) > maxDelta) {
|
||||||
deltaL = 0;
|
deltaL = 0;
|
||||||
}
|
}
|
||||||
if (abs(deltaR) > maxDelta) {
|
if (abs(deltaR) > maxDelta) {
|
||||||
deltaR = 0;
|
deltaR = 0;
|
||||||
}
|
}
|
||||||
lastCoderRead = now;
|
|
||||||
|
|
||||||
oldL = newL;
|
oldL = newL;
|
||||||
oldR = newR;
|
oldR = newR;
|
||||||
|
@ -82,18 +93,28 @@ void* TaskPosition(void* pData)
|
||||||
lCodTot += deltaL;
|
lCodTot += deltaL;
|
||||||
rCodTot += deltaR;
|
rCodTot += deltaR;
|
||||||
|
|
||||||
|
// Calcul delta en mm
|
||||||
float dR = deltaR * AV_PER_CYCLE;
|
float dR = deltaR * AV_PER_CYCLE;
|
||||||
float dL = deltaL * 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 deltaO = atan2(dR - dL, DISTANCE_BETWEEN_WHEELS);
|
||||||
float deltaD = (dL + dR) / 2;
|
float deltaD = (dL + dR) / 2;
|
||||||
|
|
||||||
pthread_mutex_lock(&posConnu);
|
pthread_mutex_lock(&posConnu);
|
||||||
|
|
||||||
|
// Modification des valeurs de position actuelle
|
||||||
connu.o += deltaO;
|
connu.o += deltaO;
|
||||||
float deltaX = deltaD * cos(connu.o);
|
float deltaX = deltaD * cos(connu.o);
|
||||||
float deltaY = deltaD * sin(connu.o);
|
float deltaY = deltaD * sin(connu.o);
|
||||||
connu.x += deltaX;
|
connu.x += deltaX;
|
||||||
connu.y += deltaY;
|
connu.y += deltaY;
|
||||||
|
|
||||||
|
// Modification des valeurs de vitesse
|
||||||
|
addMovAvg(&xVit, deltaX / deltaT);
|
||||||
|
addMovAvg(&yVit, deltaY / deltaT);
|
||||||
|
addMovAvg(&oVit, deltaO / deltaT);
|
||||||
|
|
||||||
nbCalcPos++;
|
nbCalcPos++;
|
||||||
pthread_cond_signal(&newPos);
|
pthread_cond_signal(&newPos);
|
||||||
pthread_mutex_unlock(&posConnu);
|
pthread_mutex_unlock(&posConnu);
|
||||||
|
@ -106,6 +127,9 @@ void* TaskPosition(void* pData)
|
||||||
|
|
||||||
void configurePosition()
|
void configurePosition()
|
||||||
{
|
{
|
||||||
|
initMovAvg(&xVit, VIT_MOVAVG_SIZE);
|
||||||
|
initMovAvg(&yVit, VIT_MOVAVG_SIZE);
|
||||||
|
initMovAvg(&oVit, VIT_MOVAVG_SIZE);
|
||||||
resetPosition();
|
resetPosition();
|
||||||
registerDebugVar("lCodTot", ld, &lCodTot);
|
registerDebugVar("lCodTot", ld, &lCodTot);
|
||||||
registerDebugVar("rCodTot", ld, &rCodTot);
|
registerDebugVar("rCodTot", ld, &rCodTot);
|
||||||
|
@ -114,6 +138,9 @@ void configurePosition()
|
||||||
registerDebugVar("xConnu", f, &connu.x);
|
registerDebugVar("xConnu", f, &connu.x);
|
||||||
registerDebugVar("yConnu", f, &connu.y);
|
registerDebugVar("yConnu", f, &connu.y);
|
||||||
registerDebugVar("oConnu", f, &connu.o);
|
registerDebugVar("oConnu", f, &connu.o);
|
||||||
|
registerDebugVar("xVit", f, &xVit.current);
|
||||||
|
registerDebugVar("yVit", f, &yVit.current);
|
||||||
|
registerDebugVar("oVit", f, &oVit.current);
|
||||||
registerDebugVar("nbCalcPos", d, &nbCalcPos);
|
registerDebugVar("nbCalcPos", d, &nbCalcPos);
|
||||||
pthread_mutex_init(&posConnu, NULL);
|
pthread_mutex_init(&posConnu, NULL);
|
||||||
pthread_cond_init(&newPos, NULL);
|
pthread_cond_init(&newPos, NULL);
|
||||||
|
@ -140,6 +167,16 @@ unsigned int getPosition(struct position* pos)
|
||||||
return nb;
|
return nb;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
float getAbsVitesse()
|
||||||
|
{
|
||||||
|
return hypotf(xVit.current, yVit.current);
|
||||||
|
}
|
||||||
|
|
||||||
|
float getAnglVitesse()
|
||||||
|
{
|
||||||
|
return oVit.current;
|
||||||
|
}
|
||||||
|
|
||||||
unsigned int getPositionNewer(struct position* pos, unsigned int lastCalc)
|
unsigned int getPositionNewer(struct position* pos, unsigned int lastCalc)
|
||||||
{
|
{
|
||||||
pthread_mutex_lock(&posConnu);
|
pthread_mutex_lock(&posConnu);
|
||||||
|
|
|
@ -10,8 +10,11 @@
|
||||||
|
|
||||||
#define POSITION_INTERVAL 10
|
#define POSITION_INTERVAL 10
|
||||||
|
|
||||||
|
#define VIT_MOVAVG_TIME 100
|
||||||
|
#define VIT_MOVAVG_SIZE (VIT_MOVAVG_TIME / POSITION_INTERVAL)
|
||||||
|
|
||||||
// Structures
|
// Structures
|
||||||
struct __attribute__ ((packed)) position {
|
struct __attribute__((packed)) position {
|
||||||
float x;
|
float x;
|
||||||
float y;
|
float y;
|
||||||
float o;
|
float o;
|
||||||
|
@ -25,6 +28,7 @@ unsigned int getPositionNewer(struct position* pos, unsigned int lastCalc);
|
||||||
unsigned int getPosition(struct position* pos);
|
unsigned int getPosition(struct position* pos);
|
||||||
void setPosition(struct position* pos);
|
void setPosition(struct position* pos);
|
||||||
void resetPosition();
|
void resetPosition();
|
||||||
|
float getAnglVitesse();
|
||||||
|
float getAbsVitesse();
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -39,8 +39,11 @@ int main()
|
||||||
disableAsservissement();
|
disableAsservissement();
|
||||||
freewheel();
|
freewheel();
|
||||||
|
|
||||||
|
enum boutons but = pressedButton(BUT_BLOCK);
|
||||||
|
clearLCD();
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
switch (pressedButton(BUT_BLOCK)) {
|
switch (but) {
|
||||||
case jaune:
|
case jaune:
|
||||||
isFree = !isFree;
|
isFree = !isFree;
|
||||||
if (isFree) {
|
if (isFree) {
|
||||||
|
@ -56,12 +59,13 @@ int main()
|
||||||
default:
|
default:
|
||||||
break;
|
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();
|
deconfigureMovement();
|
||||||
|
|
337
simu/simu.m
337
simu/simu.m
|
@ -3,7 +3,7 @@ SIMULATION = 0;
|
||||||
|
|
||||||
% Paramètres de lecture
|
% Paramètres de lecture
|
||||||
DIRNAME = "/home/geoffrey/CdF/cdf2018-principal/log/";
|
DIRNAME = "/home/geoffrey/CdF/cdf2018-principal/log/";
|
||||||
FILENAME = "last.csv";
|
FILENAME = "001418.csv";
|
||||||
PATH = DIRNAME + FILENAME;
|
PATH = DIRNAME + FILENAME;
|
||||||
|
|
||||||
% Paramètres de simulation
|
% Paramètres de simulation
|
||||||
|
@ -51,33 +51,37 @@ absoluteMaxVitesseRobotRevpS = (absoluteMaxVitesseRobotMMpS / wheelPerimeter); %
|
||||||
absoluteMaxVitesseRobotCycpS = (absoluteMaxVitesseRobotRevpS * coderFullResolution); % cycle/s
|
absoluteMaxVitesseRobotCycpS = (absoluteMaxVitesseRobotRevpS * coderFullResolution); % cycle/s
|
||||||
|
|
||||||
% Constantes asservissement
|
% 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
|
% 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;
|
dKI = 0.0;
|
||||||
dKD = 0.0;
|
dKD = 0.0;
|
||||||
targetTensionRatio = 0.75;
|
targetTensionRatio = 0.75;
|
||||||
targetTension = (targetTensionRatio * motorSaturationMax); % V
|
targetTension = (targetTensionRatio * motorSaturationMax); % V
|
||||||
carotteDistance = (targetTension / dKP); % mm
|
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
|
margeSecurite = 300.0; % mm
|
||||||
|
|
||||||
%END DIMENSIONS
|
%END DIMENSIONS
|
||||||
|
|
||||||
|
|
||||||
global s;
|
global s;
|
||||||
if SIMULATION == 1
|
if SIMULATION == 1
|
||||||
% Génération de la consigne
|
% Génération de la consigne
|
||||||
|
@ -128,80 +132,27 @@ end
|
||||||
|
|
||||||
% Graphes
|
% 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);
|
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)
|
function ts = getTS(name)
|
||||||
global SIMULATION s;
|
global SIMULATION s;
|
||||||
|
@ -228,6 +179,33 @@ function pt = getTimePoints()
|
||||||
end
|
end
|
||||||
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)
|
function timeGraph(series)
|
||||||
global SIMULATION_TIME p;
|
global SIMULATION_TIME p;
|
||||||
m = inf;
|
m = inf;
|
||||||
|
@ -259,55 +237,18 @@ function addTimeline(p)
|
||||||
timelines = [timelines timeline];
|
timelines = [timelines timeline];
|
||||||
end
|
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
|
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 SIMULATION_TIME;
|
||||||
global t speed playing timelines;
|
global t speed playing timelines;
|
||||||
t = 0;
|
t = 0;
|
||||||
|
@ -356,13 +297,133 @@ function initGraph()
|
||||||
ylabel("Y (mm)");
|
ylabel("Y (mm)");
|
||||||
end
|
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
|
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
|
end
|
||||||
|
|
||||||
function updateToTime(newT)
|
function updateToTime(newT)
|
||||||
|
|
Loading…
Reference in a new issue