From d63238751e591b36d991f3ab7d2e4d3613dab2d2 Mon Sep 17 00:00:00 2001 From: dd060606 Date: Wed, 14 May 2025 19:51:03 +0200 Subject: [PATCH] =?UTF-8?q?Am=C3=A9lioration=20des=20fonctions=20asynchron?= =?UTF-8?q?es=20pour=20les=20d=C3=A9placements=20en=20fonction=20d'un=20an?= =?UTF-8?q?gle=20ou=20d'un=20nombre=20de=20pas?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main.cpp | 159 +++++++++++++++++++++++++-------------------------- 1 file changed, 79 insertions(+), 80 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index c53ba60..427da91 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -8,15 +8,18 @@ HardwareSerial TMCSerial(1); TMC2209Stepper driver1(&TMCSerial, R_SENSE, DRIVER1_ADDR); TMC2209Stepper driver2(&TMCSerial, R_SENSE, DRIVER2_ADDR); -volatile int32_t speed1_steps_per_sec = 0; // target speed (signed) -volatile int32_t speed2_steps_per_sec = 0; -uint32_t last_step_time1 = 0; -uint32_t last_step_time2 = 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 Direction lastDirection = STOP; 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 @@ -29,87 +32,87 @@ void disableDrivers() digitalWrite(M2_ENABLE_PIN, HIGH); } +// Arrêt des moteurs +void stopMotors() +{ + speed_steps_per_sec = 0; +} + +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) +{ + // Informations sur les roues + const float wheelBase = 10.0; // cm + const float wheelDiameter = 6.0; // cm + const int stepsPerRev = 200 * 16; // microsteps + const float wheelCirc = PI * wheelDiameter; + + float rotationCirc = PI * wheelBase; + float distPerWheel = (angleDeg / 360.0) * rotationCirc; + int32_t steps = (distPerWheel / wheelCirc) * stepsPerRev; + + // Le nombre de pas à faire + steps_target = steps; + 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() { uint32_t now = micros(); - // Moteur 1 - if (speed1_steps_per_sec != 0) + + // Moteurs + if (speed_steps_per_sec != 0 && (steps_done < steps_target)) { - uint32_t interval = 1000000UL / abs(speed1_steps_per_sec); - if (now - last_step_time1 >= interval) + uint32_t interval = 1000000UL / abs(speed_steps_per_sec); + if (now - last_step_time >= interval) { digitalWrite(M1_STEP_PIN, HIGH); - delayMicroseconds(2); - digitalWrite(M1_STEP_PIN, LOW); - last_step_time1 = now; - } - } - // Moteur 2 - if (speed2_steps_per_sec != 0) - { - uint32_t interval = 1000000UL / abs(speed2_steps_per_sec); - if (now - last_step_time2 >= interval) - { digitalWrite(M2_STEP_PIN, HIGH); delayMicroseconds(2); + digitalWrite(M1_STEP_PIN, LOW); digitalWrite(M2_STEP_PIN, LOW); - last_step_time2 = now; + + last_step_time = now; + steps_done++; } } -} -// En avant -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; -} - -// En arrière -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; -} - -// Rotation à gauche -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; -} - -// Rotation à droite -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; -} - -// Arrêt des moteurs -void stopMotors() -{ - speed1_steps_per_sec = 0; - speed2_steps_per_sec = 0; + // Fin du mouvement ? + if (movementInProgress && + steps_done >= steps_target) + { + stopMotors(); + movementInProgress = false; + Serial.println("Mouvement terminé"); + } } // Vérifie si les moteurs sont en mouvement bool isMoving() { - return (speed1_steps_per_sec != 0 || speed2_steps_per_sec != 0); + return speed_steps_per_sec != 0; } // Capteur à ultrasons @@ -147,35 +150,31 @@ void detectObstacles() if (distance > 0 && distance < obstacleThresholdCM) { + // Si un obstacle est détecté, on arrête le robot if (isMoving()) { - lastSpeed = speed1_steps_per_sec; + lastSpeed = speed_steps_per_sec; stopMotors(); - Serial.println("Obstacle detected: Stopped"); + Serial.println("Obstacle détecté : Arrêt"); } } else { - if (!isMoving()) + // Si le robot est arrêté et qu'il n'y a pas d'obstacle, on reprend le mouvement + if (!isMoving() && movementInProgress) { switch (lastDirection) { case FORWARD: - forward(lastSpeed); + moveAsyncSteps(steps_target - steps_done, lastSpeed, true); break; case BACKWARD: - backward(lastSpeed); - break; - case LEFT: - turnLeft(lastSpeed); - break; - case RIGHT: - turnRight(lastSpeed); + moveAsyncSteps(steps_target - steps_done, lastSpeed, false); break; default: break; } - Serial.println("Path clear: Resuming movement"); + Serial.println("Plus d'obstacle : Reprise du mouvement"); } } } @@ -212,7 +211,7 @@ void setup() driver2.en_spreadCycle(false); stopMotors(); - turnRight(200); // Move forward at 200 steps/sec + moveAsyncSteps(500, 200, false); Serial.println("Setup complete"); }