mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-24 09:06:03 +01:00
WIP Contrôle
This commit is contained in:
parent
8813455901
commit
1337bd62a9
|
@ -33,14 +33,13 @@
|
||||||
#define AV_PER_CYCLE (WHEEL_PERIMETER / CODER_FULL_RESOLUTION) // mm
|
#define AV_PER_CYCLE (WHEEL_PERIMETER / CODER_FULL_RESOLUTION) // mm
|
||||||
|
|
||||||
// Constantes asservissement
|
// Constantes asservissement
|
||||||
#define D_DIR_ECART_MIN 1.0 // mm
|
#define D_DIR_ECART_MIN 7.0 // mm
|
||||||
#define D_DIR_ECART_MAX 5.0 // mm
|
#define D_DIR_ECART_MAX 10.0 // mm
|
||||||
#define O_DIR_ECART_MIN (6.0 / 360.0 * 2.0 * M_PI) // rad
|
#define O_DIR_ECART_MIN (5.0 / 360.0 * 2.0 * M_PI) // rad
|
||||||
#define O_DIR_ECART_MAX (45.0 / 360.0 * 2.0 * M_PI) // rad
|
#define O_DIR_ECART_MAX (25.0 / 360.0 * 2.0 * M_PI) // rad
|
||||||
#define D_CONS_THRESOLD 1.0 // mm
|
#define O_ECART_MAX (25.0 / 360.0 * 2.0 * M_PI) // rad
|
||||||
#define O_CONS_THRESOLD (6.0 / 360.0 * 2.0 * M_PI) // rad
|
|
||||||
#define O_GAIN 3.0
|
#define O_GAIN 3.0
|
||||||
#define P 5.0
|
#define P 7.0
|
||||||
#define I 0.0
|
#define I 0.0
|
||||||
#define D 0.0
|
#define D 0.0
|
||||||
|
|
||||||
|
|
|
@ -1,52 +1,16 @@
|
||||||
#include <pthread.h>
|
#include <math.h>
|
||||||
#include <signal.h>
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
#include <time.h> // random seed
|
|
||||||
#include <unistd.h> // sleep
|
|
||||||
|
|
||||||
pthread_mutex_t posConnu;
|
float angleGap(float target, float actual)
|
||||||
pthread_cond_t newPos;
|
|
||||||
pthread_t tPosition;
|
|
||||||
pthread_t tMovement;
|
|
||||||
|
|
||||||
void* TaskPosition(void* pData)
|
|
||||||
{
|
{
|
||||||
|
return remainderf(target - actual + M_PI, 2 * M_PI) - M_PI;
|
||||||
(void)pData;
|
|
||||||
for (;;) {
|
|
||||||
printf("Begin position\n");
|
|
||||||
sleep(1);
|
|
||||||
|
|
||||||
pthread_mutex_lock(&posConnu);
|
|
||||||
printf("Sending position\n");
|
|
||||||
pthread_cond_signal(&newPos);
|
|
||||||
pthread_mutex_unlock(&posConnu);
|
|
||||||
}
|
|
||||||
return NULL;
|
|
||||||
}
|
|
||||||
|
|
||||||
void* TaskMovement(void* pData)
|
|
||||||
{
|
|
||||||
(void)pData;
|
|
||||||
for (;;) {
|
|
||||||
printf("Begin Movement\n");
|
|
||||||
sleep(3);
|
|
||||||
|
|
||||||
pthread_mutex_lock(&posConnu);
|
|
||||||
printf("Waiting movement\n");
|
|
||||||
pthread_cond_wait(&newPos, &posConnu);
|
|
||||||
pthread_mutex_unlock(&posConnu);
|
|
||||||
|
|
||||||
}
|
|
||||||
return NULL;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int main()
|
int main()
|
||||||
{
|
{
|
||||||
pthread_mutex_init(&posConnu, NULL);
|
float cons = 2;
|
||||||
pthread_cond_init(&newPos, NULL);
|
for (float conn = 0; conn < 10.0; conn += 0.1) {
|
||||||
pthread_create(&tPosition, NULL, TaskPosition, NULL);
|
printf("%6f\n", angleGap(cons, conn));
|
||||||
pthread_create(&tMovement, NULL, TaskMovement, NULL);
|
}
|
||||||
sleep(300);
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -161,10 +161,14 @@ void* TaskMotor(void* pData)
|
||||||
|
|
||||||
void rawBrake()
|
void rawBrake()
|
||||||
{
|
{
|
||||||
|
lVolt = 0;
|
||||||
|
rVolt = 0;
|
||||||
sendCF(C2FD_PWM, &msgBrake, sizeof(struct C2FD_PWMs));
|
sendCF(C2FD_PWM, &msgBrake, sizeof(struct C2FD_PWMs));
|
||||||
}
|
}
|
||||||
void rawFreewheel()
|
void rawFreewheel()
|
||||||
{
|
{
|
||||||
|
lVolt = 0;
|
||||||
|
rVolt = 0;
|
||||||
sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
|
sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -189,6 +193,7 @@ int brake()
|
||||||
pthread_mutex_lock(&motCons);
|
pthread_mutex_lock(&motCons);
|
||||||
motState = braking;
|
motState = braking;
|
||||||
pthread_mutex_unlock(&motCons);
|
pthread_mutex_unlock(&motCons);
|
||||||
|
printf("192 Brake\n");
|
||||||
rawBrake();
|
rawBrake();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -11,7 +11,7 @@
|
||||||
// #define INVERSE_R_MOTOR
|
// #define INVERSE_R_MOTOR
|
||||||
|
|
||||||
// V/s
|
// V/s
|
||||||
#define RATE_LIMITER_UP 12
|
#define RATE_LIMITER_UP 6
|
||||||
#define RATE_LIMITER_DOWN 1
|
#define RATE_LIMITER_DOWN 1
|
||||||
|
|
||||||
#define TESTINATOR
|
#define TESTINATOR
|
||||||
|
|
|
@ -95,15 +95,16 @@ void waitDestination()
|
||||||
|
|
||||||
float angleGap(float target, float actual)
|
float angleGap(float target, float actual)
|
||||||
{
|
{
|
||||||
return fmod(target - actual + M_PI, 2 * M_PI) - M_PI;
|
float ret = fmodf(target - actual + M_PI, 2 * M_PI) - M_PI;
|
||||||
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
float angleGap180(float target, float actual, float* dist)
|
float angleGap180(float target, float actual, float* dist)
|
||||||
{
|
{
|
||||||
if (fabs(fmod(target - actual + M_PI, 2 * M_PI) - M_PI) > M_PI_2) {
|
if (fabs(fmodf(target - actual + M_PI, 2 * M_PI) - M_PI) > M_PI_2) {
|
||||||
*dist = -*dist;
|
*dist = -*dist;
|
||||||
}
|
}
|
||||||
return fmod(target - actual + M_PI_2, M_PI) - M_PI_2;
|
return fmodf(target - actual + M_PI_2, M_PI) - M_PI_2;
|
||||||
}
|
}
|
||||||
|
|
||||||
void* TaskMovement(void* pData)
|
void* TaskMovement(void* pData)
|
||||||
|
@ -128,7 +129,6 @@ void* TaskMovement(void* pData)
|
||||||
while (!movEnableBool) {
|
while (!movEnableBool) {
|
||||||
pthread_cond_wait(&movEnableCond, &movEnableMutex);
|
pthread_cond_wait(&movEnableCond, &movEnableMutex);
|
||||||
}
|
}
|
||||||
pthread_mutex_unlock(&movEnableMutex);
|
|
||||||
|
|
||||||
// Wait for new calculation if not calculated yet
|
// Wait for new calculation if not calculated yet
|
||||||
lastPosCalc = getPositionNewer(&connu, lastPosCalc);
|
lastPosCalc = getPositionNewer(&connu, lastPosCalc);
|
||||||
|
@ -145,19 +145,21 @@ void* TaskMovement(void* pData)
|
||||||
// Écart entre l'angle actuel et celui orienté vers la position de consigne
|
// Écart entre l'angle actuel et celui orienté vers la position de consigne
|
||||||
// Si l'angle se trouve à gauche du cercle trigo, on le remet à droite
|
// Si l'angle se trouve à gauche du cercle trigo, on le remet à droite
|
||||||
// et on inverse la direction
|
// et on inverse la direction
|
||||||
oDirEcart = angleGap180(atan2(yDiff, xDiff), connu.o, &dDirEcart);
|
/* oDirEcart = angleGap180(atan2(yDiff, xDiff), connu.o, &dDirEcart); */
|
||||||
|
oDirEcart = angleGap(atan2(yDiff, xDiff), connu.o);
|
||||||
pthread_mutex_unlock(&movCons);
|
pthread_mutex_unlock(&movCons);
|
||||||
|
|
||||||
// Si on est loin de la consigne, l'angle cible devient celui orienté vers la consigne
|
// Si on est loin de la consigne, l'angle cible devient celui orienté vers la consigne
|
||||||
if (dDirEcart > D_DIR_ECART_MAX) {
|
float dDirEcartAbs = fabsf(dDirEcart);
|
||||||
|
if (dDirEcartAbs > D_DIR_ECART_MAX) {
|
||||||
oRetenu = true;
|
oRetenu = true;
|
||||||
// Si on est proche de la consigne, l'angle cible devient celui voulu par la consigne
|
// Si on est proche de la consigne, l'angle cible devient celui voulu par la consigne
|
||||||
} else if (dDirEcart < D_DIR_ECART_MIN) {
|
} else if (dDirEcartAbs < D_DIR_ECART_MIN) {
|
||||||
oRetenu = false;
|
oRetenu = false;
|
||||||
}
|
}
|
||||||
oErr = oRetenu ? oDirEcart : oEcart;
|
oErr = oRetenu ? oDirEcart : oEcart;
|
||||||
|
|
||||||
float oDirEcartAbs = fabs(oDirEcart);
|
float oDirEcartAbs = fabsf(oDirEcart);
|
||||||
// Si l'écart avec l'angle orienté vers la consigne est grand, la distance cible devient 0
|
// Si l'écart avec l'angle orienté vers la consigne est grand, la distance cible devient 0
|
||||||
// pour se réorienter vers l'angle de la consigne
|
// pour se réorienter vers l'angle de la consigne
|
||||||
if (oDirEcartAbs > O_DIR_ECART_MAX) {
|
if (oDirEcartAbs > O_DIR_ECART_MAX) {
|
||||||
|
@ -179,7 +181,7 @@ void* TaskMovement(void* pData)
|
||||||
// Calcul si on est arrivé
|
// Calcul si on est arrivé
|
||||||
pthread_mutex_lock(&movDoneMutex);
|
pthread_mutex_lock(&movDoneMutex);
|
||||||
clock_gettime(CLOCK_REALTIME, &pidNow);
|
clock_gettime(CLOCK_REALTIME, &pidNow);
|
||||||
movDoneBool = !dRetenu && !oRetenu && dDirEcart < D_CONS_THRESOLD && oEcart < O_CONS_THRESOLD;
|
movDoneBool = !oRetenu && fabs(oEcart) < O_ECART_MAX;
|
||||||
if (movDoneBool) {
|
if (movDoneBool) {
|
||||||
pthread_cond_signal(&movDoneCond);
|
pthread_cond_signal(&movDoneCond);
|
||||||
}
|
}
|
||||||
|
@ -231,6 +233,7 @@ void* TaskMovement(void* pData)
|
||||||
setMoteurTension(lVolt, rVolt);
|
setMoteurTension(lVolt, rVolt);
|
||||||
|
|
||||||
nbCalcCons++;
|
nbCalcCons++;
|
||||||
|
pthread_mutex_unlock(&movEnableMutex);
|
||||||
}
|
}
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
|
@ -246,6 +249,7 @@ void deconfigureMovement()
|
||||||
|
|
||||||
void enableConsigne()
|
void enableConsigne()
|
||||||
{
|
{
|
||||||
|
printf("251 enableConsigne\n");
|
||||||
pthread_mutex_lock(&movEnableMutex);
|
pthread_mutex_lock(&movEnableMutex);
|
||||||
clock_gettime(CLOCK_REALTIME, &pidNow);
|
clock_gettime(CLOCK_REALTIME, &pidNow);
|
||||||
movEnableBool = true;
|
movEnableBool = true;
|
||||||
|
@ -255,6 +259,7 @@ void enableConsigne()
|
||||||
|
|
||||||
void disableConsigne()
|
void disableConsigne()
|
||||||
{
|
{
|
||||||
|
printf("261 disableConsigne\n");
|
||||||
pthread_mutex_lock(&movEnableMutex);
|
pthread_mutex_lock(&movEnableMutex);
|
||||||
movEnableBool = false;
|
movEnableBool = false;
|
||||||
// No signal here, will be disabled on next run
|
// No signal here, will be disabled on next run
|
||||||
|
|
|
@ -73,8 +73,8 @@ int updateParcours()
|
||||||
void stopParcours()
|
void stopParcours()
|
||||||
{
|
{
|
||||||
pthread_cancel(tParcours);
|
pthread_cancel(tParcours);
|
||||||
stop();
|
|
||||||
disableConsigne();
|
disableConsigne();
|
||||||
|
stop();
|
||||||
|
|
||||||
resetLCD();
|
resetLCD();
|
||||||
updateTimeDisplay();
|
updateTimeDisplay();
|
||||||
|
@ -95,8 +95,8 @@ void gotoPoint(float x, float y, float o)
|
||||||
struct position pos = { x, y, o };
|
struct position pos = { x, y, o };
|
||||||
setDestination(&pos);
|
setDestination(&pos);
|
||||||
waitDestination();
|
waitDestination();
|
||||||
brake();
|
|
||||||
disableConsigne();
|
disableConsigne();
|
||||||
|
brake();
|
||||||
}
|
}
|
||||||
|
|
||||||
void recuperBalles()
|
void recuperBalles()
|
||||||
|
|
|
@ -23,6 +23,23 @@ pthread_t tPosition;
|
||||||
unsigned int nbCalcPos;
|
unsigned int nbCalcPos;
|
||||||
long lCodTot, rCodTot;
|
long lCodTot, rCodTot;
|
||||||
|
|
||||||
|
void onF2CI_CODER()
|
||||||
|
{
|
||||||
|
readCF(&deltaCoders, sizeof(struct F2CI_CODERs));
|
||||||
|
pthread_mutex_unlock(&posPolling);
|
||||||
|
}
|
||||||
|
|
||||||
|
void updateDelta()
|
||||||
|
{
|
||||||
|
// Sending
|
||||||
|
pthread_mutex_lock(&posPolling);
|
||||||
|
sendCF(F2CI_CODER, NULL, 0);
|
||||||
|
|
||||||
|
// Waiting for reception
|
||||||
|
pthread_mutex_lock(&posPolling);
|
||||||
|
pthread_mutex_unlock(&posPolling);
|
||||||
|
}
|
||||||
|
|
||||||
void* TaskPosition(void* pData)
|
void* TaskPosition(void* pData)
|
||||||
{
|
{
|
||||||
(void)pData;
|
(void)pData;
|
||||||
|
@ -32,15 +49,11 @@ void* TaskPosition(void* pData)
|
||||||
lCodTot = 0;
|
lCodTot = 0;
|
||||||
rCodTot = 0;
|
rCodTot = 0;
|
||||||
|
|
||||||
|
updateDelta();
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
|
|
||||||
// Sending
|
updateDelta();
|
||||||
pthread_mutex_lock(&posPolling);
|
|
||||||
sendCF(F2CI_CODER, NULL, 0);
|
|
||||||
|
|
||||||
// Waiting for reception
|
|
||||||
pthread_mutex_lock(&posPolling);
|
|
||||||
pthread_mutex_unlock(&posPolling);
|
|
||||||
|
|
||||||
// Calculation
|
// Calculation
|
||||||
#ifdef INVERSE_L_CODER
|
#ifdef INVERSE_L_CODER
|
||||||
|
@ -67,19 +80,11 @@ void* TaskPosition(void* pData)
|
||||||
nbCalcPos++;
|
nbCalcPos++;
|
||||||
pthread_cond_signal(&newPos);
|
pthread_cond_signal(&newPos);
|
||||||
pthread_mutex_unlock(&posConnu);
|
pthread_mutex_unlock(&posConnu);
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
void onF2CI_CODER()
|
|
||||||
{
|
|
||||||
readCF(&deltaCoders, sizeof(struct F2CI_CODERs));
|
|
||||||
pthread_mutex_unlock(&posPolling);
|
|
||||||
}
|
|
||||||
|
|
||||||
void configurePosition()
|
void configurePosition()
|
||||||
{
|
{
|
||||||
pthread_mutex_init(&posPolling, NULL);
|
pthread_mutex_init(&posPolling, NULL);
|
||||||
|
@ -137,6 +142,6 @@ void setPosition(struct position* pos)
|
||||||
|
|
||||||
void resetPosition()
|
void resetPosition()
|
||||||
{
|
{
|
||||||
struct position pos = {0, 0, 0};
|
struct position pos = { 0, 0, 0 };
|
||||||
setPosition(&pos);
|
setPosition(&pos);
|
||||||
}
|
}
|
||||||
|
|
|
@ -37,18 +37,29 @@ int main()
|
||||||
configureDebug();
|
configureDebug();
|
||||||
configureCF();
|
configureCF();
|
||||||
configureIMU();
|
configureIMU();
|
||||||
|
printf("40\n");
|
||||||
configureActionneurs();
|
configureActionneurs();
|
||||||
|
printf("41\n");
|
||||||
configurePosition();
|
configurePosition();
|
||||||
|
printf("44\n");
|
||||||
configureMovement();
|
configureMovement();
|
||||||
|
printf("46\n");
|
||||||
startDebug();
|
startDebug();
|
||||||
|
printf("48\n");
|
||||||
|
|
||||||
debugSetActive(true);
|
debugSetActive(true);
|
||||||
|
sleep(1);
|
||||||
|
printf("46\n");
|
||||||
enableConsigne();
|
enableConsigne();
|
||||||
struct position pos = { 100, 0, 0 };
|
struct position pos = { 0, -100, -M_PI };
|
||||||
setDestination(&pos);
|
setDestination(&pos);
|
||||||
|
printf("50\n");
|
||||||
waitDestination();
|
waitDestination();
|
||||||
brake();
|
printf("52\n");
|
||||||
disableConsigne();
|
disableConsigne();
|
||||||
|
printf("54\n");
|
||||||
|
brake();
|
||||||
|
printf("Done\n");
|
||||||
|
|
||||||
// Bloque jusqu'à l'arrivée d'un signal
|
// Bloque jusqu'à l'arrivée d'un signal
|
||||||
pthread_mutex_init(&sRunning, NULL);
|
pthread_mutex_init(&sRunning, NULL);
|
||||||
|
|
19
simu/simu.m
19
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 = "000303.csv";
|
FILENAME = "000471.csv";
|
||||||
PATH = DIRNAME + FILENAME;
|
PATH = DIRNAME + FILENAME;
|
||||||
|
|
||||||
% Paramètres de simulation
|
% Paramètres de simulation
|
||||||
|
@ -45,15 +45,14 @@ coderFullResolution = (coderDataResolution / reducRatio); % cycles / wheel rev
|
||||||
avPerCycle = (wheelPerimeter / coderFullResolution); % mm
|
avPerCycle = (wheelPerimeter / coderFullResolution); % mm
|
||||||
|
|
||||||
% Constantes asservissement
|
% Constantes asservissement
|
||||||
global dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oGain dConsThresold oConsThresold;
|
global dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oGain oEcartMax;
|
||||||
dDirEcartMin = 1.0; % mm
|
dDirEcartMin = 7.0; % mm
|
||||||
dDirEcartMax = 5.0; % mm
|
dDirEcartMax = 10.0; % mm
|
||||||
oDirEcartMin = (6.0 / 360.0 * 2.0 * pi); % rad
|
oDirEcartMin = (5.0 / 360.0 * 2.0 * pi); % rad
|
||||||
oDirEcartMax = (45.0 / 360.0 * 2.0 * pi); % rad
|
oDirEcartMax = (25.0 / 360.0 * 2.0 * pi); % rad
|
||||||
dConsThresold = 1.0; % mm
|
oEcartMax = (25.0 / 360.0 * 2.0 * pi); % rad
|
||||||
oConsThresold = (6.0 / 360.0 * 2.0 * pi); % rad
|
|
||||||
oGain = 3.0;
|
oGain = 3.0;
|
||||||
P = 3.0;
|
P = 7.0;
|
||||||
I = 0.0;
|
I = 0.0;
|
||||||
D = 0.0;
|
D = 0.0;
|
||||||
|
|
||||||
|
@ -195,7 +194,7 @@ function timeGraph(series)
|
||||||
end
|
end
|
||||||
xlim([0 SIMULATION_TIME]);
|
xlim([0 SIMULATION_TIME]);
|
||||||
if (abs(m) ~= inf) && (abs(M) ~= inf)
|
if (abs(m) ~= inf) && (abs(M) ~= inf)
|
||||||
ylim([m M]);
|
ylim([m M+1E-9]);
|
||||||
end
|
end
|
||||||
addTimeline(p);
|
addTimeline(p);
|
||||||
end
|
end
|
||||||
|
|
Loading…
Reference in a new issue