Examples¶
Small programs to start from, in the examples/ folder of the repository. They go step by step from connecting to plotting. Set PORT = "sim" to try them without hardware.
01_connect_and_scan.py¶
"""
01 - Connect and find all motors.
Set PORT = "sim" to try this without hardware.
"""
import servolab
PORT = None # None = find the Waveshare adapter automatically, or "sim"
# Which serial ports does this computer have?
print(servolab.list_ports())
# Open the bus and scan every ID (0...252) whether a motor is there.
bus = servolab.connect(PORT)
print(bus)
for motor_id, motor in bus.motors.items():
print(f"ID {motor_id}: {motor.model}")
02_read_values.py¶
"""
02 - Read values from one motor.
Turn the motor by hand while this runs: torque is off, so it turns freely.
"""
import time
import servolab
PORT = None # or "sim"
bus = servolab.connect(PORT)
m = bus[1] # the motor with ID 1 (an ID, not a list index!)
print(m.info) # overview: model, firmware, mode, ...
m.torque_enabled = False # motor is free, you can turn it by hand
for _ in range(50):
print(f"position {m.position:+.2f} rad "
f"velocity {m.velocity:+.2f} rad/s "
f"current {m.current:.3f} A "
f"temperature {m.temperature} degC")
time.sleep(0.1)
03_move_position.py¶
"""
03 - Position mode: tell the motor where to go.
"""
import math
import servolab
PORT = None # or "sim"
bus = servolab.connect(PORT)
m = bus[1] # the motor with ID 1
m.mode = "position"
m.torque_enabled = True # now the motor holds its position
# The simple way: set the goal, the motor moves as fast as it can.
m.position = 0.0
# With limits, and wait until the motor has arrived:
m.move_to(math.pi / 2, speed=1.0, wait=True) # 90 degrees at 1 rad/s
print("arrived at", m.position)
m.move_to(-math.pi / 2, speed=3.0, acceleration=10.0, wait=True)
print("arrived at", m.position)
m.torque_enabled = False
04_feedback_loop.py¶
"""
04 - Read all motors in a loop at a fixed rate, and plot the result.
read_feedback() reads every motor with ONE message on the bus, which is
much faster than asking each motor separately.
"""
import matplotlib.pyplot as plt
import numpy as np
import servolab
PORT = None # or "sim"
bus = servolab.connect(PORT)
bus.torque_off_all() # turn the motors by hand while recording
rate = servolab.Rate(100) # 100 Hz
times = []
positions = []
for _ in range(300): # 300 passes at 100 Hz = 3 seconds
fb = bus.read_feedback()
times.append(fb.time)
positions.append(fb.position)
rate.sleep()
positions = np.array(positions) # rows = time steps, columns = motors (see fb.ids)
for column, motor_id in enumerate(fb.ids):
plt.plot(times, positions[:, column], label=f"motor {motor_id}")
plt.xlabel("time [s]")
plt.ylabel("position [rad]")
plt.legend()
plt.show()
05_pwm_mode.py¶
"""
05 - PWM mode: you decide how much voltage the motor gets.
In PWM mode there is no controller inside the motor. m.pwm = 0.3 means
"30 % of the supply voltage". This is the starting point for writing your
own controller (for example a PID controller).
Careful: the motor does not stop by itself. Keep your hand off the shaft.
"""
import time
import servolab
PORT = None # or "sim"
bus = servolab.connect(PORT)
m = bus[1] # the motor with ID 1
m.mode = "pwm"
m.torque_enabled = True
m.pwm = 0.2 # slowly one way
time.sleep(1.0)
print("velocity at 20 % PWM:", m.velocity, "rad/s")
m.pwm = -0.4 # faster the other way
time.sleep(1.0)
print("velocity at -40 % PWM:", m.velocity, "rad/s")
m.pwm = 0.0
m.torque_enabled = False
m.mode = "position" # back to the normal mode
06_registers.py¶
"""
06 - Registers: the memory table inside the motor.
Everything the properties (m.position, m.pid, ...) do is reading and writing
registers. You can also do that directly, by name. All names, addresses and
explanations are in src/servolab/registers.py.
"""
import servolab
from servolab.registers import STS3215_REGISTERS
PORT = None # or "sim"
bus = servolab.connect(PORT)
m = bus[1] # the motor with ID 1
# Read every register and print it with its explanation
for register in STS3215_REGISTERS:
value = m.read_register(register.name)
print(f"{register.address:3d} {register.name:30s} = {value:6d} {register.unit:14s} {register.description}")
# Raw values: present_position is in steps (4096 per turn, 2048 = middle)
print("raw position:", m.read_register("present_position"), "steps")
# Writing: change the dead zone of the internal controller.
# Without save=True the change is lost after power-off.
m.write_register("cw_dead_zone", 2)
m.write_register("ccw_dead_zone", 2)
07_plotting.py¶
"""
07 - Record values in a loop and plot them.
Motor 1 follows a slow sine wave for 10 seconds. Its position and the goal are
recorded and plotted afterwards. With LIVE = True you also watch them live
while the loop runs; with LIVE = False they are only recorded.
"""
import math
import servolab
PORT = None # or "sim"
LIVE = True # True = open a live plot window (Rerun viewer)
bus = servolab.connect(PORT)
m = bus[1] # the motor with ID 1
m.mode = "position"
m.torque_enabled = True
if LIVE:
rec = servolab.LivePlot() # records AND shows live
else:
rec = servolab.Recorder() # only records
rate = servolab.Rate(100) # 100 Hz
for step in range(1000): # 1000 passes at 100 Hz = 10 seconds
goal = math.sin(step / 1000 * 2 * math.pi) # one full sine period, -1 ... 1 rad
m.move_to(goal, speed=2.0, acceleration=10.0)
fb = bus.read_feedback()
rec.add(time=fb.time, position=fb.of(1)["position"], goal=goal)
rate.sleep()
m.torque_enabled = False
rec.plot() # position and goal over time
rec.save("07_recording.csv") # open it in Excel, or load it with numpy
08_imu.py¶
"""
08 - Read an IMU (gyro + accelerometer) on the bus.
Needs an IMU on the bus (the XIAO IMU has ID 100 when it is new).
Tilt and turn it while this runs. Then try LIVE = True to see the values
as plots.
"""
import numpy as np
import servolab
PORT = None # the IMU's own USB port works too, e.g. "/dev/cu.usbmodem..."
IMU_ID = 100
LIVE = False # True = live plot window (Rerun viewer)
bus = servolab.connect(PORT)
imu = bus[IMU_ID] # the IMU with ID 100 (an ID, not a list index!)
print(imu.info) # model, firmware, ranges, sample rate
rec = servolab.LivePlot() if LIVE else None
rate = servolab.Rate(50) # 50 times per second
for _ in range(500): # 10 seconds
fb = imu.read() # everything from the same moment, ONE message
a_x, a_y, a_z = fb.acceleration # m/s^2, includes gravity
w_x, w_y, w_z = fb.angular_velocity # rad/s
if rec is not None:
rec.add(time=fb.time, a_x=a_x, a_y=a_y, a_z=a_z, w_x=w_x, w_y=w_y, w_z=w_z)
else:
print(f"acc {a_x:+6.2f} {a_y:+6.2f} {a_z:+6.2f} m/s^2 (|a| = {np.linalg.norm(fb.acceleration):5.2f}) "
f"gyro {w_x:+6.2f} {w_y:+6.2f} {w_z:+6.2f} rad/s")
rate.sleep()
# Motors and IMUs in one go: bus.read_feedback() also reads all sensors.
fb = bus.read_feedback()
print("motors:", fb.ids, " IMU acceleration:", fb.sensors[IMU_ID].acceleration)