From 1337bd62a906b3c27c495557cf5a656c34a21064 Mon Sep 17 00:00:00 2001 From: Geoffrey Frogeye Date: Wed, 9 May 2018 14:24:12 +0200 Subject: [PATCH] =?UTF-8?q?WIP=20Contr=C3=B4le?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- chef/src/dimensions.h | 13 ++++++----- chef/src/local.c | 50 ++++++------------------------------------- chef/src/motor.c | 5 +++++ chef/src/motor.h | 2 +- chef/src/movement.c | 23 ++++++++++++-------- chef/src/parcours.c | 4 ++-- chef/src/position.c | 37 ++++++++++++++++++-------------- chef/src/test1m.c | 15 +++++++++++-- simu/simu.m | 19 ++++++++-------- 9 files changed, 78 insertions(+), 90 deletions(-) diff --git a/chef/src/dimensions.h b/chef/src/dimensions.h index 31a3fc7..2f30e0f 100644 --- a/chef/src/dimensions.h +++ b/chef/src/dimensions.h @@ -33,14 +33,13 @@ #define AV_PER_CYCLE (WHEEL_PERIMETER / CODER_FULL_RESOLUTION) // mm // Constantes asservissement -#define D_DIR_ECART_MIN 1.0 // mm -#define D_DIR_ECART_MAX 5.0 // mm -#define O_DIR_ECART_MIN (6.0 / 360.0 * 2.0 * M_PI) // rad -#define O_DIR_ECART_MAX (45.0 / 360.0 * 2.0 * M_PI) // rad -#define D_CONS_THRESOLD 1.0 // mm -#define O_CONS_THRESOLD (6.0 / 360.0 * 2.0 * M_PI) // rad +#define D_DIR_ECART_MIN 7.0 // mm +#define D_DIR_ECART_MAX 10.0 // mm +#define O_DIR_ECART_MIN (5.0 / 360.0 * 2.0 * M_PI) // rad +#define O_DIR_ECART_MAX (25.0 / 360.0 * 2.0 * M_PI) // rad +#define O_ECART_MAX (25.0 / 360.0 * 2.0 * M_PI) // rad #define O_GAIN 3.0 -#define P 5.0 +#define P 7.0 #define I 0.0 #define D 0.0 diff --git a/chef/src/local.c b/chef/src/local.c index 5fa50b1..e013d6d 100644 --- a/chef/src/local.c +++ b/chef/src/local.c @@ -1,52 +1,16 @@ -#include -#include +#include #include #include -#include // random seed -#include // sleep -pthread_mutex_t posConnu; -pthread_cond_t newPos; -pthread_t tPosition; -pthread_t tMovement; - -void* TaskPosition(void* pData) +float angleGap(float target, float actual) { - - (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; + return remainderf(target - actual + M_PI, 2 * M_PI) - M_PI; } int main() { - pthread_mutex_init(&posConnu, NULL); - pthread_cond_init(&newPos, NULL); - pthread_create(&tPosition, NULL, TaskPosition, NULL); - pthread_create(&tMovement, NULL, TaskMovement, NULL); - sleep(300); + float cons = 2; + for (float conn = 0; conn < 10.0; conn += 0.1) { + printf("%6f\n", angleGap(cons, conn)); + } } diff --git a/chef/src/motor.c b/chef/src/motor.c index 7ee0162..dacdfbb 100644 --- a/chef/src/motor.c +++ b/chef/src/motor.c @@ -161,10 +161,14 @@ void* TaskMotor(void* pData) void rawBrake() { + lVolt = 0; + rVolt = 0; sendCF(C2FD_PWM, &msgBrake, sizeof(struct C2FD_PWMs)); } void rawFreewheel() { + lVolt = 0; + rVolt = 0; sendCF(C2FD_PWM, &msgFree, sizeof(struct C2FD_PWMs)); } @@ -189,6 +193,7 @@ int brake() pthread_mutex_lock(&motCons); motState = braking; pthread_mutex_unlock(&motCons); + printf("192 Brake\n"); rawBrake(); } diff --git a/chef/src/motor.h b/chef/src/motor.h index da04afd..dfa1480 100644 --- a/chef/src/motor.h +++ b/chef/src/motor.h @@ -11,7 +11,7 @@ // #define INVERSE_R_MOTOR // V/s -#define RATE_LIMITER_UP 12 +#define RATE_LIMITER_UP 6 #define RATE_LIMITER_DOWN 1 #define TESTINATOR diff --git a/chef/src/movement.c b/chef/src/movement.c index 44572b1..b89697b 100644 --- a/chef/src/movement.c +++ b/chef/src/movement.c @@ -95,15 +95,16 @@ void waitDestination() 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) { - 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; } - 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) @@ -128,7 +129,6 @@ void* TaskMovement(void* pData) while (!movEnableBool) { pthread_cond_wait(&movEnableCond, &movEnableMutex); } - pthread_mutex_unlock(&movEnableMutex); // Wait for new calculation if not calculated yet 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 // Si l'angle se trouve à gauche du cercle trigo, on le remet à droite // 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); // 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; // 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; } 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 // pour se réorienter vers l'angle de la consigne if (oDirEcartAbs > O_DIR_ECART_MAX) { @@ -179,7 +181,7 @@ void* TaskMovement(void* pData) // Calcul si on est arrivé pthread_mutex_lock(&movDoneMutex); 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) { pthread_cond_signal(&movDoneCond); } @@ -231,6 +233,7 @@ void* TaskMovement(void* pData) setMoteurTension(lVolt, rVolt); nbCalcCons++; + pthread_mutex_unlock(&movEnableMutex); } return NULL; @@ -246,6 +249,7 @@ void deconfigureMovement() void enableConsigne() { + printf("251 enableConsigne\n"); pthread_mutex_lock(&movEnableMutex); clock_gettime(CLOCK_REALTIME, &pidNow); movEnableBool = true; @@ -255,6 +259,7 @@ void enableConsigne() void disableConsigne() { + printf("261 disableConsigne\n"); pthread_mutex_lock(&movEnableMutex); movEnableBool = false; // No signal here, will be disabled on next run diff --git a/chef/src/parcours.c b/chef/src/parcours.c index 28e2776..904c1e2 100644 --- a/chef/src/parcours.c +++ b/chef/src/parcours.c @@ -73,8 +73,8 @@ int updateParcours() void stopParcours() { pthread_cancel(tParcours); - stop(); disableConsigne(); + stop(); resetLCD(); updateTimeDisplay(); @@ -95,8 +95,8 @@ void gotoPoint(float x, float y, float o) struct position pos = { x, y, o }; setDestination(&pos); waitDestination(); - brake(); disableConsigne(); + brake(); } void recuperBalles() diff --git a/chef/src/position.c b/chef/src/position.c index c7cac0b..b551a15 100644 --- a/chef/src/position.c +++ b/chef/src/position.c @@ -23,6 +23,23 @@ pthread_t tPosition; unsigned int nbCalcPos; 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)pData; @@ -32,15 +49,11 @@ void* TaskPosition(void* pData) lCodTot = 0; rCodTot = 0; + updateDelta(); + for (;;) { - // Sending - pthread_mutex_lock(&posPolling); - sendCF(F2CI_CODER, NULL, 0); - - // Waiting for reception - pthread_mutex_lock(&posPolling); - pthread_mutex_unlock(&posPolling); + updateDelta(); // Calculation #ifdef INVERSE_L_CODER @@ -67,19 +80,11 @@ void* TaskPosition(void* pData) nbCalcPos++; pthread_cond_signal(&newPos); pthread_mutex_unlock(&posConnu); - - } return NULL; } -void onF2CI_CODER() -{ - readCF(&deltaCoders, sizeof(struct F2CI_CODERs)); - pthread_mutex_unlock(&posPolling); -} - void configurePosition() { pthread_mutex_init(&posPolling, NULL); @@ -137,6 +142,6 @@ void setPosition(struct position* pos) void resetPosition() { - struct position pos = {0, 0, 0}; + struct position pos = { 0, 0, 0 }; setPosition(&pos); } diff --git a/chef/src/test1m.c b/chef/src/test1m.c index 0eaf330..488ab82 100644 --- a/chef/src/test1m.c +++ b/chef/src/test1m.c @@ -37,18 +37,29 @@ int main() configureDebug(); configureCF(); configureIMU(); + printf("40\n"); configureActionneurs(); + printf("41\n"); configurePosition(); + printf("44\n"); configureMovement(); + printf("46\n"); startDebug(); + printf("48\n"); debugSetActive(true); + sleep(1); + printf("46\n"); enableConsigne(); - struct position pos = { 100, 0, 0 }; + struct position pos = { 0, -100, -M_PI }; setDestination(&pos); + printf("50\n"); waitDestination(); - brake(); + printf("52\n"); disableConsigne(); + printf("54\n"); + brake(); + printf("Done\n"); // Bloque jusqu'à l'arrivée d'un signal pthread_mutex_init(&sRunning, NULL); diff --git a/simu/simu.m b/simu/simu.m index 3afaa07..04cfc8c 100644 --- a/simu/simu.m +++ b/simu/simu.m @@ -3,7 +3,7 @@ SIMULATION = 0; % Paramètres de lecture DIRNAME = "/home/geoffrey/CdF/cdf2018-principal/log/"; -FILENAME = "000303.csv"; +FILENAME = "000471.csv"; PATH = DIRNAME + FILENAME; % Paramètres de simulation @@ -45,15 +45,14 @@ coderFullResolution = (coderDataResolution / reducRatio); % cycles / wheel rev avPerCycle = (wheelPerimeter / coderFullResolution); % mm % Constantes asservissement -global dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oGain dConsThresold oConsThresold; -dDirEcartMin = 1.0; % mm -dDirEcartMax = 5.0; % mm -oDirEcartMin = (6.0 / 360.0 * 2.0 * pi); % rad -oDirEcartMax = (45.0 / 360.0 * 2.0 * pi); % rad -dConsThresold = 1.0; % mm -oConsThresold = (6.0 / 360.0 * 2.0 * pi); % rad +global dDirEcartMin dDirEcartMax oDirEcartMin oDirEcartMax oGain oEcartMax; +dDirEcartMin = 7.0; % mm +dDirEcartMax = 10.0; % mm +oDirEcartMin = (5.0 / 360.0 * 2.0 * pi); % rad +oDirEcartMax = (25.0 / 360.0 * 2.0 * pi); % rad +oEcartMax = (25.0 / 360.0 * 2.0 * pi); % rad oGain = 3.0; -P = 3.0; +P = 7.0; I = 0.0; D = 0.0; @@ -195,7 +194,7 @@ function timeGraph(series) end xlim([0 SIMULATION_TIME]); if (abs(m) ~= inf) && (abs(M) ~= inf) - ylim([m M]); + ylim([m M+1E-9]); end addTimeline(p); end