Pyserial and servo functions

I wanted to make a 3 DoF arm with my 64-bit BeagleY-AI and a controller for handling servo motors.

I picked the beagleboard.org BeagleY-AI and the pololu.com 18 channel Maestro.

Luckily for me, I had some servo motors leftover from another project I could use.

I am only using three servo motors for this project.

This way I can have a base, shoulder, and elbow for the arm.

The source code is basically AI generated with an emphasis on python3.

I have had to change a lot of the source code to function properly and with little bugs.

It is still a work in progress since the arm is not physically built yet and the code needs better progression directed at it.

Here you can see a simple connection from a BeagleY-AI to Maestro servo controller. It requires a USB 2.0 (type A) at least to a mini USB cable. You will also need a few jumper wires for powering the servo(s).

Here, you can see a pinout of the Maestro used in the project.

The SBC (single board computer) used can be found here: https://www.beagleboard.org/boards/beagley-ai

While the pins to be utilized on the board can be found at this neat project online: https://pinout.beagley.ai/

So far, I have no incorporated camera vision. I am also going to post a starter script for using the board and servo controller.

import sys
import math
import time
import usb.core
import usb.util

# ==========================================
# 1. HARDWARE & DEVICE CONFIGURATION
# ==========================================
VENDOR_ID = 0x1FFB
PRODUCT_ID = 0x008B  # Mini Maestro 18-Channel

device = usb.core.find(idVendor=VENDOR_ID, idProduct=PRODUCT_ID)
if device is None:
    print("Error: Maestro device not found. Check physical connections.")
    sys.exit(1)

# ==========================================
# 2. ARM GEOMETRY & SMOOTHING MATRIX
# ==========================================
L1 = 100.0  # Base height (mm)
L2 = 120.0  # Upper arm length (mm)
L3 = 150.0  # Forearm length (mm)

# Matrix containing mapping data and on-chip kinematics configuration
# Speed: 1 = 0.025 μs/ms. Try values between 10 and 50 for gentle movement.
# Acceleration: 1 = 0.0003125 μs/ms². Try values between 1 and 10.
SERVO_MATRIX = {
    "base":     {"ch": 0, "min_deg": -90, "max_deg": 90, "min_us": 1000, "max_us": 2000, "speed": 15, "accel": 4},
    "shoulder": {"ch": 1, "min_deg": 0,   "max_deg": 180, "min_us": 900,  "max_us": 2100, "speed": 12, "accel": 3},
    "elbow":    {"ch": 2, "min_deg": -135,"max_deg": 135,"min_us": 1000, "max_us": 2000, "speed": 20, "accel": 5}
}

# ==========================================
# 3. MAESTRO HARDWARE INTERFACES
# ==========================================
def set_hardware_limits(dev, channel, speed, accel):
    """Configures on-chip speed and acceleration profile curves."""
    # Speed command: 0x87, Channel, Low Bits, High Bits
    speed_low = speed & 0x7F
    speed_high = (speed >> 7) & 0x7F
    dev.write(1, [0x87, channel, speed_low, speed_high])
    
    # Acceleration command: 0x89, Channel, Low Bits, High Bits
    accel_low = accel & 0x7F
    accel_high = (accel >> 7) & 0x7F
    dev.write(1, [0x89, channel, accel_low, accel_high])

def send_maestro_command(dev, channel, target_us):
    """Sends immediate position target. Microcontroller manages profile execution."""
    target_val = int(target_us * 4)
    low_bits = target_val & 0x7F
    high_bits = (target_val >> 7) & 0x7F
    try:
        dev.write(1, [0x84, channel, low_bits, high_bits])
    except usb.core.USBError as e:
        print(f"USB Error: {e}")

# ==========================================
# 4. KINEMATICS ENGINE
# ==========================================
def calculate_ik(x, y, z):
    """Calculates target positions for (X,Y,Z) Cartesian points."""
    try:
        theta_0 = math.atan2(y, x)
        r = math.sqrt(x**2 + y**2)
        z_rel = z - L1
        D = math.sqrt(r**2 + z_rel**2)
        
        if D > (L2 + L3) or D < abs(L2 - L3):
            return None
            
        cos_theta_2 = (D**2 - L2**2 - L3**2) / (2 * L2 * L3)
        theta_2 = math.acos(cos_theta_2)  
        
        alpha = math.atan2(z_rel, r)
        cos_beta = (L2**2 + D**2 - L3**2) / (2 * L2 * D)
        beta = math.acos(cos_beta)
        theta_1 = alpha + beta
        
        return {
            "base": math.degrees(theta_0),
            "shoulder": math.degrees(theta_1),
            "elbow": math.degrees(theta_2)
        }
    except (ValueError, ZeroDivisionError):
        return None

def scale_angle_to_us(angle_deg, config):
    constrained = max(config["min_deg"], min(config["max_deg"], angle_deg))
    deg_range = config["max_deg"] - config["min_deg"]
    us_range = config["max_us"] - config["min_us"]
    return int(config["min_us"] + ((constrained - config["min_deg"]) / deg_range) * us_range)

# ==========================================
# 5. EXECUTION PIPELINE
# ==========================================
WAYPOINTS = [
    (150.0, -50.0, 120.0),
    (150.0,  50.0, 120.0),
    (120.0,  50.0, 160.0),
    (120.0, -50.0, 160.0),
]

try:
    print("\nInitializing hardware profiles inside Maestro memory...")
    for joint, config in SERVO_MATRIX.items():
        set_hardware_limits(device, config["ch"], config["speed"], config["accel"])
        print(f"  [{joint.capitalize()}] Channel {config['ch']}: Speed limit = {config['speed']}, Accel limit = {config['accel']}")
    
    print("\nHardware smoothing active. Processing loop running...")
    
    while True:
        for target_pt in WAYPOINTS:
            angles = calculate_ik(*target_pt)
            if angles is None:
                continue
                
            print(f"Sending hardware command packet to: {target_pt}")
            
            # Fire target coordinates directly. The Maestro handles acceleration profiles.
            for joint, config in SERVO_MATRIX.items():
                target_pulse = scale_angle_to_us(angles[joint], config)
                send_maestro_command(device, config["ch"], target_pulse)
                
            # Allow time for the physical arm to complete its hardware-ramped trajectory
            time.sleep(2.5)
            
except KeyboardInterrupt:
    print("\nShutting down automated loop safely.")
finally:
    # Clear active limits on exit to ensure standard script resets
    for joint, config in SERVO_MATRIX.items():
        try:
            set_hardware_limits(device, config["ch"], 0, 0)
        except:
            pass
    usb.util.dispose_resources(device)

This code was AI generated for pyusb.

If you take on the task, remember to alter this AI generated code at you own discretion and do not hold others in contempt for AI generation code supply.

For me, I used pyserial instead of pyusb to get this AI generated code in working order.

If you would like to learn more about why using AI is okay or why this code works or does not, please contact me or test at you own volition.

Seth