Skip to content
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
28 commits
Select commit Hold shift + click to select a range
4c8c7d9
WIP bus servo management
danic85 Jul 12, 2025
d18189c
Update config
danic85 Jul 12, 2025
60d524a
Added SC servo examples + compatibility
danic85 Jul 13, 2025
3e4dfc0
Configure initial positions
danic85 Jul 14, 2025
0be65a3
Allow other poses to be stored and accessed
danic85 Jul 14, 2025
91fac95
Added change_id script
danic85 Aug 3, 2025
69f2d7c
Calibrate servos on boot
danic85 Oct 4, 2025
9a57f8a
adjustments
danic85 Oct 4, 2025
0cf906e
Dynamic servo calibration + center on change_id
danic85 Oct 5, 2025
8d62b54
Controller support and WIP recording
danic85 Dec 13, 2025
f02d0db
Working with Pi
danic85 Dec 13, 2025
b052ed0
Adjust config
danic85 Dec 13, 2025
6841f63
Cleanup
danic85 Dec 14, 2025
c1ed384
Working input recorder
danic85 Dec 14, 2025
5e503e5
Support servo control (WIP - calibration needed)
danic85 Dec 14, 2025
008c369
Callibration + allow modules to be disabled on test environments
danic85 Dec 15, 2025
af0fdb2
Disable test
danic85 Dec 15, 2025
40040c8
Add documentation and tests
danic85 Dec 16, 2025
f645caa
Fix test
danic85 Dec 16, 2025
8a22781
Timeout controller connection after 30 seconds
danic85 Dec 16, 2025
d81c07c
Enable on robot
danic85 Dec 16, 2025
6a84d43
Fix for environment input to resolve pi issue
danic85 Dec 17, 2025
4803d58
Calibrate new leg position
danic85 Dec 17, 2025
dc4cb27
Improvements to gamepad controller modules. Temperature alert in disp…
danic85 Dec 18, 2025
d48ccd8
Fix tests
danic85 Dec 18, 2025
4a5a0ee
Fixes and improvements to loop and temperature management
danic85 Dec 21, 2025
5349956
Fix tests
danic85 Dec 21, 2025
cd4331b
Resolve PR comments
danic85 Dec 21, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Added SC servo examples + compatibility
  • Loading branch information
danic85 committed Jul 13, 2025
commit 60d524a92135493640e0921bc821b7fc73d763b0
23 changes: 20 additions & 3 deletions config/servo_waveshare.yml
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,28 @@ servos:
enabled: true
path: "modules.actuators.bus_servo.servo.Servo" # Include class name here
instances:
- name: "leg_l_hip"
- name: "leg_r_hip"
model: 'ST3215'
id: 1
range: [0, 4095]
start: 2048
- name: "leg_l_knee"
- name: "leg_r_knee"
model: 'ST3215'
id: 2
range: [0, 4095]
start: 2048
start: 2048
- name: "leg_r_ankle"
model: 'ST3215'
id: 3
range: [0, 4095]
start: 2048
- name: "neck_pan"
model: 'SC09'
id: 4
range: [0, 1023]
start: 511
- name: "neck_tilt"
model: 'SC09'
id: 5
range: [0, 1023]
start: 511
78 changes: 78 additions & 0 deletions modules/actuators/bus_servo/SCServo_examples/ping.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,78 @@
#!/usr/bin/env python
#
# ********* Ping Example *********
#
#
# Available SCServo model on this example : All models using Protocol SCS
# This example is tested with a SCServo(STS/SMS/SCS), and an URT
# Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000)
#

import os

if os.name == 'nt':
import msvcrt
def getch():
return msvcrt.getch().decode()
else:
import sys, tty, termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
def getch():
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch

from scservo_sdk import * # Uses SCServo SDK library
Comment thread
danic85 marked this conversation as resolved.
Resolved

# Default setting
SCS_ID = 1 # SCServo ID : 1
BAUDRATE = 115200 # Driver board default baudrate : 115200
DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"

protocol_end = 1 # SCServo bit end(STS/SMS=0, SCS=1)

# Initialize PortHandler instance
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
portHandler = PortHandler(DEVICENAME)

# Initialize PacketHandler instance
# Get methods and members of Protocol
packetHandler = PacketHandler(protocol_end)

# Open port
if portHandler.openPort():
print("Succeeded to open the port")
else:
print("Failed to open the port")
print("Press any key to terminate...")
getch()
quit()


