closed_loop_speed_pid.ino
Bucle de control PID en C++ para mantener una velocidad objetivo utilizando la retroalimentación del codificador vía I2C.
Requiere una biblioteca I2C (Wire.h). El código asume registros de lectura de codificadores de 32 bits. Ajuste las ganancias PID y la conversión Ticks/RPM según los PPR de sus motores (3-15 PPR).
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é
}