ajout système d'étapes

include/main.h

@@ -10,6 +10,19 @@ enum Direction
     STOP
 };
 
+enum StepType
+{
+    STEP_FORWARD,
+    STEP_ROTATE,
+    STEP_FORWARD_UNTIL_FALL
+};
+
+struct Step
+{
+    StepType type;
+    float value; // cm pour STEP_FORWARD, deg pour ROTATE, ignoré pour UNTIL_FALL
+};
+
 // UART
 #define TX_PIN 17
 #define RX_PIN 16

include/utils.h

@@ -0,0 +1,9 @@
+#ifndef UTILS_H
+#define UTILS_H
+
+void enableDrivers();
+void disableDrivers();
+
+int getStepsForDistance(float cm);
+
+#endif // UTILS_H

src/main.cpp

@@ -4,6 +4,7 @@
 #define SIMULATOR // Commenter cette ligne pour utiliser le matériel réel
 // Définitions des constantes dans main.h
 #include "main.h"
+#include "utils.h"
 
 // UART (bus unique)
 HardwareSerial TMCSerial(1);
@@ -20,29 +21,16 @@ volatile int32_t lastSpeed = 0;
 
 volatile int32_t steps_target = 0;
 volatile int32_t steps_done = 0;
-
 bool movementInProgress = false;
 
-void enableDrivers()
-{
-  digitalWrite(M1_ENABLE_PIN, LOW); // LOW pour activer le driver
-  digitalWrite(M2_ENABLE_PIN, LOW);
-}
-
-void disableDrivers()
-{
-  digitalWrite(M1_ENABLE_PIN, HIGH);
-  digitalWrite(M2_ENABLE_PIN, HIGH);
-}
-
-int getStepsForDistance(float cm)
-{
-  /*
-  float circumference = 3.1416 * WHEEL_DIAMETER;
-  float steps_per_cm = (STEPS_PER_REV * 16) / circumference;
-  */
-  return cm * 740; // Valeur calculée à la main pour une vitesse de 5000 :)
-}
+// Chaque étape du scénario
+Step scenario[] = {
+    {STEP_FORWARD, 20},
+    {STEP_ROTATE, 90},
+    {STEP_FORWARD_UNTIL_FALL, 0}};
+const int scenarioLength = sizeof(scenario) / sizeof(Step);
+int currentScenarioStep = 0;
+bool scenarioInProgress = false;
 
 // Arrêt des moteurs
 void stopMotors()
@@ -50,6 +38,51 @@ void stopMotors()
   speed_steps_per_sec = 0;
 }
 
+// Fonction d'initialisation
+void setup()
+{
+  Serial.begin(115200);
+  TMCSerial.begin(115200, SERIAL_8N1, RX_PIN, TX_PIN);
+
+  pinMode(M1_STEP_PIN, OUTPUT);
+  pinMode(M1_DIR_PIN, OUTPUT);
+  pinMode(M1_ENABLE_PIN, OUTPUT);
+  pinMode(M2_STEP_PIN, OUTPUT);
+  pinMode(M2_DIR_PIN, OUTPUT);
+  pinMode(M2_ENABLE_PIN, OUTPUT);
+  // Capteur à ultrasons
+  pinMode(TRIG_PIN, OUTPUT);
+  pinMode(ECHO_PIN, INPUT);
+  // Capteur de chute
+  pinMode(FALL_TRIG_PIN, OUTPUT);
+  pinMode(FALL_ECHO_PIN, INPUT);
+
+  digitalWrite(M1_ENABLE_PIN, LOW);
+  digitalWrite(M2_ENABLE_PIN, LOW);
+
+  driver1.begin();
+  driver2.begin();
+
+  driver1.rms_current(CURRENT);
+  driver2.rms_current(CURRENT);
+
+  driver1.microsteps(16);
+  driver2.microsteps(16);
+
+  driver1.en_spreadCycle(false);
+  driver2.en_spreadCycle(false);
+
+  driver1.pwm_autoscale(true);
+  driver2.pwm_autoscale(true);
+
+  stopMotors();
+
+  scenarioInProgress = true;
+  currentScenarioStep = 0;
+
+  Serial.println("Setup complete");
+}
+
 void moveAsyncSteps(int32_t steps, int32_t speed, bool forwardDir)
 {
   // Le nombre de pas à faire
@@ -115,7 +148,7 @@ void updateSteppers()
   {
     stopMotors();
     movementInProgress = false;
-    Serial.println("Mouvement terminé");
+    Serial.println("Etape terminé");
   }
 }
 
