add multi core, servo class

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

include/Servo.h

@@ -1,18 +1,40 @@
-#ifndef SERVO_H
+#ifndef PAMI_SERVO_H
-#define SERVO_H
+#define PAMI_SERVO_H
 
-#define MAX_US 544
+#define DEFAULT_MIN_US 544
-#define MIN_US 2400
+#define DEFAULT_MAX_US 2400
+
+#define DEFAULT_MIN_ANGLE 0
+#define DEFAULT_MAX_ANGLE 180
 
 class Servo {
 private:
-    const int channel;
+    int channel{};
-    const int pin;
+    int pin{};
-    const int min;
+    int min_us{};
-    const int max;
+    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 //SERVO_H
+#endif //PAMI_SERVO_H

include/main.h

@@ -1,5 +1,5 @@
-#ifndef MAIN_H
+#ifndef PAMI_MAIN_H
-#define MAIN_H
+#define PAMI_MAIN_H
 
 enum Direction {
     FORWARD,
@@ -46,7 +46,6 @@ struct Step {
 #define ECHO_PIN 12
 // Servo Moteur
 #define SERVO_PIN 15
-// FIXME : Je sais pas, loop sous le bouton d'arret d'urgence
 #define EMG_PIN 5
 
 #endif
@@ -90,4 +89,4 @@ struct Step {
 #define STEPS_PER_REV 200.0
 #define MICROSTEPPING 8.0
 
-#endif // MAIN_H
+#endif // PAMI_MAIN_H

include/utils.h

@@ -1,5 +1,5 @@
-#ifndef UTILS_H
+#ifndef PAMI_UTILS_H
-#define UTILS_H
+#define PAMI_UTILS_H
 
 void enableMotorDrivers();
 
@@ -9,6 +9,4 @@ int getStepsForDistance(float cm);
 
 int getRotationSteps(float angleDeg);
 
-void setServoAngle(int angle, int channel = 0);
+#endif // PAMI_UTILS_H
-
-#endif // UTILS_H

platformio.ini

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

src/Servo.cpp

@@ -1 +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,46 +1,33 @@
 #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
-// TODO : Brève description de quel pami correspond a quel numéro
-// Numéro du pami pour les différents robots car chemins différents
 #define PAMI_NUM 2
-// FIXME : Mettre au min à START_DELAY 85000 (85s)
 #define START_DELAY 5000
 
-//FIXME : Le reset des value devrait pas etre fait dans le setup ? (Si on considère que le code peut s'auto restart)
+// Stepper
-uint32_t startTime = 0;
+volatile int32_t speed_steps_per_sec = 0;
-
+volatile int32_t steps_target = 0;
-volatile int32_t speed_steps_per_sec = 0; // target speed (signed)
+volatile int32_t steps_done = 0;
-uint32_t last_step_time = 0;
+volatile bool movementInProgress = false;
-
-// Pour le redémarrage des moteurs après capteur obstacle
 Direction lastDirection = STOP;
 volatile int32_t lastSpeed = 0;
 
-volatile int32_t steps_target = 0;
+// Scénario
-volatile int32_t steps_done = 0;
-bool movementInProgress = false;
-
-// Les différents scénarios possibles
 #if PAMI_NUM == 1
-// Chaque étape du scénario écureuil ninja
-// TODO
 Step scenario[] = {
   {STEP_FORWARD, 105, 2500},
-  {STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90, 1000}, // Tourner à gauche si côté bleu, droite si jaune
+  {STEP_ROTATE, PAMI_SIDE == 1 ? -90 : 90, 1000},
   {STEP_FORWARD, 50, 2000}
 };
 #elif PAMI_NUM == 2
-// TODO : Voir comment gerer les différence de distance a parcourir selon PAMI_NUM, plein de if ?
 Step scenario[] = {
   {STEP_BACKWARD, 35, 2500},
-  // {STEP_ROTATE, PAMI_SIDE == 1 ? -45 + 10 * (PAMI_NUM - 2) : 45 - 10 * (PAMI_NUM - 2), 500},
   {STEP_BACKWARD, 80 - 10 * PAMI_NUM, 3000}
 };
 #else
@@ -55,16 +42,47 @@ const int scenarioLength = sizeof(scenario) / sizeof(Step);
 int currentScenarioStep = 0;
 bool scenarioInProgress = false;
 
-// Arrêt des moteurs
+// Ultrason (non bloquant)
-void stopMotors() {
+volatile unsigned long distanceCM = 1000;
-  speed_steps_per_sec = 0;
+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
+  }
 }
 
-// Fonction d'initialisation
+// ========================= Setup =========================
 void setup() {
   Serial.begin(115200);
-  // Bouton d'arrêt d'urgence (HIGH si appuyé)
+
-  pinMode(EMG_PIN, INPUT);
+  // Pins moteurs
   // Moteur 1 (Droite)
   pinMode(M1_STEP_PIN, OUTPUT);
   pinMode(M1_DIR_PIN, OUTPUT);
@@ -73,70 +91,47 @@ void setup() {
   pinMode(M2_STEP_PIN, OUTPUT);
   pinMode(M2_DIR_PIN, OUTPUT);
   pinMode(M2_ENABLE_PIN, OUTPUT);
-  // Tirette
+
-  pinMode(TIRETTE_PIN, INPUT_PULLUP);
+  // Capteurs
-  // Capteur à ultrasons
   pinMode(TRIG_PIN, OUTPUT);
   pinMode(ECHO_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
+  // Servo
-  pinMode(SERVO_PIN, OUTPUT);
+  servo_eat = Servo(SERVO_PIN, 1);
+  servo_eat.writeAngle(SERVO_EAT_UP);
 
-  ledcSetup(0, 50, 16);
-  ledcAttachPin(SERVO_PIN, 0);
-
-  setServoAngle(130);
 
   enableMotorDrivers();
-  stopMotors();
+  speed_steps_per_sec = 0;
 
-  scenarioInProgress = false;
+  // Créer la tâche sur Core 0
-  currentScenarioStep = 0;
+  xTaskCreatePinnedToCore(
+    ultrasonicTask,
+    "UltrasonicTask",
+    2000,
+    NULL,
+    1,
+    NULL,
+    0); // Core 0
 
   Serial.println("Setup complete");
-  Serial.println("Waiting for the trigger initialization");
 }
 
-void moveAsyncSteps(int32_t steps, int32_t speed, bool forwardDir) {
+// ========================= Steppers =========================
-  // 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() {
+  static uint32_t last_step_time1 = 0;
   uint32_t now = micros();
 
-  // Moteur 1
+  if (speed_steps_per_sec != 0 && steps_done < steps_target) {
-  if (speed_steps_per_sec != 0 && (steps_done < steps_target)) {
+    uint32_t interval = 1000000UL / abs(speed_steps_per_sec);
-    uint32_t interval1 = 1000000UL / abs(speed_steps_per_sec);
+    if (now - last_step_time1 >= interval) {
-    static uint32_t last_step_time1 = 0;
-    if (now - last_step_time1 >= interval1) {
       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;
@@ -144,86 +139,62 @@ void updateSteppers() {
     }
   }
 
-  // Comptage des pas (on considère le moteur le plus lent pour terminer l'étape)
   if (movementInProgress && steps_done >= steps_target) {
-    stopMotors();
+    speed_steps_per_sec = 0;
     movementInProgress = false;
-    Serial.println("Etape terminé");
+    Serial.println("Etape terminée");
   }
 }
 
-// Vérifie si les moteurs sont en mouvement
+// Arrêt des moteurs
+void stopMotors() {
+  speed_steps_per_sec = 0;
+}
+
 bool isMoving() {
   return speed_steps_per_sec != 0;
 }
 
-// Capteur à ultrasons
+// ========================= Détection obstacles =========================
-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;
-}
-
-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 (distanceCM > 0 && distanceCM < obstacleThresholdCM) {
-  if (now - lastObstacleCheckTime >= obstacleCheckInterval) {
+    if (isMoving()) {
-    lastObstacleCheckTime = now;
+      lastSpeed = speed_steps_per_sec;
-    // TODO : Threashold pour rotation ?
+      speed_steps_per_sec = 0;
-    if (lastDirection == RIGHT || lastDirection == LEFT) {
+      Serial.println("Obstacle détecté : Arrêt");
-      return;
+      Serial.println(distanceCM);
-    }
-    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");
-      }
-    } else {
-      if (!isMoving() && movementInProgress) {
-        Serial.println("Plus d'obstacle : Reprise du mouvement");
-        switch (lastDirection) {
-          case FORWARD:
-            moveAsyncSteps(steps_target - steps_done, lastSpeed, true);
-            break;
-          case BACKWARD:
-            moveAsyncSteps(steps_target - steps_done, lastSpeed, false);
-            break;
-          default:
-            break;
-        }
-      }
     }
+  } else if (!isMoving() && movementInProgress) {
+    Serial.println("Obstacle évité : reprise mouvement");
+    Serial.println(distanceCM);
+    speed_steps_per_sec = lastSpeed;
   }
 }
 
-// On gère le scénario du robot, retourne 0 si le scénario est terminé, 1 si en cours
+// ========================= 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;
+}
+
+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;
+}
+
 int processScenario() {
-  if (!scenarioInProgress)
+  if (!scenarioInProgress) return 0;
-    return 0;
+  if (movementInProgress) return 1;
-  if (movementInProgress)
-    return 1;
 
   if (currentScenarioStep >= scenarioLength) {
     Serial.println("Scénario terminé !");
@@ -233,65 +204,53 @@ int processScenario() {
 
   Step &step = scenario[currentScenarioStep];
   switch (step.type) {
-    case STEP_FORWARD:
+    case STEP_FORWARD: moveAsyncSteps(getStepsForDistance(step.value), step.speed, true);
-      moveAsyncSteps(getStepsForDistance(step.value), step.speed, true);
       break;
-    case STEP_BACKWARD:
+    case STEP_BACKWARD: moveAsyncSteps(getStepsForDistance(step.value), step.speed, false);
-      moveAsyncSteps(getStepsForDistance(step.value), step.speed, false);
       break;
     case STEP_ROTATE:
-      if (step.value >= 0)
+      if (step.value >= 0) rotateAsync(step.value, step.speed, true);
-        rotateAsync(step.value, step.speed, true);
+      else rotateAsync(-step.value, step.speed, false);
-      else
-        rotateAsync(-step.value, step.speed, false);
       break;
   }
   currentScenarioStep++;
-
   return 1;
 }
 
-bool tirettePose = false;
+// ========================= Loop principal =========================
-
 void loop() {
+  // Tirette
   if (digitalRead(TIRETTE_PIN) == LOW && !tirettePose) {
-    // Tirette posée
     tirettePose = true;
     Serial.println("Trigger initialized");
   } else if (digitalRead(TIRETTE_PIN) == HIGH && tirettePose) {
-    // Tirette activée
     Serial.println("Trigger removed");
-    // TODO : Mettre le bon delay selon le num
+    delay(START_DELAY);
-    uint32_t startDelay = START_DELAY + (PAMI_NUM - 1) * /*2000*/0;
-    Serial.print("Waiting : ");
-    Serial.println(startDelay);
-    delay(startDelay);
     Serial.println("Starting the script");
     startTime = millis();
-
     scenarioInProgress = true;
+
     while (true) {
-      updateSteppers(); // Mise à jour des moteurs asynchrone
+      updateSteppers();
+      detectObstacles();
+      processScenario();
 
-      // detectObstacles(); // Vérification des obstacles
+      // Fin de scénario ou timeout
-
+      if (!scenarioInProgress || millis() - startTime >= 100000) {
-      // 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.");
-        // TODO : Faire bouger le servo a la fin
         stopMotors();
+        Serial.println("Script finished, motor stopped");
 
+        // Servo en boucle jusqu'à reset
+        Serial.println("Starting the post game action");
         while (true) {
           if (!digitalRead(EMG_PIN)) {
-            Serial.println("Servo run");
+            servo_eat.writeAngle(SERVO_EAT_UP);
-            setServoAngle(45);
             delay(500);
-            setServoAngle(135);
+            servo_eat.writeAngle(SERVO_EAT_DOWN);
             delay(500);
           } else {
-            Serial.println("Restart");
+            Serial.println("Restarting the ESP 32 ...");
             ESP.restart();
-            break;
           }
         }
       }

src/utils.cpp

@@ -29,13 +29,3 @@ int getRotationSteps(float angleDeg) {
 
     return (int) round(microStepsForAngle);
 }
-
-void setServoAngle(int angle, int channel) {
-    int value = map(angle, 0, 180, 544, 2400);
-    // Serial.print("Servo angle: ");
-    // Serial.println(value);
-    if (value < 544) value = 544;
-    if (value > 2400) value = 2400;
-    int duty = value * 65535 / 20000;
-    ledcWrite(channel, duty);
-}