closed_loop_speed_pid.ino

Unit 4-Encoder Motor Driver (DRV8837DSGR) · File : closed_loop_speed_pid.ino · Language : C++ · Use case : Implémentation d'une régulation de vitesse en boucle fermée utilisant les données d'encodeur.

PID control loop in C++ to maintain a target speed using encoder feedback via I2C.

closed_loop_speed_pid.ino

#include <Wire.h>

// Adresse I2C
#define ADDR 0x24

// Registre de lecture des encodeurs (Exemple simplifié)
// Dans la réalité, il faut lire les registres de comptage pour chaque moteur
#define REG_ENC_M1 0x10 

// Paramètres PID
float Kp = 1.5, Ki = 0.01, Kd = 0.1;
float integral = 0, prev_error = 0;
int targetRPM = 100;

void setup() {
  Wire.begin();
  Serial.begin(115200);
}

void loop() {
  // 1. Lire la vitesse actuelle (simulation de lecture registre)
  int currentTicks = readEncoder(0); // Moteur 1
  int currentRPM = ticksToRPM(currentTicks);
  
  // 2. Calcul de l'erreur
  int error = targetRPM - currentRPM;
  
  // 3. Calcul PID
  integral += error;
  float derivative = error - prev_error;
  float output = (Kp * error) + (Ki * integral) + (Kd * derivative);
  prev_error = error;
  
  // 4. Appliquer la sortie au moteur (Clamp 0-255)
  int pwm = constrain(output, -255, 255);
  setMotorSpeed(0, pwm);
  
  delay(10);
}

int readEncoder(int motor) {
  Wire.beginTransmission(ADDR);
  Wire.write(REG_ENC_M1 + (motor * 4)); // Offset supposé
  Wire.endTransmission();
  Wire.requestFrom(ADDR, 4);
  if (Wire.available() >= 4) {
    return Wire.read() | (Wire.read() << 8) | (Wire.read() << 16) | (Wire.read() << 24);
  }
  return 0;
}

void setMotorSpeed(int motor, int speed) {
  // Logique d'écriture registre de vitesse/direction
  // À adapter selon les registres réels
}

int ticksToRPM(int ticks) {
  // Conversion basée sur PPR et temps de lecture
  return abs(ticks) / 10; // Exemple simplifié
}

← Back to the board