Motor Control Code

# robstride.py
import can
import struct
import time
 
# Physical limits for Robstride 00
P_MIN, P_MAX   = -12.5, 12.5   # rad
V_MIN, V_MAX   = -30.0, 30.0   # rad/s
T_MIN, T_MAX   = -18.0, 18.0   # N·m
KP_MIN, KP_MAX =   0.0, 500.0  # N·m/rad
KD_MIN, KD_MAX =   0.0,   5.0  # N·m·s/rad
 
 
def _float_to_uint(x, x_min, x_max, bits):
    """Linearly map float x ∈ [x_min, x_max] to unsigned integer [0, 2^bits - 1]."""
    x = max(x_min, min(x_max, x))
    return int((x - x_min) / (x_max - x_min) * ((1 << bits) - 1))
 
 
def _uint_to_float(raw, x_min, x_max, bits):
    """Inverse map from unsigned integer back to float."""
    return raw / ((1 << bits) - 1) * (x_max - x_min) + x_min
 
 
class Robstride00:
    """Driver for a single Robstride 00 actuator."""
 
    CMD_ENTER = 0xFC   # enter motor mode
    CMD_EXIT  = 0xFD   # exit motor mode
    CMD_ZERO  = 0xFE   # set zero position
    CMD_MIT   = 0x00   # MIT control (position/velocity/torque)
 
    def __init__(self, motor_id: int, channel: str = 'can0'):
        self.id  = motor_id
        self.bus = can.Bus(channel=channel, interface='socketcan')
 
    # ── Low-level frame helpers ──────────────────────────────────────────────
 
    def _send(self, ext_id: int, data: bytes):
        msg = can.Message(
            arbitration_id=ext_id,
            data=data,
            is_extended_id=True
        )
        self.bus.send(msg)
 
    def _recv(self, timeout: float = 0.05):
        return self.bus.recv(timeout=timeout)
 
    # ── Motor mode control ───────────────────────────────────────────────────
 
    def enter_mode(self):
        """Enable the motor driver."""
        self._send(self.id, bytes([0xFF, 0xFF, 0xFF, 0xFF,
                                   0xFF, 0xFF, 0xFF, 0xFC]))
 
    def exit_mode(self):
        """Disable the motor driver."""
        self._send(self.id, bytes([0xFF, 0xFF, 0xFF, 0xFF,
                                   0xFF, 0xFF, 0xFF, 0xFD]))
 
    def set_zero(self):
        """Zero the current position (writes to flash — use sparingly)."""
        self._send(self.id, bytes([0xFF, 0xFF, 0xFF, 0xFF,
                                   0xFF, 0xFF, 0xFF, 0xFE]))
 
    # ── MIT control frame ────────────────────────────────────────────────────
 
    def control(self,
                pos_des: float,
                vel_des: float,
                kp: float,
                kd: float,
                torque_ff: float):
        """
        Send one MIT control frame.
 
        Args:
            pos_des:   desired position (rad)
            vel_des:   desired velocity (rad/s)
            kp:        position gain (N·m/rad)
            kd:        velocity gain (N·m·s/rad)
            torque_ff: feedforward torque (N·m)
        """
        p  = _float_to_uint(pos_des,   P_MIN,  P_MAX,  16)
        v  = _float_to_uint(vel_des,   V_MIN,  V_MAX,  12)
        kp_ = _float_to_uint(kp,       KP_MIN, KP_MAX,  12)
        kd_ = _float_to_uint(kd,       KD_MIN, KD_MAX,  12)
        t  = _float_to_uint(torque_ff, T_MIN,  T_MAX,  12)
 
        data = bytes([
            (p  >> 8) & 0xFF,
             p        & 0xFF,
            (v  >> 4) & 0xFF,
            ((v & 0xF) << 4) | ((kp_ >> 8) & 0xF),
             kp_       & 0xFF,
            (kd_ >> 4) & 0xFF,
            ((kd_ & 0xF) << 4) | ((t >> 8) & 0xF),
             t         & 0xFF,
        ])
        self._send(self.id, data)
 
    # ── Read-back ────────────────────────────────────────────────────────────
 
    def read_state(self):
        """
        Parse a reply frame from the motor.
        Returns dict with pos (rad), vel (rad/s), torque (N·m), temp (°C).
        """
        msg = self._recv()
        if msg is None or len(msg.data) < 8:
            return None
        d = msg.data
        p_raw  = (d[1] << 8) | d[2]
        v_raw  = (d[3] << 4) | (d[4] >> 4)
        t_raw  = ((d[4] & 0xF) << 8) | d[5]
        temp   = d[6]
        return {
            'pos':    _uint_to_float(p_raw, P_MIN, P_MAX, 16),
            'vel':    _uint_to_float(v_raw, V_MIN, V_MAX, 12),
            'torque': _uint_to_float(t_raw, T_MIN, T_MAX, 12),
            'temp':   temp,
        }
 
    def shutdown(self):
        self.exit_mode()
        self.bus.shutdown()

from robstride import Robstride00
import time
 
motor = Robstride00(motor_id=0x01)
motor.enter_mode()
time.sleep(0.1)
 
target_rad = 1.57   # 90 degrees
 
try:
    for _ in range(1000):               # 1 second @ 1 kHz
        motor.control(
            pos_des=target_rad,
            vel_des=0.0,
            kp=50.0,   # spring stiffness
            kd=1.0,    # damping
            torque_ff=0.0
        )
        state = motor.read_state()
        if state:
            print(f"pos={state['pos']:.3f} rad  vel={state['vel']:.3f} rad/s")
        time.sleep(0.001)
finally:
    motor.shutdown()

motor.control(pos_des=0, vel_des=0, kp=0, kd=0, torque_ff=2.0)  # 2 N·m

# Gravity compensation: apply a constant upward torque
# and allow the joint to be moved freely
motor.control(pos_des=0, vel_des=0, kp=0, kd=0.5, torque_ff=3.5)

import can, time
 
bus = can.Bus(channel='can0', interface='socketcan')
 
for motor_id in range(1, 32):
    # Send enter-mode command
    bus.send(can.Message(arbitration_id=motor_id,
                         data=bytes([0xFF]*7 + [0xFC]),
                         is_extended_id=True))
    time.sleep(0.005)
    msg = bus.recv(timeout=0.01)
    if msg:
        print(f"Found motor at ID {motor_id:#x}")
 
bus.shutdown()