Parcours total

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;
+        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()

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

chef/src/parcours.c

@@ -2,9 +2,9 @@
 #include <stdio.h>
 #include <unistd.h>
 
+#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};
+    // Récupération des balles
-    setDestination(&dest1);
+    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};
+    // Évitement des cubes
-    setDestination(&dest2);
+    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;
 }

chef/src/parcours.h

@@ -2,8 +2,41 @@
 #define __PARCOURS_H__
 
 #include <stdbool.h>
+#include <math.h>
+
+#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);