374 lines
11 KiB
Python
374 lines
11 KiB
Python
"""
|
|
Contrôle d'une base roulante avec ESP32-C3-DevkitM-1
|
|
- Pont en H L298N pour 2 moteurs DC
|
|
- Capteur à ultrasons pour détection d'obstacles
|
|
"""
|
|
|
|
from machine import Pin, PWM, time_pulse_us
|
|
import time
|
|
import math
|
|
|
|
# ============================================================================
|
|
# Configuration des pins GPIO
|
|
# ============================================================================
|
|
|
|
# Capteur à ultrasons
|
|
TRIG_PIN = 8
|
|
ECHO_PIN = 9
|
|
|
|
# Pont en H L298N
|
|
ENA_PIN = 4 # PWM pour moteur A
|
|
IN1_PIN = 5 # Direction moteur A
|
|
IN2_PIN = 6 # Direction moteur A
|
|
IN3_PIN = 2 # Direction moteur B
|
|
IN4_PIN = 1 # Direction moteur B
|
|
ENB_PIN = 0 # PWM pour moteur B
|
|
|
|
# ============================================================================
|
|
# Paramètres
|
|
# ============================================================================
|
|
|
|
PWM_FREQ = 1000 # Fréquence PWM en Hz
|
|
MAX_PWM = 65535 # Valeur PWM maximale pour ESP32
|
|
SPEED_MAX = 65535 # Vitesse maximale (100%)
|
|
SPEED_MIN = 32767 # Vitesse minimale (50%)
|
|
|
|
# Distance de sécurité pour le capteur à ultrasons (en cm)
|
|
SAFE_DISTANCE = 20
|
|
|
|
|
|
# ============================================================================
|
|
# Classe pour le contrôle des moteurs
|
|
# ============================================================================
|
|
|
|
class MoteurDC:
|
|
"""Classe pour contrôler un moteur DC via le pont en H"""
|
|
|
|
def __init__(self, en_pin, in1_pin, in2_pin):
|
|
"""
|
|
Initialise un moteur DC
|
|
|
|
Args:
|
|
en_pin: PIN PWM pour la vitesse
|
|
in1_pin: PIN pour la direction (sens 1)
|
|
in2_pin: PIN pour la direction (sens 2)
|
|
"""
|
|
self.en = PWM(Pin(en_pin))
|
|
self.en.freq(PWM_FREQ)
|
|
self.in1 = Pin(in1_pin, Pin.OUT)
|
|
self.in2 = Pin(in2_pin, Pin.OUT)
|
|
|
|
def avant(self, vitesse=SPEED_MAX):
|
|
"""
|
|
Fait tourner le moteur vers l'avant
|
|
|
|
Args:
|
|
vitesse: Vitesse (0-65535), par défaut vitesse maximale
|
|
"""
|
|
vitesse = min(vitesse, SPEED_MAX)
|
|
vitesse = max(vitesse, 0)
|
|
self.in1.on()
|
|
self.in2.off()
|
|
self.en.duty_u16(vitesse)
|
|
|
|
def arriere(self, vitesse=SPEED_MAX):
|
|
"""
|
|
Fait tourner le moteur vers l'arrière
|
|
|
|
Args:
|
|
vitesse: Vitesse (0-65535), par défaut vitesse maximale
|
|
"""
|
|
vitesse = min(vitesse, SPEED_MAX)
|
|
vitesse = max(vitesse, 0)
|
|
self.in1.off()
|
|
self.in2.on()
|
|
self.en.duty_u16(vitesse)
|
|
|
|
def arreter(self):
|
|
"""Arrête le moteur"""
|
|
self.in1.off()
|
|
self.in2.off()
|
|
self.en.duty_u16(0)
|
|
|
|
|
|
# ============================================================================
|
|
# Classe pour le capteur à ultrasons
|
|
# ============================================================================
|
|
|
|
class CapteurUltrasons:
|
|
"""Classe pour lire la distance avec un capteur à ultrasons HC-SR04"""
|
|
|
|
def __init__(self, trig_pin, echo_pin):
|
|
"""
|
|
Initialise le capteur à ultrasons
|
|
|
|
Args:
|
|
trig_pin: PIN de déclenchement (TRIG)
|
|
echo_pin: PIN de retour (ECHO)
|
|
"""
|
|
self.trig = Pin(trig_pin, Pin.OUT)
|
|
self.echo = Pin(echo_pin, Pin.IN)
|
|
|
|
def mesurer_distance(self):
|
|
"""
|
|
Mesure la distance avec le capteur à ultrasons
|
|
|
|
Returns:
|
|
Distance en centimètres (float)
|
|
"""
|
|
# Envoyer une impulsion de 10µs sur TRIG
|
|
self.trig.off()
|
|
time.sleep_us(2)
|
|
self.trig.on()
|
|
time.sleep_us(10)
|
|
self.trig.off()
|
|
|
|
# Mesurer le temps de l'impulsion ECHO
|
|
try:
|
|
pulse_time = time_pulse_us(self.echo, 1, 30000) # Timeout: 30ms
|
|
except OSError:
|
|
# En cas de timeout, retourner une grande valeur
|
|
return 400.0
|
|
|
|
# Calculer la distance (vitesse du son ≈ 343 m/s à 20°C)
|
|
# Distance = (temps / 2) * vitesse = (pulse_time / 2) * 343 * 10^-6 * 100
|
|
distance_cm = (pulse_time / 2) * 0.01715
|
|
|
|
return distance_cm
|
|
|
|
def obstacle_detecte(self, distance_min=SAFE_DISTANCE):
|
|
"""
|
|
Détecte si un obstacle est présent
|
|
|
|
Args:
|
|
distance_min: Distance minimale en cm (par défaut SAFE_DISTANCE)
|
|
|
|
Returns:
|
|
True si un obstacle est détecté, False sinon
|
|
"""
|
|
distance = self.mesurer_distance()
|
|
return distance < distance_min
|
|
|
|
|
|
# ============================================================================
|
|
# Classe pour la base roulante
|
|
# ============================================================================
|
|
|
|
class BaseRoulante:
|
|
"""Classe pour contrôler la base roulante complète"""
|
|
|
|
def __init__(self):
|
|
"""Initialise la base roulante"""
|
|
# Initialiser les moteurs
|
|
self.moteur_a = MoteurDC(ENA_PIN, IN1_PIN, IN2_PIN)
|
|
self.moteur_b = MoteurDC(ENB_PIN, IN3_PIN, IN4_PIN)
|
|
|
|
# Initialiser le capteur à ultrasons
|
|
self.capteur = CapteurUltrasons(TRIG_PIN, ECHO_PIN)
|
|
|
|
print("Base roulante initialisée ✓")
|
|
|
|
def avancer(self, vitesse=SPEED_MAX):
|
|
"""
|
|
Fait avancer la base roulante
|
|
|
|
Args:
|
|
vitesse: Vitesse (0-65535), par défaut vitesse maximale
|
|
"""
|
|
self.moteur_a.avant(vitesse)
|
|
self.moteur_b.avant(vitesse)
|
|
|
|
def reculer(self, vitesse=SPEED_MAX):
|
|
"""
|
|
Fait reculer la base roulante
|
|
|
|
Args:
|
|
vitesse: Vitesse (0-65535), par défaut vitesse maximale
|
|
"""
|
|
self.moteur_a.arriere(vitesse)
|
|
self.moteur_b.arriere(vitesse)
|
|
|
|
def tourner_droite(self, vitesse=SPEED_MAX):
|
|
"""
|
|
Fait tourner la base roulante à droite
|
|
|
|
Args:
|
|
vitesse: Vitesse (0-65535), par défaut vitesse maximale
|
|
"""
|
|
self.moteur_a.avant(vitesse)
|
|
self.moteur_b.arriere(vitesse)
|
|
|
|
def tourner_gauche(self, vitesse=SPEED_MAX):
|
|
"""
|
|
Fait tourner la base roulante à gauche
|
|
|
|
Args:
|
|
vitesse: Vitesse (0-65535), par défaut vitesse maximale
|
|
"""
|
|
self.moteur_a.arriere(vitesse)
|
|
self.moteur_b.avant(vitesse)
|
|
|
|
def arreter(self):
|
|
"""Arrête la base roulante"""
|
|
self.moteur_a.arreter()
|
|
self.moteur_b.arreter()
|
|
|
|
def obtenir_distance(self):
|
|
"""
|
|
Obtient la distance mesurée par le capteur à ultrasons
|
|
|
|
Returns:
|
|
Distance en centimètres (float)
|
|
"""
|
|
return self.capteur.mesurer_distance()
|
|
|
|
def avancer_securise(self, vitesse=SPEED_MAX, distance_min=SAFE_DISTANCE):
|
|
"""
|
|
Fait avancer la base roulante avec détection d'obstacle
|
|
|
|
Args:
|
|
vitesse: Vitesse (0-65535)
|
|
distance_min: Distance minimale de sécurité en cm
|
|
"""
|
|
distance = self.obtenir_distance()
|
|
|
|
if distance > distance_min:
|
|
self.avancer(vitesse)
|
|
return True
|
|
else:
|
|
self.arreter()
|
|
print(f"Obstacle détecté à {distance:.1f} cm!")
|
|
return False
|
|
|
|
def comportement_autonome(self, duree=None):
|
|
"""
|
|
Fait se déplacer la base roulante de manière autonome en évitant les obstacles
|
|
|
|
Args:
|
|
duree: Durée du comportement autonome en secondes (None = infini)
|
|
"""
|
|
debut = time.time()
|
|
|
|
print("Démarrage du mode autonome...")
|
|
|
|
try:
|
|
while duree is None or (time.time() - debut) < duree:
|
|
# Vérifier les obstacles
|
|
distance = self.obtenir_distance()
|
|
|
|
if distance > SAFE_DISTANCE:
|
|
# Pas d'obstacle, avancer
|
|
self.avancer(SPEED_MAX)
|
|
print(f"Distance: {distance:.1f} cm - Avance")
|
|
else:
|
|
# Obstacle détecté, arrêter et tourner
|
|
print(f"Obstacle à {distance:.1f} cm - Demi-tour!")
|
|
self.arreter()
|
|
time.sleep(0.5)
|
|
|
|
# Faire un demi-tour (tourner à gauche)
|
|
self.tourner_gauche(SPEED_MAX)
|
|
time.sleep(1)
|
|
|
|
time.sleep(0.1) # Délai pour la lecture capteur
|
|
|
|
except KeyboardInterrupt:
|
|
print("Mode autonome interrompu")
|
|
|
|
finally:
|
|
self.arreter()
|
|
print("Base arrêtée")
|
|
|
|
|
|
# ============================================================================
|
|
# Fonction de démonstration
|
|
# ============================================================================
|
|
|
|
def demonstration():
|
|
"""Fonction de démonstration des mouvements"""
|
|
|
|
base = BaseRoulante()
|
|
|
|
# Afficher la distance initiale
|
|
distance = base.obtenir_distance()
|
|
print(f"Distance initiale: {distance:.1f} cm\n")
|
|
|
|
# Séquence de test
|
|
print("=== Test des mouvements ===\n")
|
|
|
|
# Avancer
|
|
print("Avance pendant 2 secondes...")
|
|
base.avancer(SPEED_MAX)
|
|
time.sleep(2)
|
|
base.arreter()
|
|
print("Arrêt\n")
|
|
|
|
# Reculer
|
|
print("Recule pendant 2 secondes...")
|
|
base.reculer(SPEED_MAX)
|
|
time.sleep(2)
|
|
base.arreter()
|
|
print("Arrêt\n")
|
|
|
|
# Tourner gauche
|
|
print("Tourne à gauche pendant 1 seconde...")
|
|
base.tourner_gauche(SPEED_MAX)
|
|
time.sleep(1)
|
|
base.arreter()
|
|
print("Arrêt\n")
|
|
|
|
# Tourner droite
|
|
print("Tourne à droite pendant 1 seconde...")
|
|
base.tourner_droite(SPEED_MAX)
|
|
time.sleep(1)
|
|
base.arreter()
|
|
print("Arrêt\n")
|
|
|
|
# Test avec vitesse réduite
|
|
print("Avance lentement pendant 3 secondes (50% vitesse)...")
|
|
base.avancer(SPEED_MIN)
|
|
time.sleep(3)
|
|
base.arreter()
|
|
print("Arrêt\n")
|
|
|
|
print("Démonstration terminée!")
|
|
|
|
|
|
# ============================================================================
|
|
# Point d'entrée principal
|
|
# ============================================================================
|
|
|
|
if __name__ == "__main__":
|
|
try:
|
|
# Créer l'instance de la base roulante
|
|
base = BaseRoulante()
|
|
|
|
# Décommenter la fonction à exécuter:
|
|
|
|
# Démonstration simple
|
|
# demonstration()
|
|
|
|
# Mode autonome (infini)
|
|
# base.comportement_autonome()
|
|
|
|
# Mode autonome (60 secondes)
|
|
# base.comportement_autonome(duree=60)
|
|
|
|
# Pour tester manuellement, vous pouvez faire:
|
|
# base.avancer()
|
|
# time.sleep(2)
|
|
# base.arreter()
|
|
|
|
print("Base roulante prête! Utilisez les méthodes de la classe BaseRoulante")
|
|
print("Exemples:")
|
|
print(" base.avancer()")
|
|
print(" base.reculer()")
|
|
print(" base.tourner_gauche()")
|
|
print(" base.tourner_droite()")
|
|
print(" base.arreter()")
|
|
print(" base.obtenir_distance()")
|
|
print(" base.comportement_autonome(duree=60)")
|
|
|
|
except Exception as e:
|
|
print(f"Erreur: {e}")
|