☀️ 🌙
← Retour au Hub

</> Intelligence & Code

Vue d'Ensemble du Programme

Le code ci-dessous est le cerveau de notre robot. Écrit en C++ pour un microcontrôleur de type ESP32, il gère le déplacement précis grâce à des moteurs pas-à-pas et utilise un Lidar pour la détection d'obstacles. Le robot peut être contrôlé par des commandes envoyées via le port série.

Les deux bibliothèques principales utilisées sont :

  • AccelStepper.h : Pour un contrôle avancé des moteurs pas-à-pas, gérant l'accélération et la décélération.
  • RPLidar.h : Pour communiquer avec le Lidar et interpréter les données de distance.

Le Code Complet

Voici le code source principal qui est téléversé sur le microcontrôleur XIAO ESP32-S3.


#include <AccelStepper.h>
#include <RPLidar.h>

const int LEFT_STEP_PIN = D1;
const int LEFT_DIR_PIN = D0;
const int RIGHT_STEP_PIN = D3;
const int RIGHT_DIR_PIN = D2;
const int DRIVER_ENABLE_PIN = D8;
const int LIDAR_MOTOR_PIN = D9;

const float STEPS_PER_REV = 200;
const float MICROSTEP_FACTOR = 8;
const float TOTAL_STEPS_PER_REV = STEPS_PER_REV * MICROSTEP_FACTOR;
const float WHEEL_DIAMETER_MM = 80;
const float WHEEL_CIRCUMFERENCE_MM = WHEEL_DIAMETER_MM * PI;
const float STEPS_PER_MM = TOTAL_STEPS_PER_REV / WHEEL_CIRCUMFERENCE_MM;
const float ROBOT_WIDTH_MM = 150.0;
const float MAX_SPEED_MM_PER_SEC = 300;
const float ACCELERATION_MM_PER_SEC2 = 2000;

AccelStepper leftMotor(AccelStepper::DRIVER, LEFT_STEP_PIN, LEFT_DIR_PIN);
AccelStepper rightMotor(AccelStepper::DRIVER, RIGHT_STEP_PIN, RIGHT_DIR_PIN);
RPLidar lidar;

float mmPerSecToStepsPerSec(float mm_per_sec) {
  return mm_per_sec * STEPS_PER_MM;
}

float mmPerSec2ToStepsPerSec2(float mm_per_sec2) {
  return mm_per_sec2 * STEPS_PER_MM;
}

void setup() {
  Serial.begin(115200);
  lidar.begin(Serial1);

  pinMode(DRIVER_ENABLE_PIN, OUTPUT);
  digitalWrite(DRIVER_ENABLE_PIN, LOW);

  pinMode(LIDAR_MOTOR_PIN, OUTPUT);
  analogWrite(LIDAR_MOTOR_PIN, 200);

  leftMotor.setMaxSpeed(mmPerSecToStepsPerSec(MAX_SPEED_MM_PER_SEC));
  leftMotor.setAcceleration(mmPerSec2ToStepsPerSec2(ACCELERATION_MM_PER_SEC2));

  rightMotor.setMaxSpeed(mmPerSecToStepsPerSec(MAX_SPEED_MM_PER_SEC));
  rightMotor.setAcceleration(mmPerSec2ToStepsPerSec2(ACCELERATION_MM_PER_SEC2));

  delay(1000);

  rplidar_response_device_health_t health;
  if (!IS_OK(lidar.getHealth(health)) || health.status != RPLIDAR_STATUS_OK) {
    analogWrite(LIDAR_MOTOR_PIN, 0);
    while(1);
  }
  
  lidar.startScan();
}

void moveForward(int distance_mm) {
  long steps = round(distance_mm * STEPS_PER_MM);
  leftMotor.move(steps);
  rightMotor.move(steps);
}

void turnRobot(int angle_deg) {
  float wheel_arc_mm = (angle_deg * (PI / 180.0)) * (ROBOT_WIDTH_MM / 2.0);
  long steps = round(wheel_arc_mm * STEPS_PER_MM);
  leftMotor.move(steps);
  rightMotor.move(-steps);
}

