From f9401174c4e33d819deeb27bc459dc7f782ff7f5 Mon Sep 17 00:00:00 2001
From: dd060606 <dd06.developpement@gmail.com>
Date: Wed, 28 May 2025 16:56:01 +0200
Subject: [PATCH] =?UTF-8?q?centim=C3=A8tres=20vers=20pas=20pour=20rotation?=
 =?UTF-8?q?=20et=20mouvement=20rectiligne?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 include/main.h  | 15 ++++++++-------
 include/utils.h |  1 +
 src/main.cpp    | 23 +++++++++++------------
 src/utils.cpp   | 26 +++++++++++++++++++-------
 4 files changed, 39 insertions(+), 26 deletions(-)

diff --git a/include/main.h b/include/main.h
index 300158e..958d85c 100644
--- a/include/main.h
+++ b/include/main.h
@@ -31,9 +31,9 @@ struct Step
 #ifndef SIMULATOR
 
 // Moteur 1 - Gauche
-#define M1_DIR_PIN 18
-#define M1_STEP_PIN 22
-#define M1_ENABLE_PIN 32
+#define M1_DIR_PIN 19
+#define M1_STEP_PIN 23
+#define M1_ENABLE_PIN 26
 
 // Moteur 2 - Droite
 #define M2_DIR_PIN 21
@@ -81,9 +81,10 @@ struct Step
 #define DRIVER1_ADDR 0b01
 #define DRIVER2_ADDR 0b10
 
-// Constantes pour les roues
-#define WHEEL_DIAMETER 5.5     // cm
-#define STEPS_PER_REV 200 * 16 // microsteps
-#define WHEEL_BASE 7.2         // cm
+// Constantes
+#define WHEEL_DIAMETER 5.5 // cm
+#define WHEEL_BASE 7.2     // cm
+#define STEPS_PER_REV 200.0
+#define MICROSTEPPING 8.0
 
 #endif // MAIN_H
\ No newline at end of file
diff --git a/include/utils.h b/include/utils.h
index f407fb5..498b70d 100644
--- a/include/utils.h
+++ b/include/utils.h
@@ -5,5 +5,6 @@ void enableDrivers();
 void disableDrivers();
 
 int getStepsForDistance(float cm);
+int getRotationSteps(float angleDeg);
 
 #endif // UTILS_H
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 21ee26d..1b517b5 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,6 +1,6 @@
 #include <Arduino.h>
 // #define SIMULATOR // Commenter cette ligne pour utiliser le matériel réel
-//   Définitions des constantes dans main.h
+//     Définitions des constantes dans main.h
 #include "main.h"
 #include "utils.h"
 
@@ -17,8 +17,8 @@ bool movementInProgress = false;
 
 // Chaque étape du scénario
 Step scenario[] = {
-    {STEP_FORWARD, 20},
-    // {STEP_ROTATE, 90},
+    //{STEP_FORWARD, 20},
+    {STEP_ROTATE, 90},
     // {STEP_FORWARD_UNTIL_FALL, 0}
 };
 const int scenarioLength = sizeof(scenario) / sizeof(Step);
@@ -74,15 +74,9 @@ void moveAsyncSteps(int32_t steps, int32_t speed, bool forwardDir)
 
 void rotateAsync(float angleDeg, int32_t speed, bool toRight)
 {
-  // Informations sur les roues
-  const float wheelCirc = PI * WHEEL_DIAMETER;
-
-  float rotationCirc = PI * WHEEL_BASE;
-  float distPerWheel = (angleDeg / 360.0) * rotationCirc;
-  int32_t steps = (distPerWheel / wheelCirc) * STEPS_PER_REV;
 
   // Le nombre de pas à faire
-  steps_target = steps;
+  steps_target = getRotationSteps(angleDeg + 4.0); // 4.0 correspond à une correction trouvée à la main :)
   steps_done = 0;
 
   // La direction du moteur
@@ -191,6 +185,11 @@ void detectObstacles()
         lastSpeed = speed_steps_per_sec;
         stopMotors();
         Serial.println("Obstacle détecté : Arrêt");
+
+        // On recherche une place
+        if (currentScenarioStep == STEP_FORWARD_UNTIL_FALL)
+        {
+        }
       }
     }
     else
@@ -234,14 +233,14 @@ void processScenario()
   switch (step.type)
   {
   case STEP_FORWARD:
-    moveAsyncSteps(getStepsForDistance(step.value), 5000, true);
+    moveAsyncSteps(getStepsForDistance(step.value), 2500, 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"
+    moveAsyncSteps(getStepsForDistance(200), 1000, true); // 200cm = "infini"
     break;
   }
   currentScenarioStep++;
diff --git a/src/utils.cpp b/src/utils.cpp
index 74ca011..0f07877 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -1,6 +1,6 @@
-#include "main.h"
-#include "utils.h"
 #include <Arduino.h>
+#include "utils.h"
+#include "main.h"
 
 void enableDrivers()
 {
@@ -16,9 +16,21 @@ void disableDrivers()
 
 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 :)
+    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;
+    double microStepsForAngle = microStepsPerRotation * (angleDeg / 360.0);
+
+    return (int)round(microStepsForAngle);
 }
\ No newline at end of file