mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-11-14 04:16:05 +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
|
||||
|
||||
// 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
|
||||
|
||||
|
|
|
@ -1,52 +1,16 @@
|
|||
#include <pthread.h>
|
||||
#include <signal.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <time.h> // random seed
|
||||
#include <unistd.h> // 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));
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
19
simu/simu.m
19
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
|
||||
|
|
Loading…
Reference in a new issue