add switch read

Use core 0 for ultrasonic detection
Use a custom class to pilot servo

README.md

@@ -1,2 +1,2 @@
-# PAMI-2025
-Code pour les PAMI de 2025
+# PAMI-2026
+Code pour les PAMI de 2026

include/Servo.h

@@ -0,0 +1,40 @@
+#ifndef PAMI_SERVO_H
+#define PAMI_SERVO_H
+
+#define DEFAULT_MIN_US 544
+#define DEFAULT_MAX_US 2400
+
+#define DEFAULT_MIN_ANGLE 0
+#define DEFAULT_MAX_ANGLE 180
+
+class Servo {
+private:
+    int channel{};
+    int pin{};
+    int min_us{};
+    int max_us{};
+    int min_angle{};
+    int max_angle{};
+
+    void initServo() const;
+
+public:
+    Servo();
+
+    Servo(int pin, int channel, int min_angle = DEFAULT_MIN_ANGLE, int max_angle = DEFAULT_MAX_ANGLE,
+          int min_us = DEFAULT_MIN_US,
+          int max_us = DEFAULT_MAX_US);
+
+    void writeAngle(int angle) const;
+
+    void writeUs(int us) const;
+
+    /**
+     * Write to the servo the value as degree if it's in the degree range of the servo otherwise as us value
+     * @param value The value to write in angle or us
+     */
+    void write(int value) const;
+};
+
+
+#endif //PAMI_SERVO_H

include/main.h

@@ -1,8 +1,7 @@
-#ifndef MAIN_H
-#define MAIN_H
+#ifndef PAMI_MAIN_H
+#define PAMI_MAIN_H
 
