From a289f4cb314bb786a023a787a60617dad9976b1e Mon Sep 17 00:00:00 2001 From: dd060606 Date: Wed, 7 May 2025 11:50:19 +0200 Subject: [PATCH] =?UTF-8?q?Reprise=20apr=C3=A8s=20d=C3=A9tection=20d'obsta?= =?UTF-8?q?cles=20dynamique?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/main.h | 38 +++++++++++++++++ src/main.cpp | 110 +++++++++++++++++++++++++------------------------ 2 files changed, 95 insertions(+), 53 deletions(-) create mode 100644 include/main.h diff --git a/include/main.h b/include/main.h new file mode 100644 index 0000000..13718c0 --- /dev/null +++ b/include/main.h @@ -0,0 +1,38 @@ +#ifndef MAIN_H +#define MAIN_H + +enum Direction +{ + FORWARD, + BACKWARD, + LEFT, + RIGHT, + STOP +}; + +// UART +#define TX_PIN 17 +#define RX_PIN 16 + +// Moteur 1 - Gauche +#define M1_DIR_PIN 4 +#define M1_STEP_PIN 33 +#define M1_ENABLE_PIN 14 + +// Moteur 2 - Droite +#define M2_DIR_PIN 16 +#define M2_STEP_PIN 32 +#define M2_ENABLE_PIN 27 + +// Paramètres moteurs +#define R_SENSE 0.11f +#define CURRENT 600 // mA +// Adresses des drivers TMC2209 +#define DRIVER1_ADDR 0b00 +#define DRIVER2_ADDR 0b01 + +// Capteur à ultrasons +#define TRIG_PIN 18 +#define ECHO_PIN 5 + +#endif // MAIN_H \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index ef1c3ac..63d4075 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,31 +1,10 @@ #include #include - -// UART -#define TX_PIN 17 -#define RX_PIN 16 - -// Moteur 1 - Gauche -#define M1_DIR_PIN 4 -#define M1_STEP_PIN 33 -#define M1_ENABLE_PIN 14 - -// Moteur 2 - Droite -#define M2_DIR_PIN 16 -#define M2_STEP_PIN 32 -#define M2_ENABLE_PIN 27 - -// Paramètres moteurs -#define R_SENSE 0.11f -#define CURRENT 600 // mA +#include "main.h" // UART (bus unique) HardwareSerial TMCSerial(1); -// Adresses des drivers TMC2209 -#define DRIVER1_ADDR 0b00 -#define DRIVER2_ADDR 0b01 - TMC2209Stepper driver1(&TMCSerial, R_SENSE, DRIVER1_ADDR); TMC2209Stepper driver2(&TMCSerial, R_SENSE, DRIVER2_ADDR); @@ -34,6 +13,10 @@ volatile int32_t speed2_steps_per_sec = 0; uint32_t last_step_time1 = 0; uint32_t last_step_time2 = 0; +// Pour le redémarrage des moteurs après capteur obstacle +Direction lastDirection = STOP; +volatile int32_t lastSpeed = 0; + void enableDrivers() { digitalWrite(M1_ENABLE_PIN, LOW); // LOW pour activer le driver @@ -81,6 +64,7 @@ void forward(int32_t speed) { digitalWrite(M1_DIR_PIN, HIGH); digitalWrite(M2_DIR_PIN, HIGH); + lastDirection = FORWARD; speed1_steps_per_sec = speed; speed2_steps_per_sec = speed; } @@ -90,6 +74,7 @@ void backward(int32_t speed) { digitalWrite(M1_DIR_PIN, LOW); digitalWrite(M2_DIR_PIN, LOW); + lastDirection = BACKWARD; speed1_steps_per_sec = speed; speed2_steps_per_sec = speed; } @@ -99,6 +84,7 @@ void turnLeft(int32_t speed) { digitalWrite(M1_DIR_PIN, LOW); digitalWrite(M2_DIR_PIN, HIGH); + lastDirection = LEFT; speed1_steps_per_sec = speed; speed2_steps_per_sec = speed; } @@ -108,6 +94,7 @@ void turnRight(int32_t speed) { digitalWrite(M1_DIR_PIN, HIGH); digitalWrite(M2_DIR_PIN, LOW); + lastDirection = RIGHT; speed1_steps_per_sec = speed; speed2_steps_per_sec = speed; } @@ -130,9 +117,6 @@ const int obstacleCheckInterval = 100; // ms between distance checks unsigned long lastObstacleCheckTime = 0; const int obstacleThresholdCM = 20; // stop if closer than 20 cm -#define TRIG_PIN 18 -#define ECHO_PIN 5 - long readDistanceCM() { digitalWrite(TRIG_PIN, LOW); @@ -145,6 +129,52 @@ long readDistanceCM() return distance; } +void detectObstacles() +{ + // On vérifie les obstacles + unsigned long now = millis(); + if (now - lastObstacleCheckTime >= obstacleCheckInterval) + { + lastObstacleCheckTime = now; + + long distance = readDistanceCM(); + + if (distance > 0 && distance < obstacleThresholdCM) + { + if (isMoving()) + { + lastSpeed = speed1_steps_per_sec; + stopMotors(); + Serial.println("Obstacle detected: Stopped"); + } + } + else + { + if (!isMoving()) + { + switch (lastDirection) + { + case FORWARD: + forward(lastSpeed); + break; + case BACKWARD: + backward(lastSpeed); + break; + case LEFT: + turnLeft(lastSpeed); + break; + case RIGHT: + turnRight(lastSpeed); + break; + default: + break; + } + Serial.println("Path clear: Resuming movement"); + } + } + } +} + void setup() { Serial.begin(115200); @@ -176,7 +206,7 @@ void setup() driver2.en_spreadCycle(false); stopMotors(); - forward(200); // Move forward at 200 steps/sec + turnRight(200); // Move forward at 200 steps/sec Serial.println("Setup complete"); } @@ -184,31 +214,5 @@ void loop() { updateSteppers(); // Mise à jour des moteurs asynchrone - // On vérifie les obstacles - unsigned long now = millis(); - if (now - lastObstacleCheckTime >= obstacleCheckInterval) - { - lastObstacleCheckTime = now; - - long distance = readDistanceCM(); - - if (distance > 0 && distance < obstacleThresholdCM) - { - if (isMoving()) - { - stopMotors(); - Serial.println("Obstacle detected: Stopped"); - } - } - else - { - if (!isMoving()) - { - forward(200); - Serial.println("Path clear: Moving forward"); - } - } - } - - // Later: integrate sensors and controller commands here + detectObstacles(); // Vérification des obstacles }