modelec/pami
1
0
Fork 0
mirror of https://github.com/modelec/pami.git synced 2026-03-18 21:40:35 +01:00
Code Issues Packages Projects Releases Wiki Activity

Reprise après détection d'obstacles dynamique

Browse Source
This commit is contained in:
dd060606
2025-05-07 11:50:19 +02:00
parent a9c78c271c
commit a289f4cb31
2 changed files with 95 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
include/main.h Normal file
View File

@@ -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

110
src/main.cpp
View File

@@ -1,31 +1,10 @@
#include <Arduino.h>
#include <TMCStepper.h>
// 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
}