-enum Direction
-{
+enum Direction {
     FORWARD,
     BACKWARD,
     LEFT,
@@ -10,18 +9,16 @@ enum Direction
     STOP
 };
 
-enum StepType
-{
+enum StepType {
     STEP_FORWARD,
     STEP_ROTATE,
     STEP_FORWARD_UNTIL_END,
     STEP_BACKWARD
 };
 
-struct Step
-{
+struct Step {
     StepType type;
-    float value;   // cm pour STEP_FORWARD, deg pour ROTATE, ignoré pour UNTIL_FALL
+    float value; // cm pour STEP_FORWARD, deg pour ROTATE, ignoré pour UNTIL_FALL
     int32_t speed; // Vitesse en pas / seconde
 };
 
@@ -34,25 +31,24 @@ struct Step
 // PINs réels
 #ifndef SIMULATOR
 
-// Moteur 1 - Gauche
+// Moteur 1 - Droite
 #define M1_DIR_PIN 19
 #define M1_STEP_PIN 23
 #define M1_ENABLE_PIN 26
 
-// Moteur 2 - Droite
+// Moteur 2 - Gauche
 #define M2_DIR_PIN 21
 #define M2_STEP_PIN 25
 #define M2_ENABLE_PIN 27
 
 // Capteur à ultrasons
-#define TRIG_PIN 15
-#define ECHO_PIN 14
-
-// Capteur de chute
-#define FALL_PIN 13
-
-#define SERVO_PIN 12
+#define TRIG_PIN 13
+#define ECHO_PIN 12
+// Servo Moteur
+#define SERVO_PIN 15
 #define EMG_PIN 5
+// Bouton de sélection de côté
+#define SWITCH_PIN 14
 
 #endif
 
@@ -88,10 +84,11 @@ struct Step
 #define DRIVER1_ADDR 0b01
 #define DRIVER2_ADDR 0b10
 
+// TODO : Changer les constantes avec nouvelles dim
 // Constantes
-#define WHEEL_DIAMETER 5.5 // cm
-#define WHEEL_BASE 7.2     // cm
+#define WHEEL_DIAMETER 5.6  // cm
+#define WHEEL_BASE 7        // cm
 #define STEPS_PER_REV 200.0
 #define MICROSTEPPING 8.0
 
-#endif // MAIN_H
+#endif // PAMI_MAIN_H

include/utils.h

@@ -1,10 +1,14 @@
-#ifndef UTILS_H
-#define UTILS_H
+#ifndef PAMI_UTILS_H
+#define PAMI_UTILS_H
+
+void enableMotorDrivers();
 
-void enableDrivers();
 void disableDrivers();
 
 int getStepsForDistance(float cm);
+
 int getRotationSteps(float angleDeg);
 
-#endif // UTILS_H
+int readSwitchOnce(int pin);
+
+#endif // PAMI_UTILS_H

platformio.ini

@@ -12,3 +12,4 @@
 platform = espressif32
 board = esp32dev
 framework = arduino
+monitor_speed = 115200

src/Servo.cpp

@@ -0,0 +1,41 @@
+#include "Servo.h"
+
+#include <Arduino.h>
+
+void Servo::initServo() const {
+    pinMode(pin, OUTPUT);
+    ledcSetup(channel, 50, 16);
+    ledcAttachPin(pin, channel);
+}
+
+Servo::Servo() = default;
+
+Servo::Servo(int pin, int channel, int min_angle, int max_angle, int min_us, int max_us) {
+    this->pin = pin;
+    this->channel = channel;
+    this->min_us = min_us;
+    this->max_us = max_us;
+    this->min_angle = min_angle;
+    this->max_angle = max_angle;
+
+    initServo();
+}
+
+void Servo::writeAngle(int angle) const {
+    if (angle < min_angle) angle = min_angle;
+    if (angle > max_angle) angle = max_angle;
+    const int value = map(angle, min_angle, max_angle, min_us, max_us);
+    writeUs(value);
+}
+
+void Servo::writeUs(int us) const {
+    if (us < min_us) us = min_us;
+    if (us > max_us) us = max_us;
+    int duty = us * 65535 / 20000;
+    ledcWrite(channel, duty);
+}
+
+void Servo::write(int value) const {
+    if (value > min_angle && value < max_angle) return writeAngle(value);
+    return writeUs(value);
+}

src/main.cpp

@@ -1,143 +1,137 @@
 #include <Arduino.h>
-// #define SIMULATOR // Commenter cette ligne pour utiliser le matériel réel
-//     Définitions des constantes dans main.h
 #include "main.h"
+
+#include "Servo.h"
 #include "utils.h"
 
+// TODO : Sélection du camps avec un bouton et non un define
 // Côté bleu = 1 et Côté jaune = 2
 #define PAMI_SIDE 1
-// Numéro du pami pour les différents robots car chemins différents
-#define PAMI_NUM 4
+#define PAMI_NUM 2
 #define START_DELAY 5000
 
-uint32_t startTime = 0;
-
-volatile int32_t speed_steps_per_sec = 0; // target speed (signed)
-uint32_t last_step_time = 0;
-
-// Pour le redémarrage des moteurs après capteur obstacle
+// Stepper
+volatile int32_t speed_steps_per_sec = 0;
+volatile int32_t steps_target = 0;
+volatile int32_t steps_done = 0;
+volatile bool movementInProgress = false;
 Direction lastDirection = STOP;
 volatile int32_t lastSpeed = 0;
 
-volatile int32_t steps_target = 0;
-volatile int32_t steps_done = 0;
-bool movementInProgress = false;
-
-// Les différents scénario possibles : le superstar et les autres
+// Scénario
 #if PAMI_NUM == 1
-// Chaque étape du scénario
 Step scenario[] = {
-    {STEP_FORWARD, 105, 2500},
-    {STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90, 1000}, // Tourner à gauche si côté bleu, droite si jaune
-    {STEP_FORWARD_UNTIL_END, 50, 2000}};
+  {STEP_FORWARD, 105, 2500},
+  {STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90, 1000},
+  {STEP_FORWARD, 50, 2000}
+};
+#elif PAMI_NUM == 2
+Step scenario[] = {
+  {STEP_BACKWARD, 35, 2500},
+  {STEP_BACKWARD, 80 - 10 * PAMI_NUM, 3000}
+};
 #else
 Step scenario[] = {
-    {STEP_FORWARD, 35, 2500},
-    {STEP_ROTATE, PAMI_SIDE == 1 ? -45 + 10 * (PAMI_NUM - 2) : 45 - 10 * (PAMI_NUM - 2), 500},
-    {STEP_FORWARD, 80 - 10 * PAMI_NUM, 3000},
-    {STEP_ROTATE, PAMI_SIDE == 1 ? 45 - 10 * (PAMI_NUM - 2) : -45 + 10 * (PAMI_NUM - 2), 500},
-    {STEP_FORWARD, 60 - 10 * PAMI_NUM, 3000},
-    {STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90, 500},
-    {STEP_BACKWARD, 50 - 10 * PAMI_NUM, 1000}};
+  {STEP_FORWARD, 35, 2500},
+  {STEP_ROTATE, PAMI_SIDE == 1 ? -45 + 10 * (PAMI_NUM - 2) : 45 - 10 * (PAMI_NUM - 2), 500},
+  {STEP_FORWARD, 80 - 10 * PAMI_NUM, 3000}
+};
 #endif
 
 const int scenarioLength = sizeof(scenario) / sizeof(Step);
 int currentScenarioStep = 0;
 bool scenarioInProgress = false;
 
-// Arrêt des moteurs
-void stopMotors()
-{
-  speed_steps_per_sec = 0;
+// Ultrason (non bloquant)
+volatile unsigned long distanceCM = 1000;
+const unsigned int obstacleThresholdCM = 7;
+unsigned long lastUltrasonicTrigger = 0;
+bool waitingEcho = false;
+unsigned long echoStart = 0;
+
+// Servo
+const int SERVO_EAT_UP = 135;
+const int SERVO_EAT_DOWN = 45;
+Servo servo_eat;
+
+// Tirette et EMG
+bool tirettePose = false;
+uint32_t startTime = 0;
+
+// Tâche ultrason sur Core 0
+[[noreturn]] void ultrasonicTask(void *pvParameters) {
+  for (;;) {
+    // Trigger
+    digitalWrite(TRIG_PIN, LOW);
+    delayMicroseconds(2);
+    digitalWrite(TRIG_PIN, HIGH);
+    delayMicroseconds(20);
+    digitalWrite(TRIG_PIN, LOW);
+
+    // Lecture bloquante
+    unsigned long duration = pulseIn(ECHO_PIN, HIGH, 20000); // timeout 20ms
+    unsigned long newDistance = duration * 0.034 / 2; // cm
+
+    distanceCM = newDistance;
+
+    vTaskDelay(50 / portTICK_PERIOD_MS); // 50ms entre mesures
+  }
 }
 
-// Servo moteur
-const int angleCentre = 90;                 // Position centrale
-const int amplitude = 10;                   // Amplitude de mouvement
-const unsigned long intervalleServo = 1000; // Intervalle en millisecondes
-unsigned long tempsServoPrecedent = 0;
-bool directionServo = true; // true = vers le haut, false = vers le bas
-
-// Fonction d'initialisation
-void setup()
-{
+// ========================= Setup =========================
+void setup() {
   Serial.begin(115200);
-  pinMode(EMG_PIN, INPUT);
+
+  // Pins moteurs
+  // Moteur 1 (Droite)
   pinMode(M1_STEP_PIN, OUTPUT);
   pinMode(M1_DIR_PIN, OUTPUT);
   pinMode(M1_ENABLE_PIN, OUTPUT);
+  // Moteur 2 (Gauche)
   pinMode(M2_STEP_PIN, OUTPUT);
   pinMode(M2_DIR_PIN, OUTPUT);
   pinMode(M2_ENABLE_PIN, OUTPUT);
-  pinMode(TIRETTE_PIN, INPUT_PULLUP);
-  // Capteur à ultrasons
+
+  // Capteurs
   pinMode(TRIG_PIN, OUTPUT);
   pinMode(ECHO_PIN, INPUT);
-  // Capteur de chute
-  pinMode(FALL_PIN, INPUT);
+  // Tirette de démarrage
+  pinMode(TIRETTE_PIN, INPUT_PULLUP);
+  // Bouton d'arrêt d'urgence (HIGH si appuyé)
+  pinMode(EMG_PIN, INPUT);
 
-  // Servo moteur
-  // pinMode(SERVO_PIN, OUTPUT);
+  // Servo
+  servo_eat = Servo(SERVO_PIN, 1);
+  servo_eat.writeAngle(SERVO_EAT_UP);
 
-  ledcSetup(0, 50, 16);
-  ledcAttachPin(SERVO_PIN, 0);
 
-  enableDrivers();
-  stopMotors();
+  enableMotorDrivers();
+  speed_steps_per_sec = 0;
 
-  scenarioInProgress = true;
-  currentScenarioStep = 0;
+  // Créer la tâche sur Core 0
+  xTaskCreatePinnedToCore(
+    ultrasonicTask,
+    "UltrasonicTask",
+    2000,
+    NULL,
+    1,
+    NULL,
+    0); // Core 0
 
   Serial.println("Setup complete");
 }
 
-void moveAsyncSteps(int32_t steps, int32_t speed, bool forwardDir)
-{
-  // Le nombre de pas à faire
-  steps_target = steps;
-  steps_done = 0;
-
-  // La direction du moteur
-  digitalWrite(M1_DIR_PIN, forwardDir ? HIGH : LOW);
-  digitalWrite(M2_DIR_PIN, forwardDir ? HIGH : LOW);
-
-  speed_steps_per_sec = speed;
-  movementInProgress = true;
-  lastDirection = forwardDir ? FORWARD : BACKWARD;
-}
-
-void rotateAsync(float angleDeg, int32_t speed, bool toRight)
-{
-
-  // Le nombre de pas à faire
-  steps_target = getRotationSteps(angleDeg + 10.0); // 10.0 correspond à une correction trouvée à la main :)
-  steps_done = 0;
-
-  // La direction du moteur
-  digitalWrite(M1_DIR_PIN, toRight ? HIGH : LOW);
-  digitalWrite(M2_DIR_PIN, toRight ? LOW : HIGH);
-
-  speed_steps_per_sec = speed;
-  movementInProgress = true;
-  lastDirection = toRight ? RIGHT : LEFT;
-}
-
-// Non-blocking stepper update function
-void updateSteppers()
-{
+// ========================= Steppers =========================
+void updateSteppers() {
+  static uint32_t last_step_time1 = 0;
   uint32_t now = micros();
 
-  // Moteur 1
-  if (speed_steps_per_sec != 0 && (steps_done < steps_target))
-  {
-    uint32_t interval1 = 1000000UL / abs(speed_steps_per_sec);
-    static uint32_t last_step_time1 = 0;
-    if (now - last_step_time1 >= interval1)
-    {
-
+  if (speed_steps_per_sec != 0 && steps_done < steps_target) {
+    uint32_t interval = 1000000UL / abs(speed_steps_per_sec);
+    if (now - last_step_time1 >= interval) {
       digitalWrite(M1_STEP_PIN, HIGH);
       digitalWrite(M2_STEP_PIN, HIGH);
-      delayMicroseconds(2); // Short pulse width for step signal
+      delayMicroseconds(2);
       digitalWrite(M1_STEP_PIN, LOW);
       digitalWrite(M2_STEP_PIN, LOW);
       last_step_time1 = now;
@@ -145,261 +139,123 @@ void updateSteppers()
     }
   }
 
-  // Comptage des pas (on considère le moteur le plus lent pour terminer l'étape)
-  if (movementInProgress && steps_done >= steps_target)
-  {
-
-    stopMotors();
+  if (movementInProgress && steps_done >= steps_target) {
+    speed_steps_per_sec = 0;
     movementInProgress = false;
-    Serial.println("Etape terminé");
+    Serial.println("Etape terminée");
   }
 }
 
-// Vérifie si les moteurs sont en mouvement
-bool isMoving()
-{
+// Arrêt des moteurs
+void stopMotors() {
+  speed_steps_per_sec = 0;
+}
+
+bool isMoving() {
   return speed_steps_per_sec != 0;
 }
 
-// Capteur à ultrasons
-const int obstacleCheckInterval = 100; // ms between distance checks
-unsigned long lastObstacleCheckTime = 0;
-const int obstacleThresholdCM = 7; // stop if closer than 7 cm
-
-long readDistanceCM(uint8_t trigPin, uint8_t echoPin)
-{
-  digitalWrite(trigPin, LOW);
-  delayMicroseconds(2);
-  digitalWrite(trigPin, HIGH);
-  delayMicroseconds(10);
-  digitalWrite(trigPin, LOW);
-  long duration = pulseIn(echoPin, HIGH, 20000); // timeout 20 ms
-  long distance = duration * 0.034 / 2;
-  return distance;
-}
-
-// Vérification de la distance du sol pour éviter les chutes
-void checkFall()
-{
-  // On a fini notre chemin si on arrive en bout de table (détection de distance du sol)
-  if (digitalRead(FALL_PIN) == HIGH && movementInProgress)
-  {
-    stopMotors();
-    movementInProgress = false;
-    scenarioInProgress = false;
-    Serial.println("Bout de table détecté : Arrêt");
+// ========================= Détection obstacles =========================
+void detectObstacles() {
+  if (distanceCM > 0 && distanceCM < obstacleThresholdCM) {
+    if (isMoving()) {
+      lastSpeed = speed_steps_per_sec;
+      speed_steps_per_sec = 0;
+      Serial.println("Obstacle détecté : Arrêt");
+      Serial.println(distanceCM);
+    }
+  } else if (!isMoving() && movementInProgress) {
+    Serial.println("Obstacle évité : reprise mouvement");
+    Serial.println(distanceCM);
+    speed_steps_per_sec = lastSpeed;
   }
 }
 
-bool recherchePlace = false;
-int rechercheStep = 0;
-// Nombre de pas restants à faire après la reprise de place
-int stepsRemainingPlace = 0;
-
-// Sous-scénario pour la recherche de place
-const int NB_RECHERCHE_STEPS = 3;
-Step rechercheScenario[NB_RECHERCHE_STEPS] = {
-    {STEP_FORWARD, 20},
-    {STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90}};
-
-void detectObstacles()
-{
-  unsigned long now = millis();
-  if (now - lastObstacleCheckTime >= obstacleCheckInterval)
-  {
-    lastObstacleCheckTime = now;
-#ifndef SIMULATOR
-    // On vérifie si on est en train de tomber seulement dans la phase finale
-    if (PAMI_NUM == 1 && currentScenarioStep > 1)
-    {
-      checkFall();
-    }
-#endif
-    if (lastDirection == RIGHT || lastDirection == LEFT)
-    {
-      return;
-    }
-    long distance = readDistanceCM(TRIG_PIN, ECHO_PIN);
-    if (distance > 0 && distance < obstacleThresholdCM)
-    {
-      if (isMoving())
-      {
-        lastSpeed = speed_steps_per_sec;
-        stopMotors();
-        Serial.println("Obstacle détecté : Arrêt");
-        // Si on est dans STEP_FORWARD_UNTIL_END, on lance la recherche de place
-        if (scenarioInProgress && currentScenarioStep > 0 && scenario[currentScenarioStep - 1].type == STEP_FORWARD_UNTIL_END && !recherchePlace)
-        {
-          recherchePlace = true;
-          rechercheStep = 0;
-          stepsRemainingPlace = steps_target - steps_done; // On garde en mémoire les pas restants
-          Serial.println("Début recherche de place...");
-          // On tourne pour éviter le robot adverse
-          if (PAMI_SIDE == 1)
-          {
-            rotateAsync(90, 1000, true); // Tourner à droite si côté bleu
-          }
-          else
-          {
-            rotateAsync(90, 1000, false); // Tourner à gauche si côté jaune
-          }
-        }
-        // Si les PAMIs du bas arrivent sur leur phase finale et détecte un robot, alors on s'arrête et se met en position finale
-        if (PAMI_NUM > 1 && currentScenarioStep == 2)
-        {
-          recherchePlace = true;
-          Serial.println("Début recherche de place...");
-        }
-      }
-    }
-    else
-    {
-      if (!isMoving() && movementInProgress)
-      {
-        switch (lastDirection)
-        {
-        case FORWARD:
-          moveAsyncSteps(steps_target - steps_done, lastSpeed, true);
-          break;
-        case BACKWARD:
-          moveAsyncSteps(steps_target - steps_done, lastSpeed, false);
-          break;
-        default:
-          break;
-        }
-        Serial.println("Plus d'obstacle : Reprise du mouvement");
-      }
-    }
-  }
+// ========================= Scenario =========================
+void moveAsyncSteps(int32_t steps, int32_t speed, bool forwardDir) {
+  steps_target = steps;
+  steps_done = 0;
+  digitalWrite(M1_DIR_PIN, forwardDir ? HIGH : LOW);
+  digitalWrite(M2_DIR_PIN, forwardDir ? HIGH : LOW);
+  speed_steps_per_sec = speed;
+  movementInProgress = true;
+  lastDirection = forwardDir ? FORWARD : BACKWARD;
 }
 
-// On gère le scénario du robot, retourne 0 si le scénario est terminé, 1 si en cours
-int processScenario()
-{
-  if (!scenarioInProgress)
-    return 0;
-  if (movementInProgress)
-    return 1;
+void rotateAsync(float angleDeg, int32_t speed, bool toRight) {
+  steps_target = getRotationSteps(angleDeg + 10.0);
+  steps_done = 0;
+  digitalWrite(M1_DIR_PIN, toRight ? HIGH : LOW);
+  digitalWrite(M2_DIR_PIN, toRight ? LOW : HIGH);
+  speed_steps_per_sec = speed;
+  movementInProgress = true;
+  lastDirection = toRight ? RIGHT : LEFT;
+}
 
-  // Si on est en phase de recherche de place
-  if (recherchePlace)
-  {
-    // Les Pamis du bas
-    if (PAMI_NUM > 1)
-    {
-      currentScenarioStep++;
-      recherchePlace = false;
-    }
-    // PAMI superstar du haut
-    else if (rechercheStep < NB_RECHERCHE_STEPS)
-    {
-      Step &step = rechercheScenario[rechercheStep];
-      switch (step.type)
-      {
-      case STEP_FORWARD:
-        moveAsyncSteps(getStepsForDistance(step.value), step.speed, true);
-        break;
-      case STEP_ROTATE:
-        if (step.value >= 0)
-          rotateAsync(step.value, 1000, true);
-        else
-          rotateAsync(-step.value, 1000, false);
-        break;
-      default:
-        break;
-      }
-      rechercheStep++;
-    }
-    else
-    {
-      // Après le sous-scénario, on tente d'avancer à nouveau
-      moveAsyncSteps(stepsRemainingPlace, 2500, true);
-      recherchePlace = false;
-      rechercheStep = 0;
-    }
-    return 1;
-  }
+int processScenario() {
+  if (!scenarioInProgress) return 0;
+  if (movementInProgress) return 1;
 
-  if (currentScenarioStep >= scenarioLength)
-  {
+  if (currentScenarioStep >= scenarioLength) {
     Serial.println("Scénario terminé !");
     scenarioInProgress = false;
     return 0;
   }
 
   Step &step = scenario[currentScenarioStep];
-  switch (step.type)
-  {
-  case STEP_FORWARD:
-    moveAsyncSteps(getStepsForDistance(step.value), step.speed, true);
-    break;
-  case STEP_BACKWARD:
-    moveAsyncSteps(getStepsForDistance(step.value), step.speed, false);
-    break;
-  case STEP_ROTATE:
-    if (step.value >= 0)
-      rotateAsync(step.value, step.speed, true);
-    else
-      rotateAsync(-step.value, step.speed, false);
-    break;
-  case STEP_FORWARD_UNTIL_END:
-    moveAsyncSteps(getStepsForDistance(step.value), step.speed, true);
-    break;
+  switch (step.type) {
+    case STEP_FORWARD: moveAsyncSteps(getStepsForDistance(step.value), step.speed, true);
+      break;
+    case STEP_BACKWARD: moveAsyncSteps(getStepsForDistance(step.value), step.speed, false);
+      break;
+    case STEP_ROTATE:
+      if (step.value >= 0) rotateAsync(step.value, step.speed, true);
+      else rotateAsync(-step.value, step.speed, false);
+      break;
   }
   currentScenarioStep++;
-
   return 1;
 }
 
-bool tirettePose = false;
-void loop()
-{
-
-  if (digitalRead(TIRETTE_PIN) == LOW && !tirettePose)
-  {
-    // Tirette posée
+// ========================= Loop principal =========================
+void loop() {
+  // Tirette
+  if (digitalRead(TIRETTE_PIN) == LOW && !tirettePose) {
     tirettePose = true;
-  }
+    Serial.println("Trigger initialized");
+  } else if (digitalRead(TIRETTE_PIN) == HIGH && tirettePose) {
+    Serial.println("Trigger removed");
+    Serial.print("Switch : ");
+    Serial.println(readSwitchOnce(SWITCH_PIN) ? "OFF" : "ON");
+    delay(START_DELAY);
+    Serial.println("Starting the script");
+    startTime = millis();
+    scenarioInProgress = true;
 
-  else if (digitalRead(TIRETTE_PIN) == HIGH && tirettePose)
-  {
-    // Tirette activée
-    uint32_t startDelay = START_DELAY + (PAMI_NUM - 1) * 2000;
-    delay(startDelay);
-    uint32_t startTime = millis();
+    while (true) {
+      updateSteppers();
+      detectObstacles();
+      processScenario();
 
-    while (true)
-    {
-      updateSteppers(); // Mise à jour des moteurs asynchrone
-
-      detectObstacles(); // Vérification des obstacles
-
-      // Gestion du scénario puis arrêt si terminé ou si temps dépassé
-      if (processScenario() == 0 || millis() - startTime >= 100000 - startDelay)
-      {
-        Serial.println("Scénario terminé, arrêt des moteurs.");
+      // Fin de scénario ou timeout
+      if (!scenarioInProgress || millis() - startTime >= 100000) {
         stopMotors();
-        uint32_t duty;
+        Serial.println("Script finished, motor stopped");
 
-        while (true)
-        {
-
-          if (!digitalRead(EMG_PIN))
-          {
-            duty = (uint32_t)((pow(2, 16) - 1) * 0.05);
-            ledcWrite(0, duty);
+        // Servo en boucle jusqu'à reset
+        Serial.println("Starting the post game action");
+        while (true) {
+          if (!digitalRead(EMG_PIN)) {
+            servo_eat.writeAngle(SERVO_EAT_UP);
             delay(500);
-            duty = (uint32_t)((pow(2, 16) - 1) * 0.1);
-            ledcWrite(0, duty);
+            servo_eat.writeAngle(SERVO_EAT_DOWN);
             delay(500);
-          }
-          else
-          {
+          } else {
+            Serial.println("Restarting the ESP 32 ...");
             ESP.restart();
-            break;
           }
         }
-      };
+      }
     }
   }
 }

src/utils.cpp

@@ -2,35 +2,38 @@
 #include "utils.h"
 #include "main.h"
 
-void enableDrivers()
-{
+void enableMotorDrivers() {
     digitalWrite(M1_ENABLE_PIN, LOW); // LOW pour activer le driver
     digitalWrite(M2_ENABLE_PIN, LOW);
 }
 
-void disableDrivers()
-{
+void disableDrivers() {
     digitalWrite(M1_ENABLE_PIN, HIGH);
     digitalWrite(M2_ENABLE_PIN, HIGH);
 }
 
-int getStepsForDistance(float cm)
-{
-    double wheel_circumference_cm = M_PI * (double)WHEEL_DIAMETER;
-    double steps_per_cm = ((double)STEPS_PER_REV * (double)MICROSTEPPING) / wheel_circumference_cm;
-    int total_steps = (int)round(cm * steps_per_cm);
-    int correction = (int)round(1.5 * steps_per_cm);
+int getStepsForDistance(float cm) {
+    double wheel_circumference_cm = M_PI * (double) WHEEL_DIAMETER;
+    double steps_per_cm = ((double) STEPS_PER_REV * (double) MICROSTEPPING) / wheel_circumference_cm;
+    int total_steps = (int) round(cm * steps_per_cm);
+    int correction = (int) round(1.5 * steps_per_cm);
 
     return total_steps - correction;
     // return cm * 740; // Valeur calculée à la main pour une vitesse de 5000 :)
 }
 
-int getRotationSteps(float angleDeg)
-{
-
-    double wheelTurnPerRotation = (double)WHEEL_BASE / (double)WHEEL_DIAMETER;
-    double microStepsPerRotation = wheelTurnPerRotation * (double)STEPS_PER_REV * (double)MICROSTEPPING;
+int getRotationSteps(float angleDeg) {
+    double wheelTurnPerRotation = (double) WHEEL_BASE / (double) WHEEL_DIAMETER;
+    double microStepsPerRotation = wheelTurnPerRotation * (double) STEPS_PER_REV * (double) MICROSTEPPING;
     double microStepsForAngle = microStepsPerRotation * (angleDeg / 360.0);
 
-    return (int)round(microStepsForAngle);
-}
+    return (int) round(microStepsForAngle);
+}
+
+int readSwitchOnce(int pin) {
+    pinMode(SWITCH_PIN, INPUT_PULLUP);
+    delay(10);
+    const bool switchState = digitalRead(SWITCH_PIN);
+    pinMode(SWITCH_PIN, INPUT); // retire le pull-up
+    return switchState;
+}