position_control_absolute.py
Position servoing using encoder counter reading to stop the motor once the target is reached.
Reads absolute position via I2C (assumed register 0x20) and applies proportional correction. The code handles direction inversion and power saturation. It is crucial to check the encoder resolution (PPR) to calibrate the gain constant.
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)