@@ -206,46 +239,36 @@ void detectObstacles()
   }
 }
 
-void setup()
+// On gère le scénario du robot
+void processScenario()
 {
-  Serial.begin(115200);
-  TMCSerial.begin(115200, SERIAL_8N1, RX_PIN, TX_PIN);
+  if (!scenarioInProgress)
+    return;
+  if (movementInProgress)
+    return; // On attend la fin du mouvement
 
-  pinMode(M1_STEP_PIN, OUTPUT);
-  pinMode(M1_DIR_PIN, OUTPUT);
-  pinMode(M1_ENABLE_PIN, OUTPUT);
-  pinMode(M2_STEP_PIN, OUTPUT);
-  pinMode(M2_DIR_PIN, OUTPUT);
-  pinMode(M2_ENABLE_PIN, OUTPUT);
-  // Capteur à ultrasons
-  pinMode(TRIG_PIN, OUTPUT);
-  pinMode(ECHO_PIN, INPUT);
-  // Capteur de chute
-  pinMode(FALL_TRIG_PIN, OUTPUT);
-  pinMode(FALL_ECHO_PIN, INPUT);
+  if (currentScenarioStep >= scenarioLength)
+  {
+    Serial.println("Scénario terminé !");
+    scenarioInProgress = false;
+    return;
+  }
 
-  digitalWrite(M1_ENABLE_PIN, LOW);
-  digitalWrite(M2_ENABLE_PIN, LOW);
-
-  driver1.begin();
-  driver2.begin();
-
-  driver1.rms_current(CURRENT);
-  driver2.rms_current(CURRENT);
-
-  driver1.microsteps(16);
-  driver2.microsteps(16);
-
-  driver1.en_spreadCycle(false);
-  driver2.en_spreadCycle(false);
-
-  driver1.pwm_autoscale(true);
-  driver2.pwm_autoscale(true);
-
-  stopMotors();
-  moveAsyncSteps(getStepsForDistance(20), 5000, false);
-  // rotateAsync(90, 1000, true);
-  Serial.println("Setup complete");
+  Step &step = scenario[currentScenarioStep];
+  switch (step.type)
+  {
+  case STEP_FORWARD:
+    moveAsyncSteps(getStepsForDistance(step.value), 5000, true);
+    break;
+  case STEP_ROTATE:
+    rotateAsync(step.value, 1000, true);
+    break;
+  case STEP_FORWARD_UNTIL_FALL:
+    // On lance un mouvement très long, on s'arrêtera à la détection de chute
+    moveAsyncSteps(getStepsForDistance(200), 5000, true); // 200cm = "infini"
+    break;
+  }
+  currentScenarioStep++;
 }
 
 void loop()
@@ -253,4 +276,6 @@ void loop()
   updateSteppers(); // Mise à jour des moteurs asynchrone
 
   detectObstacles(); // Vérification des obstacles
+
+  processScenario(); // Gestion du scénario
 }

src/utils.cpp

@@ -0,0 +1,24 @@
+#include "main.h"
+#include "utils.h"
+#include <Arduino.h>
+
+void enableDrivers()
+{
+    digitalWrite(M1_ENABLE_PIN, LOW); // LOW pour activer le driver
+    digitalWrite(M2_ENABLE_PIN, LOW);
+}
+
+void disableDrivers()
+{
+    digitalWrite(M1_ENABLE_PIN, HIGH);
+    digitalWrite(M2_ENABLE_PIN, HIGH);
+}
+
+int getStepsForDistance(float cm)
+{
+    /*
+    float circumference = 3.1416 * WHEEL_DIAMETER;
+    float steps_per_cm = (STEPS_PER_REV * 16) / circumference;
+    */
+    return cm * 740; // Valeur calculée à la main pour une vitesse de 5000 :)
+}