# Set port baudrate
if portHandler.setBaudRate(BAUDRATE):
print("Succeeded to change the baudrate")
else:
print("Failed to change the baudrate")
print("Press any key to terminate...")
getch()
quit()

# Try to ping the SCServo
# Get SCServo model number
scs_model_number, scs_comm_result, scs_error = packetHandler.ping(portHandler, SCS_ID)
if scs_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(scs_comm_result))
elif scs_error != 0:
print("%s" % packetHandler.getRxPacketError(scs_error))
else:
print("[ID:%03d] ping Succeeded. SCServo model number : %d" % (SCS_ID, scs_model_number))

# Close port
portHandler.closePort()
138 changes: 138 additions & 0 deletions modules/actuators/bus_servo/SCServo_examples/read_write.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,138 @@
#!/usr/bin/env python
#
# ********* Gen Write Example *********
#
#
# Available SCServo model on this example : All models using Protocol SCS
# This example is tested with a SCServo(STS/SMS/SCS), and an URT
# Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000)
#

import os

if os.name == 'nt':
import msvcrt
def getch():
return msvcrt.getch().decode()

else:
import sys, tty, termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
def getch():
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch

from scservo_sdk import * # Uses SCServo SDK library
Comment thread
danic85 marked this conversation as resolved.
Resolved

# Control table address
ADDR_SCS_TORQUE_ENABLE = 40
ADDR_SCS_GOAL_ACC = 41
ADDR_SCS_GOAL_POSITION = 42
ADDR_SCS_GOAL_SPEED = 46
ADDR_SCS_PRESENT_POSITION = 56

# Default setting
SCS_ID = 1 # SCServo ID : 1
BAUDRATE = 115200 # Driver board default baudrate : 115200
DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"

SCS_MINIMUM_POSITION_VALUE = 100 # SCServo will rotate between this value
SCS_MAXIMUM_POSITION_VALUE = 4000 # and this value (note that the SCServo would not move when the position value is out of movable range. Check e-manual about the range of the SCServo you use.)
SCS_MOVING_STATUS_THRESHOLD = 20 # SCServo moving status threshold
SCS_MOVING_SPEED = 0 # SCServo moving speed
SCS_MOVING_ACC = 0 # SCServo moving acc
protocol_end = 1 # SCServo bit end(STS/SMS=0, SCS=1)

index = 0
scs_goal_position = [SCS_MINIMUM_POSITION_VALUE, SCS_MAXIMUM_POSITION_VALUE] # Goal position


# Initialize PortHandler instance
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
portHandler = PortHandler(DEVICENAME)

# Initialize PacketHandler instance
# Get methods and members of Protocol
packetHandler = PacketHandler(protocol_end)

# Open port
if portHandler.openPort():
print("Succeeded to open the port")
else:
print("Failed to open the port")
print("Press any key to terminate...")
getch()
quit()

# Set port baudrate
if portHandler.setBaudRate(BAUDRATE):
print("Succeeded to change the baudrate")
else:
print("Failed to change the baudrate")
print("Press any key to terminate...")
getch()
quit()

# Write SCServo acc
scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS_ID, ADDR_SCS_GOAL_ACC, SCS_MOVING_ACC)
if scs_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(scs_comm_result))
elif scs_error != 0:
print("%s" % packetHandler.getRxPacketError(scs_error))

# Write SCServo speed
scs_comm_result, scs_error = packetHandler.write2ByteTxRx(portHandler, SCS_ID, ADDR_SCS_GOAL_SPEED, SCS_MOVING_SPEED)
if scs_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(scs_comm_result))
elif scs_error != 0:
print("%s" % packetHandler.getRxPacketError(scs_error))

while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(0x1b):
break

# Write SCServo goal position
scs_comm_result, scs_error = packetHandler.write2ByteTxRx(portHandler, SCS_ID, ADDR_SCS_GOAL_POSITION, scs_goal_position[index])
if scs_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(scs_comm_result))
elif scs_error != 0:
print("%s" % packetHandler.getRxPacketError(scs_error))

while 1:
# Read SCServo present position
scs_present_position_speed, scs_comm_result, scs_error = packetHandler.read4ByteTxRx(portHandler, SCS_ID, ADDR_SCS_PRESENT_POSITION)
if scs_comm_result != COMM_SUCCESS:
print(packetHandler.getTxRxResult(scs_comm_result))
elif scs_error != 0:
print(packetHandler.getRxPacketError(scs_error))

