1
0
Fork 0
mirror of https://github.com/RobotechLille/cdf2018-principal synced 2024-11-21 15:46:06 +01:00

Meilleur asservissement

This commit is contained in:
Geoffrey Frogeye 2018-05-16 07:58:23 +02:00
parent aa519e33bf
commit d0d3e7f244
13 changed files with 462 additions and 206 deletions

View file

@ -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

View file

@ -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;
}
}

View file

@ -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

View file

@ -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); */
} }

View file

@ -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

View file

@ -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;
} }

View file

@ -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);

View file

@ -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);

View file

@ -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;
} }

View file

@ -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);

View file

@ -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

View file

@ -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();

View file

@ -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)