void stopMotors() {
  leftMotor.stop();
  rightMotor.stop();
}

void processLidar() {
  if (IS_OK(lidar.waitPoint())) {
    float distance = lidar.point.distance;
    float angle = lidar.point.angle;
    byte quality = lidar.point.quality;
    if (quality > 10 && distance > 0 && distance < 200) {
        stopMotors();
    }
  }
}

void processSerialCommands() {
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    command.trim();

    char cmdChar = command.charAt(0);
    int value = command.substring(1).toInt();

    if (cmdChar == 'f') {
      moveForward(value);
    } else if (cmdChar == 't') {
      turnRobot(value);
    } else if (cmdChar == 's') {
      stopMotors();
    }
  }
}

void loop() {
  processSerialCommands();
  
  if (leftMotor.isRunning() || rightMotor.isRunning()) {
      leftMotor.run();
      rightMotor.run();
  } else {
      processLidar();
  }
}

Décomposition du Code

Constantes et Configuration

Le début du code définit toutes les constantes importantes :

  • Pins de connexion : Les variables `..._PIN` lient le code aux broches physiques du microcontrôleur pour les moteurs et le Lidar.
  • Paramètres mécaniques : Des constantes comme `WHEEL_DIAMETER_MM` (diamètre de roue) et `ROBOT_WIDTH_MM` (largeur du robot) permettent de convertir des distances et angles en nombre de pas pour les moteurs.
  • Paramètres de performance : `MAX_SPEED_MM_PER_SEC` et `ACCELERATION_MM_PER_SEC2` définissent le comportement dynamique du robot.

Initialisation (fonction `setup`)

Cette fonction est exécutée une seule fois au démarrage. Elle configure le robot :

  1. Démarre les communications série pour le débogage et le Lidar.
  2. Active les drivers des moteurs (`digitalWrite(DRIVER_ENABLE_PIN, LOW)`).
  3. Lance la rotation du moteur du Lidar.
  4. Configure la vitesse et l'accélération maximales pour chaque moteur.
  5. Vérifie l'état de santé du Lidar. Si un problème est détecté, le programme s'arrête.
  6. Démarre le scan du Lidar pour commencer à recevoir des données.

Fonctions de Mouvement

Trois fonctions gèrent les déplacements de base :

  • moveForward(distance) : Fait avancer le robot en ligne droite sur une distance donnée en millimètres.
  • turnRobot(angle) : Fait pivoter le robot sur lui-même d'un certain angle en degrés. Un angle positif tourne dans un sens, un angle négatif dans l'autre.
  • stopMotors() : Arrête immédiatement les moteurs.

Gestion des Capteurs et Commandes

  • processLidar() : Cette fonction lit un point de donnée du Lidar. Si un obstacle de bonne qualité est détecté à moins de 200 mm, elle appelle `stopMotors()` pour un arrêt d'urgence simple.
  • processSerialCommands() : Lit les commandes arrivant sur le port série. Elle permet de piloter le robot manuellement.

Boucle Principale (fonction `loop`)

Le `loop` est le cœur du programme qui s'exécute en continu. Sa logique est la suivante :

  1. Il vérifie d'abord si une commande série a été reçue.
  2. Si un mouvement est en cours : il se consacre à l'exécution de ce mouvement en appelant `leftMotor.run()` et `rightMotor.run()`. Ces fonctions doivent être appelées le plus souvent possible.
  3. Si le robot est à l'arrêt : il utilise ce temps libre pour analyser les données du Lidar (`processLidar()`).

Comment Utiliser

Pour contrôler le robot, connectez-vous au microcontrôleur via le moniteur série de l'IDE Arduino (ou un autre terminal série).

  1. Assurez-vous que la vitesse de communication (baudrate) est réglée sur 115200.
  2. Envoyez des commandes suivies de la touche "Entrée".

Format des commandes : `[lettre][valeur]`

  • f100 : Fait avancer le robot de 100 mm.
  • t90 : Fait tourner le robot de 90 degrés.
  • t-45 : Fait tourner le robot de -45 degrés.
  • s : Arrête les moteurs.
← Retour au Hub