1
0
Fork 0
mirror of https://github.com/RobotechLille/cdf2018-principal synced 2024-11-14 12:26:06 +01:00

WIP Contrôle

This commit is contained in:
Geoffrey Frogeye 2018-05-09 14:24:12 +02:00
parent 8813455901
commit 1337bd62a9
9 changed files with 78 additions and 90 deletions

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

View file

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

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 = "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