geoffrey/cdf2018-principal
geoffrey/cdf2018-principal
1
0
Fork 0
mirror of https://github.com/RobotechLille/cdf2018-principal synced 2024-11-21 15:46:06 +01:00
Code Issues Releases Wiki Activity

WIP Contrôle

Browse source
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
Show stats Download patch file Download diff file Expand all files Collapse all files

13
chef/src/dimensions.h
View file

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

50
chef/src/local.c
View file

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

5
chef/src/motor.c
View file

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

2
chef/src/motor.h
View file

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

23
chef/src/movement.c
View file

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

4
chef/src/parcours.c
View file

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

37
chef/src/position.c
View file

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

15
chef/src/test1m.c
View file

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

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