from motorbridge import Controller, Mode

with Controller("can0") as ctrl:
    motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")
    ctrl.enable_all()
    motor.ensure_mode(Mode.MIT, 1000)

    # Send position command
    motor.send_mit(pos=0.5, vel=0.0, kp=20.0, kd=1.0, tau=0.0)

    # Read state
    print(motor.get_state())

from motorbridge import Controller, Mode, MotorState
from motorbridge.errors import CallError, MotorBridgeError

# SocketCAN (most common)
ctrl = Controller("can0")

# CAN-FD (for Hexfellow)
# ctrl = Controller.from_socketcanfd("can0")

# DM Serial bridge (for Damiao)
# ctrl = Controller.from_dm_serial("/dev/ttyACM0", 921600)

# Damiao motor
motor = ctrl.add_damiao_motor(
    motor_id=0x01,      # Command CAN ID
    feedback_id=0x11,   # Feedback CAN ID
    model="4340P"       # Motor model string
)

# Enable motor output (apply power)
ctrl.enable_all()

# Switch to MIT control mode with 1-second timeout
motor.ensure_mode(Mode.MIT, timeout_ms=1000)

# MIT mode: full control over position, velocity, stiffness, damping, torque
motor.send_mit(
    pos=1.0,    # Target position (radians)
    vel=0.0,    # Target velocity (rad/s)
    kp=30.0,    # Position stiffness
    kd=1.0,     # Velocity damping
    tau=0.0     # Feedforward torque (Nm)
)

# Position-velocity mode: simpler interface
# motor.send_pos_vel(pos=1.0, vlim=2.0)

# Velocity mode: speed control only
# motor.send_vel(vel=1.5)

# Force position mode: compliant position control
# motor.send_force_pos(pos=1.0, vlim=2.0, ratio=0.3)

import time

# Request fresh feedback
motor.request_feedback()

# Read cached state (may be None if no feedback yet)
state = motor.get_state()

if state:
    print(f"Position: {state.pos:.3f} rad")
    print(f"Velocity: {state.vel:.3f} rad/s")
    print(f"Torque: {state.torq:.3f} Nm")
    print(f"MOS Temperature: {state.t_mos:.1f} °C")
    print(f"Rotor Temperature: {state.t_rotor:.1f} °C")
else:
    print("No feedback received yet")

# Disable motor
motor.disable()

# Close resources (automatic with context manager)
motor.close()
ctrl.close()

import time
from motorbridge import Controller, Mode

# Context manager ensures proper cleanup
with Controller("can0") as ctrl:
    motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")

    # Enable and set mode
    ctrl.enable_all()
    motor.ensure_mode(Mode.POS_VEL, timeout_ms=1000)

    # Control loop
    dt_s = 0.02  # 20ms period = 50Hz
    target_pos = 1.0  # radians

    for i in range(100):
        t0 = time.time()

        # Send command
        motor.send_pos_vel(pos=target_pos, vlim=1.0)

        # Read state
        state = motor.get_state()
        if state:
            print(f"#{i:03d} pos={state.pos:+.3f} vel={state.vel:+.3f}")

        # Maintain loop timing
        elapsed = time.time() - t0
        if elapsed < dt_s:
            time.sleep(dt_s - elapsed)

from motorbridge import Controller, Mode

with Controller("can0") as ctrl:
    # Add multiple motors
    motor1 = ctrl.add_damiao_motor(0x01, 0x11, "4340P")
    motor2 = ctrl.add_damiao_motor(0x02, 0x12, "4340P")

    # Enable all at once
    ctrl.enable_all()

    # Set modes individually
    motor1.ensure_mode(Mode.MIT, 1000)
    motor2.ensure_mode(Mode.MIT, 1000)

    # Control each motor
    motor1.send_mit(0.5, 0.0, 20.0, 1.0, 0.0)
    motor2.send_mit(-0.5, 0.0, 20.0, 1.0, 0.0)

from motorbridge import Controller, Mode
from motorbridge.errors import CallError, MotorBridgeError

try:
    with Controller("can0") as ctrl:
        motor = ctrl.add_damiao_motor(0x01, 0x11, "4340P")
        ctrl.enable_all()
        motor.ensure_mode(Mode.MIT, 1000)

        for i in range(100):
            try:
                motor.send_mit(0.5, 0.0, 20.0, 1.0, 0.0)
            except CallError as e:
                print(f"Command failed: {e}")
                # Optionally continue or break

except MotorBridgeError as e:
    print(f"Motor bridge error: {e}")
except Exception as e:
    print(f"Unexpected error: {e}")

# Scan for motors
motorbridge-cli scan --vendor all --channel can0 --start-id 1 --end-id 32

# Run motor in MIT mode
motorbridge-cli run \
    --vendor damiao \
    --channel can0 \
    --model 4340P \
    --motor-id 0x01 \
    --feedback-id 0x11 \
    --mode mit \
    --loop 100 \
    --dt-ms 20 \
    --pos 1.0 \
    --kp 30.0 \
    --kd 1.0

# Read Damiao registers
motorbridge-cli id-dump \
    --vendor damiao \
    --channel can0 \
    --motor-id 0x01 \
    --feedback-id 0x11 \
    --rids 7,8,9,10,21,22,23