geoffrey/cdf2018-principal
geoffrey
/
cdf2018-principal
mirror of https://github.com/RobotechLille/cdf2018-principal
1
0
Fork 0
Code Issues Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
chef/Makefile
View File

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

22
chef/src/common.c
View File

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

20
chef/src/common.h
View File

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

18
chef/src/diagnostics.c
View File

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

29
chef/src/dimensions.h
View File

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

7
chef/src/ihm.c
View File

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

124
chef/src/movement.c
View File

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

13
chef/src/movement.h
View File

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

27
chef/src/parcours.c
View File

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

43
chef/src/position.c
View File

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

8
chef/src/position.h
View File

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

18
chef/src/testRetour.c
View File

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

337
simu/simu.m
View File

@ -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<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));
% Graphiques
function p = newGraph()
global curGraph graphWidth graphHeight p;
fprintf("Graphe %d/%d\n", curGraph, graphWidth * graphHeight);
p = subplot(graphHeight, graphWidth, curGraph);
hold on;
curGraph = curGraph + 1;
end
function sliderCallback(hObject, ~)
updateToTime(get(hObject, 'Value'));
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;
function graphSpatiale()
p = newGraph();
global SIMULATION_TIME;
global t speed playing timelines;
t = 0;
@ -356,13 +297,133 @@ function initGraph()
ylabel("Y (mm)");
end
function d = getTSData(name, i)
ts = getTS(name);
if isempty(ts.Data)
d = 0;
else
d = ts.Data(i);
function graphRoues()
p = newGraph();
timeGraph(["lVolt", "rVolt", "dVolt", "oVolt"]);
global motorSaturationMin motorSaturationMax;
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");
end
function graphCodeuses()
p = newGraph();
timeGraph(["lCodTot", "rCodTot", "newL", "newR"]);
addLimitline(p, 2^16-1);
addLimitline(p, 0);
title("Codeuses");
xlabel("Temps (s)");
ylabel("Crans");
legend("Total gauche", "Total droite", "Brut gauche", "Brut droite");
end
function graphDistance()
p = newGraph();
timeGraph(["dDirEcart", "dEcart", "oErr"]);
global dDirEcartMin dDirEcartMax;
addLimitline(p, 0);
addLimitline(p, dDirEcartMin);
addLimitline(p, dDirEcartMax);
addLimitline(p, -dDirEcartMin);
addLimitline(p, -dDirEcartMax);
title("Distance");
xlabel("Temps (s)");
ylabel("Distance (mm)");
legend("Err. distance", "Err. retenue", "Err rotation");
end
function graphRotation()
p = newGraph();
timeGraph(["oDirEcart", "oConsEcart", "oEcart"]);
global oDirEcartMin;
addLimitline(p, oDirEcartMin);
addLimitline(p, -oDirEcartMin);
title("Rotation");
xlabel("Temps (s)");
ylabel("Angle (rad)");
legend("Err. direction", "Err. consigne", "Err. retenue");
end
function graphSecurite()
p = newGraph();
timeGraph(["dDirEcart", "secFrontL", "secFrontR", "secBackL", "secBackR", "dErr"]);
global margeSecurite;
addLimitline(p, 0);
addLimitline(p, margeSecurite);
addLimitline(p, -margeSecurite);
title("Distances de sécurité");
xlabel("Temps (s)");
ylabel("Distance (mm)");
legend("Err. distance", "Avant gauche", "Avant droite", "Arrière gauche", "Arrière droite", "Err. retenue");
end
function graphVitesseDist()
p = newGraph();
timeGraph(["xVit", "yVit"]);
addLimitline(p, 0);
title("Vitesse");
xlabel("Temps (s)");
ylabel("Vitesse (mm/s)");
legend("X", "Y");
end
function graphEtat()
p = newGraph();
timeGraph(["etat"]);
addLimitline(p, 0);
title("Etat");
xlabel("Temps (s)");
%ylabel("Vitesse (mm/s)");
%legend("X", "Y");
end
function graphVitesseAngl()
p = newGraph();
timeGraph(["oVit"]);
addLimitline(p, 0);
title("Vitesse");
xlabel("Temps (s)");
ylabel("Vitesse (rad/s)");
legend("O");
end
% Playback
function play()
global SIMULATION_TIME speed t playing;
if playing == 1
return
end
startCpu=cputime;
startT=t;
n=0;
playing=1;
while t<SIMULATION_TIME && playing == 1
updateToTime((cputime-startCpu)*speed + startT);
drawnow limitrate;
n = n + 1;
end
playing=0;
fprintf("Refresh rate : %f Hz\n", n/(cputime-startCpu));
end
function sliderCallback(hObject, ~)
updateToTime(get(hObject, 'Value'));
end
function playCallback(~, ~)
play();
end
function pauseCallback(~, ~)
global playing;
playing=0;
end
function updateToTime(newT)