From 9cdebaa91581d8cbbd9476fa0c265499b75d62fe Mon Sep 17 00:00:00 2001 From: Geoffrey Frogeye Date: Wed, 9 May 2018 04:10:45 +0200 Subject: [PATCH] Parcours total --- chef/src/movement.c | 26 +++++++------ chef/src/movement.h | 3 ++ chef/src/parcours.c | 91 +++++++++++++++++++++++++++++++++++++-------- chef/src/parcours.h | 33 ++++++++++++++++ 4 files changed, 126 insertions(+), 27 deletions(-) diff --git a/chef/src/movement.c b/chef/src/movement.c index 26037f0..8f90dab 100644 --- a/chef/src/movement.c +++ b/chef/src/movement.c @@ -8,6 +8,7 @@ #include "debug.h" #include "motor.h" #include "movement.h" +#include "securite.h" pthread_t tMovement; struct position cons; @@ -44,6 +45,8 @@ void configureMovement() { stop(); + configureSecurite(); + nbCalcCons = 0; pthread_mutex_init(&movCons, NULL); @@ -93,16 +96,6 @@ void waitDestination() pthread_mutex_unlock(&movDoneMutex); } -void sendTo(float x, float y, float o) -{ - enableConsigne(); - struct position pos = {x, y, o}; - setDestination(&pos); - waitDestination(); - brake(); - disableConsigne(); -} - float angleGap(float target, float actual) { return fmod(target - actual + M_PI, 2 * M_PI) - M_PI; @@ -144,10 +137,11 @@ void* TaskMovement(void* pData) lastPosCalc = getPositionNewer(&connu, lastPosCalc); // Destination → ordre + bool angleInsignifiant = isnan(cons.o); pthread_mutex_lock(&movCons); xDiff = cons.x - connu.x; yDiff = cons.y - connu.y; - oEcart = angleGap(cons.o, connu.o); + oEcart = angleInsignifiant ? 0 : angleGap(cons.o, connu.o); // Distance d'écart entre la position actuelle et celle de consigne dDirEcart = hypotf(xDiff, yDiff); @@ -178,11 +172,18 @@ void* TaskMovement(void* pData) } dErr = dRetenu ? 0 : dDirEcart; + // Limitation par les valeurs des capteurs + float avant, arriere; + getDistance(&avant, &arriere); + dErr = fmaxf(-arriere, fminf(avant, dErr)); + // 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; - pthread_cond_signal(&movDoneCond); + if (movDoneBool) { + pthread_cond_signal(&movDoneCond); + } pthread_mutex_unlock(&movDoneMutex); // Ordre → Volt @@ -240,6 +241,7 @@ void deconfigureMovement() { stop(); pthread_cancel(tMovement); + deconfigureSecurite(); } void enableConsigne() diff --git a/chef/src/movement.h b/chef/src/movement.h index c488335..1183d8b 100644 --- a/chef/src/movement.h +++ b/chef/src/movement.h @@ -5,6 +5,8 @@ #ifndef __MOVEMENT_H_ #define __MOVEMENT_H_ +#define ANGLE_INSIGNIFIANT NAN + #include "position.h" void configureMovement(); @@ -13,5 +15,6 @@ void setDestination(struct position* pos); void* TaskMovement(void* pData); void enableConsigne(); void disableConsigne(); +void waitDestination(); #endif diff --git a/chef/src/parcours.c b/chef/src/parcours.c index bc0f3e8..613043f 100644 --- a/chef/src/parcours.c +++ b/chef/src/parcours.c @@ -2,9 +2,9 @@ #include #include +#include "actionneurs.h" #include "debug.h" #include "lcd.h" -#include "movement.h" #include "motor.h" #include "parcours.h" #include "points.h" @@ -32,6 +32,8 @@ void prepareParcours(bool orange) resetPoints(); showPoints(); printRightLCD(LCD_LINE_2, isOrange ? "Org" : "Vrt"); + + resetActionneurs(); enableConsigne(); } @@ -81,29 +83,88 @@ void stopParcours() debugSetActive(false); } +void gotoPoint(float x, float y, float o) +{ + if (isOrange) { + x = M_PISTE_WIDTH - x; + if (!isnan(o)) { + o = M_PI - o; + } + } + enableConsigne(); + struct position pos = { x, y, o }; + setDestination(&pos); + waitDestination(); + brake(); + disableConsigne(); +} + +void recuperBalles() +{ + setLoquet(false); + for (int i = 0; i < NB_BALLES; i++) { + barilletSuivant(); + } + setLoquet(true); +} + void* TaskParcours(void* pdata) { (void)pdata; - struct position dest1 = {0, 0, 0}; - setDestination(&dest1); + // Récupération des balles + gotoPoint(X_RECUP_1, Y_RECUP_1, O_RECUP_1); + setLoquet(false); + recuperBalles(); - sleep(1); + // Lancement des balles + gotoPoint(X_LANCER, Y_LANCER, O_LANCER); + setPositionBalle(ejection); + setPropulsion(true); + pousserBalle(); + for (int i = 0; i < NB_BALLES - 1; i++) { + barilletSuivant(); + pousserBalle(); + } + setPropulsion(false); + setPositionBalle(attente); - struct position dest2 = {0, 0, M_PI_2}; - setDestination(&dest2); + // Évitement des cubes + gotoPoint(X_EVIT, Y_EVIT, O_EVIT); - sleep(10); + // Aller à l'abeille + gotoPoint(X_ABEILLE, Y_ABEILLE, O_ABEILLE); + + // Récupération des balles + gotoPoint(X_RECUP_2, Y_RECUP_2, O_RECUP_2); + recuperBalles(); + + // Dépot des balles adverses + gotoPoint(X_USE, Y_USE, O_USE); + setPositionBalle(evacuation); + barilletSuivant(); // TODO Peut-être pas utile en fonction + // de quelle balle arrive en premier + pousserBalle(); + for (int i = 0; i < NB_BALLES / 2 - 1; i++) { + barilletSkip(); + pousserBalle(); + } + setPositionBalle(attente); + + // Lancement des balles + gotoPoint(X_LANCER, Y_LANCER, O_LANCER); + setPositionBalle(ejection); + setPropulsion(true); + barilletSuivant(); + pousserBalle(); + for (int i = 0; i < NB_BALLES / 2 - 1; i++) { + barilletSkip(); + pousserBalle(); + } + setPropulsion(false); + setPositionBalle(attente); stop(); - /* */ - /* struct position dest3 = {1000, 1000, 0}; */ - /* setDestination(&dest3); */ - /* */ - /* sleep(5); */ - - - return NULL; } diff --git a/chef/src/parcours.h b/chef/src/parcours.h index e4bdff7..3978810 100644 --- a/chef/src/parcours.h +++ b/chef/src/parcours.h @@ -2,8 +2,41 @@ #define __PARCOURS_H__ #include +#include + +#include "movement.h" #define TEMPS_PARCOURS 100 +#define NB_BALLES 8 + +#define X_RECUP_1 1160 +#define Y_RECUP_1 210 +#define O_RECUP_1 -M_PI + +#define X_LANCER X_RECUP_1 +#define Y_LANCER Y_RECUP_1 + 100 +#define O_LANCER 3 * M_PI_4 + +#define X_RECUP_2 610 +#define Y_RECUP_2 50 +// ↑ +#define O_RECUP_2 -M_PI_2 + +#define X_EVIT X_RECUP_2 +#define Y_EVIT 1000 +#define O_EVIT ANGLE_INSIGNIFIANT + +#define X_ABEILLE 210 +#define Y_ABEILLE 50 +// ↑ +#define O_ABEILLE -M_PI_2 + +#define X_USE 800 +// ↑ +#define Y_USE 100 +// ↑ +#define O_USE 0 + void configureParcours(); void prepareParcours(bool orange);