import time
from motorbridge import (
Controller, Mode,
RID_MST_ID, RID_ESC_ID, RID_TIMEOUT,
RID_PMAX, RID_VMAX, RID_TMAX,
RID_KP_ASR, RID_KI_ASR,
DAMIAO_RW_REGISTERS, get_damiao_register_spec
)
def configure_motor(motor, config):
"""Apply configuration to motor."""
# Set limits
motor.write_register_f32(RID_PMAX, config["pmax"])
motor.write_register_f32(RID_VMAX, config["vmax"])
motor.write_register_f32(RID_TMAX, config["tmax"])
# Set gains
motor.write_register_f32(RID_KP_ASR, config["kp_asr"])
motor.write_register_f32(RID_KI_ASR, config["ki_asr"])
# Set timeout
motor.write_register_u32(RID_TIMEOUT, config["timeout_ms"])
# Save
motor.store_parameters()
print("Configuration applied and saved")
def verify_config(motor, config):
"""Verify configuration matches expected values."""
checks = [
(RID_PMAX, config["pmax"], "f32"),
(RID_VMAX, config["vmax"], "f32"),
(RID_TMAX, config["tmax"], "f32"),
(RID_KP_ASR, config["kp_asr"], "f32"),
(RID_KI_ASR, config["ki_asr"], "f32"),
]
all_ok = True
for rid, expected, dtype in checks:
if dtype == "f32":
actual = motor.get_register_f32(rid, 1000)
ok = abs(actual - expected) < 0.001
else:
actual = motor.get_register_u32(rid, 1000)
ok = actual == expected
spec = get_damiao_register_spec(rid)
name = spec.variable if spec else f"RID{rid}"
status = "OK" if ok else "MISMATCH"
print(f" {name}: {actual} ({status})")
all_ok = all_ok and ok
return all_ok
# Configuration
MOTOR_CONFIG = {
"pmax": 12.566, # ±4π rad
"vmax": 30.0, # 30 rad/s
"tmax": 10.0, # 10 Nm
"kp_asr": 0.5,
"ki_asr": 0.01,
"timeout_ms": 5000,
}
with Controller("can0") as ctrl:
motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")
print("Current configuration:")
verify_config(motor, MOTOR_CONFIG)
print("\nApplying new configuration...")
configure_motor(motor, MOTOR_CONFIG)
print("\nVerifying new configuration:")
if verify_config(motor, MOTOR_CONFIG):
print("Configuration verified successfully!")
else:
print("Configuration verification FAILED!")