mirror of
https://github.com/modelec/PAMI-2025.git
synced 2026-03-18 21:40:36 +01:00
Mise à jour de la configuration des moteurs et ajout d'un capteur à ultrasons
This commit is contained in:
dd060606
32
diagram.json
32
diagram.json
@@ -26,21 +26,6 @@
|
||||
"left": -258.77,
|
||||
"attrs": { "size": "17" }
|
||||
},
|
||||
{
|
||||
"type": "wokwi-a4988",
|
||||
"id": "drv2",
|
||||
"top": 91.4,
|
||||
"left": -33.2,
|
||||
"rotate": 270,
|
||||
"attrs": {}
|
||||
},
|
||||
{
|
||||
"type": "wokwi-stepper-motor",
|
||||
"id": "stepper2",
|
||||
"top": -140.39,
|
||||
"left": -85.97,
|
||||
"attrs": { "size": "17" }
|
||||
},
|
||||
{
|
||||
"type": "wokwi-a4988",
|
||||
"id": "drv3",
|
||||
@@ -55,6 +40,13 @@
|
||||
"top": -140.39,
|
||||
"left": -431.57,
|
||||
"attrs": { "size": "17" }
|
||||
},
|
||||
{
|
||||
"type": "wokwi-hc-sr04",
|
||||
"id": "ultrasonic1",
|
||||
"top": 203.1,
|
||||
"left": -71.3,
|
||||
"attrs": {}
|
||||
}
|
||||
],
|
||||
"connections": [
|
||||
@@ -64,10 +56,6 @@
|
||||
["drv1:1A", "stepper1:B+", "green", ["v0"]],
|
||||
["drv1:2A", "stepper1:A+", "green", ["v0"]],
|
||||
["drv1:2B", "stepper1:A-", "green", ["v0"]],
|
||||
["drv2:1B", "stepper2:B-", "green", ["v0"]],
|
||||
["drv2:1A", "stepper2:B+", "green", ["v0"]],
|
||||
["drv2:2A", "stepper2:A+", "green", ["v0"]],
|
||||
["drv2:2B", "stepper2:A-", "green", ["v0"]],
|
||||
["drv3:1B", "stepper3:B-", "green", ["v0"]],
|
||||
["drv3:1A", "stepper3:B+", "green", ["v0"]],
|
||||
["drv3:2A", "stepper3:A+", "green", ["v0"]],
|
||||
@@ -75,15 +63,13 @@
|
||||
["drv3:STEP", "esp:33", "gold", ["v28.8", "h86.4"]],
|
||||
["drv1:STEP", "esp:32", "gold", ["v28.8", "h-38.4"]],
|
||||
["drv1:SLEEP", "drv1:RESET", "green", ["v9.6", "h-9.6", "v9.6"]],
|
||||
["drv2:RESET", "drv2:SLEEP", "green", ["v9.6", "h9.6"]],
|
||||
["drv3:RESET", "drv3:SLEEP", "green", ["v9.6", "h9.6"]],
|
||||
["esp:14", "drv3:ENABLE", "green", ["v-19.05", "h-124.8"]],
|
||||
["esp:27", "drv1:ENABLE", "green", ["v0"]],
|
||||
["esp:26", "drv2:ENABLE", "green", ["v-28.65", "h144"]],
|
||||
["esp:16", "drv1:DIR", "blue", ["v9.6", "h144"]],
|
||||
["esp:4", "drv3:DIR", "blue", ["v19.2", "h-57.6", "v-201.6"]],
|
||||
["drv2:DIR", "esp:2", "blue", ["v211.2", "h-288"]],
|
||||
["drv2:STEP", "esp:13", "gold", ["v76.8", "h-105.6"]]
|
||||
["ultrasonic1:TRIG", "esp:18", "violet", ["v76.8", "h-221.2"]],
|
||||
["ultrasonic1:ECHO", "esp:5", "violet", ["v86.4", "h-240.8"]]
|
||||
],
|
||||
"dependencies": {}
|
||||
}
|
||||
|
||||
328
src/main.cpp
328
src/main.cpp
@@ -1,200 +1,214 @@
|
||||
#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 800
|
||||
#define CURRENT 600 // mA
|
||||
|
||||
// UART
|
||||
#define TX_PIN1 17
|
||||
#define RX_PIN1 16
|
||||
// UART (bus unique)
|
||||
HardwareSerial TMCSerial(1);
|
||||
|
||||
// Moteur 1 - Avant gauche
|
||||
#define DIR_PIN1 4
|
||||
#define STEP_PIN1 33
|
||||
// Adresses des drivers TMC2209
|
||||
#define DRIVER1_ADDR 0b00
|
||||
#define DRIVER2_ADDR 0b01
|
||||
|
||||
// Moteur 2 - Avant droite
|
||||
#define DIR_PIN2 16
|
||||
#define STEP_PIN2 32
|
||||
TMC2209Stepper driver1(&TMCSerial, R_SENSE, DRIVER1_ADDR);
|
||||
TMC2209Stepper driver2(&TMCSerial, R_SENSE, DRIVER2_ADDR);
|
||||
|
||||
// Moteur 3 - Arrière
|
||||
#define DIR_PIN3 2
|
||||
#define STEP_PIN3 13
|
||||
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;
|
||||
|
||||
TMC2209Stepper driver1(&Serial1, R_SENSE, 0b00);
|
||||
TMC2209Stepper driver2(&Serial1, R_SENSE, 0b01);
|
||||
TMC2209Stepper driver3(&Serial1, R_SENSE, 0b10);
|
||||
|
||||
// Moteur state
|
||||
enum MovementState
|
||||
void enableDrivers()
|
||||
{
|
||||
STOP,
|
||||
FORWARD,
|
||||
BACKWARD,
|
||||
LEFT,
|
||||
RIGHT
|
||||
};
|
||||
|
||||
MovementState currentState = STOP;
|
||||
|
||||
unsigned long lastStepTime = 0;
|
||||
const int stepDelay = 800; // microsecondes entre les pas
|
||||
int stepsRemaining = 0;
|
||||
|
||||
struct Motor
|
||||
{
|
||||
int stepPin;
|
||||
int dirPin;
|
||||
bool dir;
|
||||
bool enabled;
|
||||
Motor(int s, int d, bool direction, bool en = false)
|
||||
: stepPin(s), dirPin(d), dir(direction), enabled(en) {}
|
||||
};
|
||||
|
||||
Motor motors[3] = {
|
||||
Motor(STEP_PIN1, DIR_PIN1, true),
|
||||
Motor(STEP_PIN2, DIR_PIN2, true),
|
||||
Motor(STEP_PIN3, DIR_PIN3, true),
|
||||
};
|
||||
|
||||
void setupMotors()
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
pinMode(motors[i].stepPin, OUTPUT);
|
||||
pinMode(motors[i].dirPin, OUTPUT);
|
||||
}
|
||||
|
||||
Serial1.begin(115200, SERIAL_8N1, RX_PIN1, TX_PIN1);
|
||||
|
||||
driver1.begin();
|
||||
driver1.rms_current(CURRENT);
|
||||
driver1.microsteps(16);
|
||||
driver1.en_spreadCycle(false);
|
||||
driver1.pdn_disable(true);
|
||||
driver2.begin();
|
||||
driver2.rms_current(CURRENT);
|
||||
driver2.microsteps(16);
|
||||
driver2.en_spreadCycle(false);
|
||||
driver2.pdn_disable(true);
|
||||
driver3.begin();
|
||||
driver3.rms_current(CURRENT);
|
||||
driver3.microsteps(16);
|
||||
driver3.en_spreadCycle(false);
|
||||
driver3.pdn_disable(true);
|
||||
digitalWrite(M1_ENABLE_PIN, LOW); // LOW pour activer le driver
|
||||
digitalWrite(M2_ENABLE_PIN, LOW);
|
||||
}
|
||||
|
||||
void applyDirection()
|
||||
void disableDrivers()
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
digitalWrite(M1_ENABLE_PIN, HIGH);
|
||||
digitalWrite(M2_ENABLE_PIN, HIGH);
|
||||
}
|
||||
|
||||
// Non-blocking stepper update function
|
||||
void updateSteppers()
|
||||
{
|
||||
uint32_t now = micros();
|
||||
// Moteur 1
|
||||
if (speed1_steps_per_sec != 0)
|
||||
{
|
||||
digitalWrite(motors[i].dirPin, motors[i].dir);
|
||||
uint32_t interval = 1000000UL / abs(speed1_steps_per_sec);
|
||||
if (now - last_step_time1 >= 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(M2_STEP_PIN, LOW);
|
||||
last_step_time2 = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void setMovement(MovementState state, int steps = 400)
|
||||
// En avant
|
||||
void forward(int32_t speed)
|
||||
{
|
||||
currentState = state;
|
||||
stepsRemaining = steps;
|
||||
|
||||
switch (state)
|
||||
{
|
||||
case FORWARD:
|
||||
motors[0].dir = true;
|
||||
motors[1].dir = true;
|
||||
motors[2].dir = true;
|
||||
motors[0].enabled = motors[1].enabled = motors[2].enabled = true;
|
||||
break;
|
||||
case BACKWARD:
|
||||
motors[0].dir = false;
|
||||
motors[1].dir = false;
|
||||
motors[2].dir = false;
|
||||
motors[0].enabled = motors[1].enabled = motors[2].enabled = true;
|
||||
break;
|
||||
case LEFT:
|
||||
motors[0].dir = false;
|
||||
motors[1].dir = true;
|
||||
motors[2].enabled = false;
|
||||
motors[0].enabled = motors[1].enabled = true;
|
||||
break;
|
||||
case RIGHT:
|
||||
motors[0].dir = true;
|
||||
motors[1].dir = false;
|
||||
motors[2].enabled = false;
|
||||
motors[0].enabled = motors[1].enabled = true;
|
||||
break;
|
||||
case STOP:
|
||||
default:
|
||||
motors[0].enabled = motors[1].enabled = motors[2].enabled = false;
|
||||
break;
|
||||
}
|
||||
|
||||
applyDirection();
|
||||
digitalWrite(M1_DIR_PIN, HIGH);
|
||||
digitalWrite(M2_DIR_PIN, HIGH);
|
||||
speed1_steps_per_sec = speed;
|
||||
speed2_steps_per_sec = speed;
|
||||
}
|
||||
|
||||
void updateMotors()
|
||||
// En arrière
|
||||
void backward(int32_t speed)
|
||||
{
|
||||
static bool stepState = false;
|
||||
digitalWrite(M1_DIR_PIN, LOW);
|
||||
digitalWrite(M2_DIR_PIN, LOW);
|
||||
speed1_steps_per_sec = speed;
|
||||
speed2_steps_per_sec = speed;
|
||||
}
|
||||
|
||||
if (stepsRemaining <= 0)
|
||||
{
|
||||
setMovement(STOP);
|
||||
return;
|
||||
}
|
||||
// Rotation à gauche
|
||||
void turnLeft(int32_t speed)
|
||||
{
|
||||
digitalWrite(M1_DIR_PIN, LOW);
|
||||
digitalWrite(M2_DIR_PIN, HIGH);
|
||||
speed1_steps_per_sec = speed;
|
||||
speed2_steps_per_sec = speed;
|
||||
}
|
||||
|
||||
unsigned long now = micros();
|
||||
if (now - lastStepTime >= stepDelay)
|
||||
{
|
||||
stepState = !stepState;
|
||||
lastStepTime = now;
|
||||
// Rotation à droite
|
||||
void turnRight(int32_t speed)
|
||||
{
|
||||
digitalWrite(M1_DIR_PIN, HIGH);
|
||||
digitalWrite(M2_DIR_PIN, LOW);
|
||||
speed1_steps_per_sec = speed;
|
||||
speed2_steps_per_sec = speed;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
if (motors[i].enabled)
|
||||
{
|
||||
digitalWrite(motors[i].stepPin, stepState ? HIGH : LOW);
|
||||
}
|
||||
}
|
||||
// Arrêt des moteurs
|
||||
void stopMotors()
|
||||
{
|
||||
speed1_steps_per_sec = 0;
|
||||
speed2_steps_per_sec = 0;
|
||||
}
|
||||
|
||||
if (!stepState)
|
||||
stepsRemaining--; // Compte les pas complets
|
||||
}
|
||||
// Vérifie si les moteurs sont en mouvement
|
||||
bool isMoving()
|
||||
{
|
||||
return (speed1_steps_per_sec != 0 || speed2_steps_per_sec != 0);
|
||||
}
|
||||
|
||||
// Capteur à ultrasons
|
||||
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);
|
||||
delayMicroseconds(2);
|
||||
digitalWrite(TRIG_PIN, HIGH);
|
||||
delayMicroseconds(10);
|
||||
digitalWrite(TRIG_PIN, LOW);
|
||||
long duration = pulseIn(ECHO_PIN, HIGH, 20000); // timeout 20 ms
|
||||
long distance = duration * 0.034 / 2;
|
||||
return distance;
|
||||
}
|
||||
|
||||
void setup()
|
||||
{
|
||||
Serial.begin(115200);
|
||||
setupMotors();
|
||||
Serial.println("Moteurs prêts.");
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
stopMotors();
|
||||
forward(200); // Move forward at 200 steps/sec
|
||||
Serial.println("Setup complete");
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
updateMotors();
|
||||
updateSteppers(); // Mise à jour des moteurs asynchrone
|
||||
|
||||
// Exemple : déclencher des mouvements toutes les 4 secondes
|
||||
static unsigned long lastAction = 0;
|
||||
static int actionIndex = 0;
|
||||
// On vérifie les obstacles
|
||||
unsigned long now = millis();
|
||||
if (now - lastObstacleCheckTime >= obstacleCheckInterval)
|
||||
{
|
||||
lastObstacleCheckTime = now;
|
||||
|
||||
if (now - lastAction > 4000 && currentState == STOP)
|
||||
long distance = readDistanceCM();
|
||||
|
||||
if (distance > 0 && distance < obstacleThresholdCM)
|
||||
{
|
||||
switch (actionIndex)
|
||||
if (isMoving())
|
||||
{
|
||||
case 0:
|
||||
setMovement(FORWARD);
|
||||
break;
|
||||
case 1:
|
||||
setMovement(BACKWARD);
|
||||
break;
|
||||
case 2:
|
||||
setMovement(LEFT);
|
||||
break;
|
||||
case 3:
|
||||
setMovement(RIGHT);
|
||||
break;
|
||||
stopMotors();
|
||||
Serial.println("Obstacle detected: Stopped");
|
||||
}
|
||||
actionIndex = (actionIndex + 1) % 4;
|
||||
lastAction = now;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!isMoving())
|
||||
{
|
||||
forward(200);
|
||||
Serial.println("Path clear: Moving forward");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Later: integrate sensors and controller commands here
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user