scs_present_position = SCS_LOWORD(scs_present_position_speed)
scs_present_speed = SCS_HIWORD(scs_present_position_speed)
print("[ID:%03d] GoalPos:%03d PresPos:%03d PresSpd:%03d"
% (SCS_ID, scs_goal_position[index], scs_present_position, SCS_TOHOST(scs_present_speed, 15)))

if not (abs(scs_goal_position[index] - scs_present_position_speed) > SCS_MOVING_STATUS_THRESHOLD):
break


# Change goal position
if index == 0:
index = 1
else:
index = 0

scs_comm_result, scs_error = packetHandler.write1ByteTxRx(portHandler, SCS_ID, ADDR_SCS_TORQUE_ENABLE, 0)
if scs_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(scs_comm_result))
elif scs_error != 0:
print("%s" % packetHandler.getRxPacketError(scs_error))
# Close port
portHandler.closePort()
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
#!/usr/bin/env python

from .port_handler import *
from .packet_handler import *
from .group_sync_read import *
Comment thread
danic85 marked this conversation as resolved.
Resolved
from .group_sync_write import *
Comment thread
danic85 marked this conversation as resolved.
Resolved
Comment thread
danic85 marked this conversation as resolved.
Resolved
Comment thread
danic85 marked this conversation as resolved.
Resolved
Original file line number Diff line number Diff line change
@@ -0,0 +1,110 @@
#!/usr/bin/env python

from .scservo_def import *
Comment thread
danic85 marked this conversation as resolved.
Resolved

class GroupSyncRead:
def __init__(self, port, ph, start_address, data_length):
self.port = port
self.ph = ph
self.start_address = start_address
self.data_length = data_length

self.last_result = False
self.is_param_changed = False
self.param = []
self.data_dict = {}

self.clearParam()

def makeParam(self):
if not self.data_dict: # len(self.data_dict.keys()) == 0:
return

self.param = []

for scs_id in self.data_dict:
self.param.append(scs_id)

def addParam(self, scs_id):
if scs_id in self.data_dict: # scs_id already exist
return False

self.data_dict[scs_id] = [] # [0] * self.data_length

self.is_param_changed = True
return True

def removeParam(self, scs_id):
if scs_id not in self.data_dict: # NOT exist
return

del self.data_dict[scs_id]

self.is_param_changed = True

def clearParam(self):
self.data_dict.clear()

def txPacket(self):
if len(self.data_dict.keys()) == 0:
return COMM_NOT_AVAILABLE

if self.is_param_changed is True or not self.param:
self.makeParam()

return self.ph.syncReadTx(self.port, self.start_address, self.data_length, self.param,
len(self.data_dict.keys()) * 1)

def rxPacket(self):
self.last_result = False

result = COMM_RX_FAIL

if len(self.data_dict.keys()) == 0:
return COMM_NOT_AVAILABLE

for scs_id in self.data_dict:
self.data_dict[scs_id], result, _ = self.ph.readRx(self.port, scs_id, self.data_length)
if result != COMM_SUCCESS:
return result

if result == COMM_SUCCESS:
self.last_result = True

return result

def txRxPacket(self):
result = self.txPacket()
if result != COMM_SUCCESS:
return result

return self.rxPacket()

def isAvailable(self, scs_id, address, data_length):
#if self.last_result is False or scs_id not in self.data_dict:
if scs_id not in self.data_dict:
return False

if (address < self.start_address) or (self.start_address + self.data_length - data_length < address):
return False

if len(self.data_dict[scs_id])<data_length:
return False
return True

def getData(self, scs_id, address, data_length):
if not self.isAvailable(scs_id, address, data_length):
return 0

if data_length == 1:
return self.data_dict[scs_id][address - self.start_address]
elif data_length == 2:
return SCS_MAKEWORD(self.data_dict[scs_id][address - self.start_address],
self.data_dict[scs_id][address - self.start_address + 1])
elif data_length == 4:
return SCS_MAKEDWORD(SCS_MAKEWORD(self.data_dict[scs_id][address - self.start_address + 0],
self.data_dict[scs_id][address - self.start_address + 1]),
SCS_MAKEWORD(self.data_dict[scs_id][address - self.start_address + 2],
self.data_dict[scs_id][address - self.start_address + 3]))
else:
return 0
Loading