def __init__(self,
device: str = DEVICE_PORT_DEFAULT,
is_init_pose: bool = True):
'''机械臂初始化'''
# 创建串口对象
try:
self.uart = serial.Serial(port=device, baudrate=115200,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 创建串口舵机管理器
self.uservo = UartServoManager(self.uart, srv_num=SERVO_NUM)
# InitPose
if is_init_pose:
self.init_pose()
except serial.SerialException as e:
logging.error('该设备不存在,请重新填写UART舵机转接板的端口号')
# 角度定义
SERVO_PORT_NAME = 'COM7' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
# 数据表定义
ADDRESS_SOFTSTART = 49 # 上电缓启动地址位
SOFTSTART_OPEN = 1 # 上电缓启动-开启
SOFTSTART_CLOSE = 0 # 上电缓启动-关闭
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart, is_debug=True)
# 重置用户数据
uservo.reset_user_data(SERVO_ID)
# 舵机扫描
print("开始进行舵机扫描")
uservo.scan_servo()
servo_list = list(uservo.servos.keys())
print("舵机扫描结束, 舵机列表: {}".format(servo_list))
if SERVO_ID not in servo_list:
print("指定的SERVO_ID无效, 请修改舵机ID列表")
exit(-1)
print("重置舵机内存表: 舵机ID = {}".format(SERVO_ID))
import time
import struct
import serial
from uservo import UartServoManager
# 参数配置
# 角度定义
SERVO_PORT_NAME = 'COM7' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
# 数据表定义
ADDRESS_SOFTSTART = 49 # 上电缓启动地址位
SOFTSTART_OPEN = 1 # 上电缓启动-开启
SOFTSTART_CLOSE = 0 # 上电缓启动-关闭
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart)
# 内存表写入
# 注: 在写入之前，需要查阅手册确保该数据位可写
# 缓启动数据类型 uint8_t, 首先构造数据位
softstart_bytes = struct.pack('<B', SOFTSTART_OPEN)
# 将数据写入内存表
ret = uservo.write_data(SERVO_ID, ADDRESS_SOFTSTART, softstart_bytes)
# 打印日志
print("缓启动数据写入是否成功: {}".format(ret))
import time
import serial
from uservo import UartServoManager
# 参数配置
# 角度定义
SERVO_PORT_NAME = 'COM3' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart,is_debug=True)
# 舵机扫描
print("开始进行舵机扫描")
uservo.scan_servo()
servo_list = list(uservo.servos.keys())
for SERVO_ID in servo_list:
is_online = uservo.ping(SERVO_ID,with_logging_info=True)
print("舵机ID={} 是否在线: {}".format(SERVO_ID, is_online))
print("舵机扫描结束, 舵机列表: {}".format(servo_list))
print("测试常规模式")
# 设置舵机为轮式普通模式
# 旋转方向(is_cw) : 顺时针
# 角速度(mean_dps) : 单位°/s
for SERVO_ID in servo_list:
# 导入依赖
import time
import struct
import serial
from uservo import UartServoManager
# 参数配置
# 角度定义
SERVO_PORT_NAME = 'COM3' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
# 数据表定义
ADDRESS_VOLTAGE = 1 # 总线电压值的地址
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart, is_scan_servo=True, is_debug=True)
# 内存表读取
# 注: 因为每个数据位数据格式各不相同
# 因此读取得到的是字节流
voltage_bytes = uservo.read_data(SERVO_ID, ADDRESS_VOLTAGE)
# 数据解析
# 电压的数据格式为uint16_t,单位: mV
# 关于struct的用法，请参阅官方手册: https://docs.python.org/3/library/struct.html
voltage = struct.unpack('<H', voltage_bytes)
print("总线电压 {} mV".format(voltage))
import struct
import serial
from uservo import UartServoManager
# 参数配置
# 角度定义
SERVO_PORT_NAME = 'COM12' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0xFE # 舵机的ID号
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart, is_debug=True, is_scan_servo=False)
def pump_value(uservo, value):
param_bytes = struct.pack('<BhHH', 0xFE, int(value * 10), int(0), int(0))
uservo.send_request(uservo.CODE_SET_SERVO_ANGLE, param_bytes)
def pump_on(uservo):
'''气泵打开'''
pump_value(uservo, -90.0)
def pump_off(uservo):
'''气泵关闭'''
pump_value(uservo, 0.0)
class Arm5DoF:
def __init__(self,
device: str = DEVICE_PORT_DEFAULT,
is_init_pose: bool = True):
'''机械臂初始化'''
# 创建串口对象
try:
self.uart = serial.Serial(port=device, baudrate=115200,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 创建串口舵机管理器
self.uservo = UartServoManager(self.uart, srv_num=SERVO_NUM)
# InitPose
if is_init_pose:
self.init_pose()
except serial.SerialException as e:
logging.error('该设备不存在,请重新填写UART舵机转接板的端口号')
def init_pose(self):
'''机械臂位姿初始化'''
self.set_joint(THETA_INIT_POSE, wait=True)
def set_servo_velocity(self, speed: float):
'''设置舵机的转速 单位°/s'''
self.uservo.mean_dps = max(min(abs(speed), SERVO_SPEED_MAX),
SERVO_SPEED_MIN)
def set_servo_angle(self, angles, wait: bool = False):
'''设置舵机的原始角度'''
if type(angles) == list:
for srv_idx, angle in enumerate(angles):
logging.info('设置舵机角度, 舵机#{} 目标角度 {:.1f}'.format(
srv_idx, angle))
self.uservo.set_servo_angle(int(srv_idx), float(angle))
elif type(angles) == dict:
for srv_idx, angle in angles.items():
logging.info('设置舵机角度, 舵机#{} 目标角度 {:.1f}'.format(
srv_idx, angle))
self.uservo.set_servo_angle(int(srv_idx), float(angle))
if wait:
self.wait() # 等待舵机角度到达目标角度
def set_joint(self,
thetas,
wait: bool = False,
interval=None,
power: int = 0):
'''设置关节的弧度'''
if type(thetas) == list:
for srv_idx, theta in enumerate(thetas):
# 检查弧度的范围约束
theta = min(max(THETA_LOWERB[srv_idx], theta),
THETA_UPPERB[srv_idx])
logging.info('设置关节弧度, 关节#{} 弧度 {:.4f} 角度 {:.1f}'.format(
srv_idx + 1, theta, math.degrees(theta)))
# 根据关节的弧度计算出舵机的原始角度
angle = JOINT2SERVO_K[srv_idx] * theta + JOINT2SERVO_B[srv_idx]
self.uservo.set_servo_angle(int(srv_idx),
float(angle),
interval=interval,
power=power)
elif type(thetas) == dict:
for srv_idx, theta in thetas.items():
# 检查弧度的范围约束
theta = min(max(THETA_LOWERB[srv_idx], theta),
THETA_UPPERB[srv_idx])
logging.info('设置关节弧度, 关节#{} 弧度 {:.4f} 角度 {:.1f}'.format(
srv_idx + 1, theta, math.degrees(theta)))
# 根据关节的弧度计算出舵机的原始角度
angle = JOINT2SERVO_K[srv_idx] * theta + JOINT2SERVO_B[srv_idx]
self.uservo.set_servo_angle(int(srv_idx),
float(angle),
interval=interval,
power=power)
if wait:
self.wait() # 等待舵机角度到达目标角度
def trajectory_plan(self,
joint_id: int,
theta_e: float,
T: float,
w_s: float = 0.0,
w_e: float = 0.0,
a_s: float = 0,
a_e: float = 0):
'''Minimum Jerk轨迹规划'''
# 获取当前关节的
theta_s = self.get_theta(joint_id) # self.get_thetas()[joint_id]
# print("起始弧度 {} 中止弧度 {}".format(theta_s, theta_e))
c = minimum_jerk_plan(theta_s, theta_e, w_s, w_e, a_s, a_e, T)
t_arr, theta_arr = minimum_jerk_seq(T, c, delta_t=TRAJECTORY_DELTA_T)
# print(theta_arr)
return t_arr, theta_arr
def set_joint2(self, thetas, T: float, power: int = 0):
'''设置关节弧度2-带MinimumJerk 轨迹规划版本,需要阻塞等待.
'''
# 将thetas转换为dict类型
if type(thetas) == list:
thetas_arr = thetas
thetas = {}
for joint_id, theta in enumerate(thetas_arr):
thetas[joint_id] = theta
# theta序列
theta_seq_dict = {}
t_arr = None # 时间序列
# 生成轨迹序列
for joint_id, theta_e in thetas.items():
t_arr, theta_arr = self.trajectory_plan(joint_id, theta_e, T)
# print("joint{} theta_arr: {}".format(joint_id, theta_arr))
# print(theta_arr)
theta_seq_dict[joint_id] = theta_arr # np.copy(theta_arr)
# print("DEBUG set_joint2 - theta_seq_dict")
# print(theta_seq_dict)
# 按照轨迹去执行
i = 0
tn = len(t_arr)
while True:
if i >= tn:
break
t_start = time.time()
next_thetas = {}
for joint_id in thetas.keys():
next_thetas[joint_id] = theta_seq_dict[joint_id][i]
# print("next_thetas")
# print(next_thetas)
# 设置关节弧度
self.set_joint(next_thetas, interval=0, power=power)
# print('t_i={} next_thetas: {}'.format(i, next_thetas))
t_end = time.time()
# print('t_end - t_start = {:.4f}'.format(t_end-t_start))
# time.sleep(TRAJECTORY_DELTA_T-(t_end - t_start)) # 延迟
m = int(math.floor((t_end - t_start) / TRAJECTORY_DELTA_T))
i += (1 + m)
# 补齐所需延迟等待的时间
time.sleep(TRAJECTORY_DELTA_T -
((t_end - t_start) - m * TRAJECTORY_DELTA_T))
return theta_seq_dict
def forward_kinematics(self, theta_arr: list):
'''正向运动学'''
theta1, theta2, theta3, theta4 = theta_arr
theta23 = theta2 + theta3
theta234 = theta23 + theta4
x_e_0 = LINK23 * math.cos(theta2) + LINK34 * math.cos(
theta23) + LINK45 * math.cos(theta234)
# 末端坐标
z_e = -(LINK23 * math.sin(theta2) + LINK34 * math.sin(theta23) +
LINK45 * math.sin(theta234))
x_e = x_e_0 * math.cos(theta1)
y_e = x_e_0 * math.sin(theta1)
# 俯仰角
pitch = theta234
return (x_e, y_e, z_e), pitch
def inverse_kinematics(self, p_tool: list, pitch: float):
'''逆向运动学'''
theta1 = math.atan2(p_tool[1], p_tool[0]) # 获得关节1的弧度
if theta1 < THETA_LOWERB[JOINT1] or theta1 > THETA_UPPERB[JOINT1]:
logging.warning('theta1={}, 超出了关节1弧度范围'.format(theta1))
# 计算joint5的坐标
p_joint5 = [p_tool[0], p_tool[1], p_tool[2]]
# 计算joint4的坐标
theta234 = pitch
d = LINK45 * math.cos(pitch)
p_joint4 = [
p_joint5[0] - d * math.cos(theta1),
p_joint5[1] - d * math.sin(theta1),
p_joint5[2] + LINK45 * math.sin(pitch)
]
# 得到了Joint4的坐标之后, 后续就是按照3DoF机械臂逆向运动学求解的方法
x, y, z = p_joint4
distance = math.sqrt(x**2 + y**2 + z**2)
if distance > (LINK23 + LINK34):
logging.warning('超出机械臂的工作范围')
return False, None
# 求解theta3
b = 0
if math.cos(theta1) != 0:
b = x / math.cos(theta1)
else:
b = y / math.sin(theta1)
# 求解theta3
cos_theta3 = (z**2 + b**2 - LINK23**2 - LINK34**2) / (2 * LINK23 *
LINK34)
sin_theta3 = math.sqrt(1 - cos_theta3**2)
theta3 = math.atan2(sin_theta3, cos_theta3)
if theta3 < THETA_LOWERB[JOINT3] or theta3 > THETA_UPPERB[JOINT3]:
logging.warning('theta3={}, 超出了关节3弧度范围'.format(theta3))
return False, None
# 求解theta2
k1 = LINK23 + LINK34 * math.cos(theta3)
k2 = LINK34 * math.sin(theta3)
r = math.sqrt(k1**2 + k2**2)
theta2 = math.atan2(-z / r, b / r) - math.atan2(k2 / r, k1 / r)
if theta2 < THETA_LOWERB[JOINT2] or theta2 > THETA_UPPERB[JOINT2]:
logging.warning('theta2={}, 超出了关节2弧度范围'.format(theta2))
return False, None
# 求解theta4
# 默认设置LINK45始终与桌面平行 0 = theta2 + theta3 + theta4
theta4 = pitch - (theta2 + theta3)
if theta4 < THETA_LOWERB[JOINT4] or theta4 > THETA_UPPERB[JOINT4]:
logging.warning('theta4={}, 超出了关节4弧度范围'.format(theta4))
return False, None
return True, [theta1, theta2, theta3, theta4]
def get_theta(self, srv_idx):
'''获取关节的弧度'''
srv_angle = self.uservo.query_servo_angle(srv_idx)
theta = (srv_angle - JOINT2SERVO_B[srv_idx]) / JOINT2SERVO_K[srv_idx]
return theta
def get_thetas(self, theta_type: type = list):
'''获取当前关节的弧度'''
# 更新舵机角度
self.uservo.query_all_srv_angle()
thetas = []
for srv_idx in range(SERVO_NUM):
# 角度转换为弧度
theta = (self.uservo.servos[srv_idx].angle -
JOINT2SERVO_B[srv_idx]) / JOINT2SERVO_K[srv_idx]
thetas.append(theta)
if theta_type == dict:
# 转换为list格式
thetas_dict = {}
for joint_id, theta in enumerate(thetas):
thetas_dict[joint_id] = theta
return thetas_dict
else:
return thetas
def move(self,
p_tool: list,
pitch: float = 0.0,
wait: bool = False,
is_soft_move: bool = True,
t: float = 1.0):
'''控制机械臂的末端旋转到特定的位置'''
ret, thetas = self.inverse_kinematics(p_tool, pitch)
if ret:
logging.info('移动机械臂末端到: {} Pitch={}'.format(p_tool, pitch))
logging.info("关节弧度: {}".format(thetas))
logging.info("关节角度: {}".format(np.degrees(thetas)))
if is_soft_move:
# self.linear_interpolation(p_tool)
self.set_joint2(thetas, t)
else:
self.set_joint(thetas, wait=wait)
else:
logging.warn('机械臂末端不能到达: {} Pitch={}'.format(p_tool, pitch))
def move2(self,
p_tool: list,
pitch: float = 0.0,
T: float = 1.0,
wait: bool = False,
theta4_offset: float = 0.0):
'''使用minimum jerk测试机械臂的运动'''
ret, thetas = self.inverse_kinematics(p_tool, pitch)
thetas[JOINT4] += theta4_offset # 给theta4添加补偿
if ret:
self.set_joint2(thetas, T)
else:
logging.warn('机械臂末端不能到达: {}'.format(p_tool))
def set_gripper(self, angle: float, t: float = 1.0, power=3000):
'''设置爪子的角度'''
# print("爪子弧度： {}".format(angle))
# self.uservo.query_servo_angle(GRIPPER) # 查询当前角度
# self.set_joint2({GRIPPER:angle}, t)
# 设置爪子的角度 带关节约束
self.set_joint2({GRIPPER: angle}, t, power=power)
# 延时对应的时间
time.sleep(t)
def gripper_open(self):
'''爪子开启'''
self.set_gripper(math.radians(30), t=1.0, power=3000)
def gripper_close(self):
'''爪子闭合'''
self.set_gripper(0, t=1.0, power=3000)
def wait(self):
'''机械臂等待'''
while not self.uservo.is_stop():
# 等待10ms
time.sleep(0.01)
# 导入依赖
import time
import struct
import serial
from uservo import UartServoManager
# 参数配置
# 角度定义
SERVO_PORT_NAME = 'COM7' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
# 数据表定义
ADDRESS_VOLTAGE = 1 # 总线电压值的地址
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart)
# 内存表读取
# 注: 因为每个数据位数据格式各不相同
# 因此读取得到的是字节流
voltage_bytes = uservo.read_data(SERVO_ID, ADDRESS_VOLTAGE)
# 数据解析
# 电压的数据格式为uint16_t,单位: mV
# 关于struct的用法，请参阅官方手册: https://docs.python.org/3/library/struct.html
voltage = struct.unpack('<H', voltage_bytes)
print("总线电压 {} mV".format(voltage))
import struct
import serial
from uservo import UartServoManager
# 参数配置
# 角度定义
SERVO_PORT_NAME = 'COM3' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart, is_debug=True)
# print("[单圈模式]设置舵机角度为90.0°")
# uservo.set_servo_angle(SERVO_ID, 90.0, interval=10) # 设置舵机角度 极速模式
# uservo.wait() # 等待舵机静止
# print("-> {}".format(uservo.query_servo_angle(SERVO_ID)))
print("[单圈模式]设置舵机角度为-80.0°, 周期1000ms")
uservo.set_servo_angle(SERVO_ID, -80.0, interval=1000) # 设置舵机角度(指定周期 单位ms)
# uservo.wait() # 等待舵机静止
print("-> {}".format(uservo.query_servo_angle(SERVO_ID)))
print("[单圈模式]设置舵机角度为70.0°, 设置转速为200 °/s, 加速时间100ms, 减速时间100ms")
uservo.set_servo_angle(SERVO_ID, 170.0, velocity=100.0, t_acc=100,
t_dec=100) # 设置舵机角度(指定转速 单位°/s)
# uservo.wait() # 等待舵机静止
from uservo import UartServoManager, UsInit
# 参数配置
# 角度定义
SERVO_PORT_NAME = 'COM3' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
# 初始化并自动获取舵机串口号
uservo_init = UsInit(logger_level="INFO", scan_servo_port=False)
uservo_init.set_logging_mode()
# SERVO_PORT_NAME = uservo_init.get_servo_port_name()
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE, \
parity=serial.PARITY_NONE, stopbits=1, \
bytesize=8, timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart, is_scan_servo=False, is_debug=True)
# 舵机扫描
print("开始进行舵机扫描")
uservo.scan_servo()
servo_list = list(uservo.servos.keys())
print("舵机扫描结束, 舵机列表: {}".format(servo_list))
# 舵机通讯检测
is_online = uservo.ping(SERVO_ID, with_logging_info=True)
print("舵机ID={} 是否在线: {}".format(SERVO_ID, is_online))
-------------------------------------------------- - 作者: 阿凯 - Email: [email protected] - 更新时间: 2020-12-5 -------------------------------------------------- ''' # 添加uservo.py的系统路径 import sys sys.path.append("../../src") # 导入依赖 import time import serial from uservo import UartServoManager # 参数配置 # 角度定义 SERVO_PORT_NAME = 'COM7' # 舵机串口号 SERVO_BAUDRATE = 115200 # 舵机的波特率 SERVO_ID = 0 # 舵机的ID号 # 初始化串口 uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\ parity=serial.PARITY_NONE, stopbits=1,\ bytesize=8,timeout=0) # 初始化舵机管理器 uservo = UartServoManager(uart) # 舵机通讯检测 is_online = uservo.ping(SERVO_ID) print("舵机ID={} 是否在线: {}".format(SERVO_ID, is_online))
-------------------------------------------------- - 作者: 阿凯 - Email: [email protected] - 更新时间: 2020-12-5 -------------------------------------------------- ''' # 添加uservo.py的系统路径 import sys sys.path.append("../../src") # 导入依赖 import time import serial from uservo import UartServoManager # 参数配置 # 角度定义 SERVO_PORT_NAME = 'COM3' # 舵机串口号 SERVO_BAUDRATE = 115200 # 舵机的波特率 # 初始化串口 uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\ parity=serial.PARITY_NONE, stopbits=1,\ bytesize=8,timeout=0) # 初始化舵机管理器 uservo = UartServoManager(uart,is_scan_servo=True) # 舵机扫描 print("开始进行舵机扫描") uservo.scan_servo() servo_list = list(uservo.servos.keys()) print("舵机扫描结束, 舵机列表: {}".format(servo_list))
import sys
sys.path.append("../../src")
# 导入依赖
import time
import serial
import logging
from uservo import UartServoManager, UsInit
# 参数配置
# 角度定义
SERVO_PORT_NAME = '' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
# 自动获取舵机串口号
sys_init = UsInit()
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE, \
parity=serial.PARITY_NONE, stopbits=1, \
bytesize=8, timeout=0)
logging.info("舵机串口初始化{}".format(uart))
# 初始化舵机管理器
uservo = UartServoManager(uart, is_debug=True)
# 舵机通讯检测
is_online = uservo.ping(SERVO_ID)
print("舵机ID={} 是否在线: {}".format(SERVO_ID, is_online))
- 作者: 阿凯 - Email: [email protected] - 更新时间: 2020-12-5 -------------------------------------------------- ''' # 添加uservo.py的系统路径 import sys sys.path.append("../../src") # 导入依赖 import time import serial from uservo import UartServoManager # 参数配置 # 角度定义 SERVO_PORT_NAME = 'COM7' # 舵机串口号 SERVO_BAUDRATE = 115200 # 舵机的波特率 # 初始化串口 uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\ parity=serial.PARITY_NONE, stopbits=1,\ bytesize=8,timeout=0) # 初始化舵机管理器 uservo = UartServoManager(uart) # 舵机扫描 print("开始进行舵机扫描") uservo.scan_servo() servo_list = list(uservo.servos.keys()) print("舵机扫描结束, 舵机列表: {}".format(servo_list))
# 参数配置
# 角度定义
SERVO_PORT_NAME = 'COM7' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
# 数据表定义
ADDRESS_VOLTAGE = 1 # 总线电压值的地址
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart)
print("设置舵机角度为90.0°")
# 设置舵机角度
uservo.set_servo_angle(SERVO_ID, 90.0)
# 等待舵机静止
uservo.wait()
print("设置舵机角度为-90.0°, 周期1000ms")
# 设置舵机角度(指定周期 单位ms)
uservo.set_servo_angle(SERVO_ID, -90.0, interval=1000)
# 等待舵机静止
uservo.wait()
print("设置舵机角度为90.0°, 设置转速为200 °/s")
# 设置舵机角度(指定转速 单位°/s)
import time
import serial
from uservo import UartServoManager
# 参数配置
# 角度定义
SERVO_PORT_NAME = 'COM7' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart)
print("测试常规模式")
# 设置舵机为轮式普通模式
# 旋转方向(is_cw) : 顺时针
# 角速度(mean_dps) : 单位°/s
uservo.set_wheel_norm(SERVO_ID, is_cw=True, mean_dps=200.0)
# 延时5s然后关闭
time.sleep(5.0)
# 轮子停止
uservo.wheel_stop(SERVO_ID)
time.sleep(1)
import time
import serial
from uservo import UartServoManager
# 参数配置
# 角度定义
SERVO_PORT_NAME = 'COM7' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart)
def log_servo_status():
'''打印舵机状态'''
# 读取温度
voltage = uservo.query_voltage(SERVO_ID)
# 读取电流
current = uservo.query_current(SERVO_ID)
# 读取功率
power = uservo.query_current(SERVO_ID)
# 读取温度
temp = uservo.query_temperature(SERVO_ID)
print("Voltage: {:4.1f}V; Current: {:4.1f}A; Power: {:4.1f}W; T: {:2.0f}℃".format(\
voltage, current, power, temp), end='\r')
FashionStar Uart舵机 > Python SDK 舵机阻尼模式 < -------------------------------------------------- - 作者: 阿凯 - Email: [email protected] - 更新时间: 2020-12-5 -------------------------------------------------- ''' # 添加uservo.py的系统路径 import sys sys.path.append("../../src") # 导入依赖 import time import serial from uservo import UartServoManager # 参数配置 # 角度定义 SERVO_PORT_NAME = 'COM3' # 舵机串口号 SERVO_BAUDRATE = 115200 # 舵机的波特率 SERVO_ID = 0 # 舵机的ID号 # 初始化串口 uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\ parity=serial.PARITY_NONE, stopbits=1,\ bytesize=8,timeout=0) # 初始化舵机管理器 uservo = UartServoManager(uart) power = 200 # 阻尼模式下的功率, 单位mW uservo.set_damping(SERVO_ID, power)
--------------------------------------------------
'''
# 添加uservo.py的系统路径
import sys
sys.path.append("../../src")
# 导入依赖
import time
import serial
from uservo import UartServoManager
# 参数配置
# 角度定义
SERVO_PORT_NAME = 'COM3' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart)
# 设置舵机为阻尼模式
uservo.set_damping(SERVO_ID, 200)
# 舵机角度查询
while True:
angle = uservo.query_servo_angle(SERVO_ID)
print("当前舵机角度: {:4.1f} °".format(angle), end='\r')
time.sleep(1)
import serial
from uservo import UartServoManager
# 参数配置
# 角度定义
SERVO_PORT_NAME = 'COM6' # 舵机串口号
SERVO_BAUDRATE = 115200 # 舵机的波特率
SERVO_ID = 0 # 舵机的ID号
SERVO_HAS_MTURN_FUNC = False # 舵机是否拥有多圈模式
# 初始化串口
uart = serial.Serial(port=SERVO_PORT_NAME, baudrate=SERVO_BAUDRATE,\
parity=serial.PARITY_NONE, stopbits=1,\
bytesize=8,timeout=0)
# 初始化舵机管理器
uservo = UartServoManager(uart, is_debug=True)
print("[单圈模式]设置舵机角度为90.0°")
uservo.set_servo_angle(SERVO_ID, 90.0, interval=0) # 设置舵机角度 极速模式
uservo.wait() # 等待舵机静止
print("-> {}".format(uservo.query_servo_angle(SERVO_ID)))
print("[单圈模式]设置舵机角度为-80.0°, 周期1000ms")
uservo.set_servo_angle(SERVO_ID, -80.0, interval=1000) # 设置舵机角度(指定周期 单位ms)
uservo.wait() # 等待舵机静止
print("-> {}".format(uservo.query_servo_angle(SERVO_ID)))
print("[单圈模式]设置舵机角度为70.0°, 设置转速为200 °/s, 加速时间100ms, 减速时间100ms")
uservo.set_servo_angle(SERVO_ID, 70.0, velocity=200.0, t_acc=100, t_dec=100) # 设置舵机角度(指定转速 单位°/s)
uservo.wait() # 等待舵机静止
print("-> {}".format(uservo.query_servo_angle(SERVO_ID)))