position_control_absolute.py
Servocontrol de posición utilizando la lectura del contador del codificador para detener el motor una vez alcanzado el objetivo.
Lee la posición absoluta a través de I2C (registro 0x20 asumido) y aplica una corrección proporcional. El código maneja la inversión de dirección y la saturación de potencia. Es crucial verificar la resolución del codificador (PPR) para calibrar la constante de ganancia.
position_control_absolute.py
import time
from m5stack import i2c
ADDR = 0x2F
bus = i2c.I2C(1)
def read_position():
# Lecture de 4 octets pour la position (32 bits)
data = bus.readfrom_mem(ADDR, 0x20, 4)
return int.from_bytes(data, 'little', signed=True)
def set_motor_power(power):
# Écriture de la puissance moteur (-100 à 100 ou PWM brut)
# Supposons un registre de direction et de vitesse
bus.writeto_mem(ADDR, 0x02, bytes([abs(power), 1 if power > 0 else 0]))
target_pos = 1000 # Cible en ticks d'encodeur
max_power = 80
min_power = 10
while True:
current_pos = read_position()
error = target_pos - current_pos
if abs(error) < 5:
set_motor_power(0)
print("Position atteinte")
break
# Contrôle proportionnel simple
power = int(error * 0.5)
if power > max_power: power = max_power
elif power < -max_power: power = -max_power
elif abs(power) < min_power: power = min_power if power > 0 else -min_power
set_motor_power(power)
time.sleep(0.01)