代码拉取完成，页面将自动刷新
#!/usr/bin/python3
# -*- coding: utf-8 -*-
import time
import datetime
import platform
import serial
global SerialHdl
#//////////////// 宏定义 勿改动 //////////////////
DEMO_MODE_CYCLE = 0 #demo 循环播放
DEMO_MODE_ONCE = 1 #demo 只播放一次
ABSA = 0 # 动作描述中角度信息的类型: 绝对值 (无视舵机当前角度，直接指定舵机角度)
RELA = 1 # 动作描述中角度信息的类型: 相对值 (以舵机当前角度为基准偏移指定角度)
SERVO_SUM_MAX = 9 # 参与动作的舵机数量
#//////////////// 功能配置宏定义 充分理解含义后再改动 //////////////////
SERVO_SUM = 9
DEMO_MODE = DEMO_MODE_ONCE
#舵机群基础动作库
servo_motion_lib = [
#servoID: 1# 2# 3# 4# 5# 6# 7# 8# 9#
#--------------------------------------------------------------------------------------------------------------------------------------------------------------------------
#0#动作: 1#舵机角度归零,耗时1秒 其他不变
[[ 0,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#1#动作: 2#舵机角度归零,耗时1秒 其他不变
[[ 0,RELA, 0],[ 0,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#2#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#3#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#4#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#5#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#6#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0], ],
#7#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,ABSA, 1000],[ 0,RELA, 0], ],
#8#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,ABSA, 1000], ],
#9#动作: 9#舵机顺时针+90度 耗时1秒 其他不变
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 90,RELA, 1000], ],
#10#动作: 8#舵机顺时针+90度 耗时1秒 其他不变
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 90,RELA, 1000],[ 0,RELA, 0], ],
#11#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0], ],
#12#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#13#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#14#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#15#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#16#
[[ 0,RELA, 0],[ 90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#17#
[[ 90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#18# 所有舵机顺时针+90度 耗时1秒
[[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000], ],
#19# 所有舵机逆时针-90度 耗时1秒
[[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000], ],
#20#动作: 1#舵机逆时针-90度 其他不变
[[-90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#21#动作: 2#舵机逆时针-90度 其他不变
[[ 0,RELA, 0],[-90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#22#
[[ 0,RELA, 0],[ 0,RELA, 0],[-90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#23#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[-90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#24#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[-90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#25#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[-90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#26#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[-90,RELA, 1000],[ 0,RELA, 0],[ 0,RELA, 0], ],
#27#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[-90,RELA, 1000],[ 0,RELA, 0], ],
#28#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[-90,RELA, 1000], ],
#29#动作: 1#舵机角度180,耗时1秒 其他不变
[[180,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#30#动作: 2#舵机角度180,耗时1秒 其他不变
[[ 0,RELA, 0],[180,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#31#
[[ 0,RELA, 0],[ 0,RELA, 0],[180,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#32#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[180,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#33#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[180,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#34#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[180,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0], ],
#35#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[180,ABSA, 1000],[ 0,RELA, 0],[ 0,RELA, 0], ],
#36#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[180,ABSA, 1000],[ 0,RELA, 0], ],
#37#
[[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[ 0,RELA, 0],[180,ABSA, 1000], ],
]
#舵机群动作序列 (存储在FLASH中)
#将各种基础动作通过不同的延时粘合在一起，构成复杂的动作序列
servo_demo = [
#/////////////////////////// 位置初始化 全部躺下 //////////////////////////////////////
#//全部角度归零(横躺) 标准速度
[ 0, 0,100], [ 1, 0,100], [ 2, 0,100], [ 3, 0,100], [ 4, 0,100], [ 5, 0,100], [ 6, 0,100], [ 7, 0,100], [ 8, 2000,100],
#/////////////////////////////////////////////////////////////////
#//反序挨个竖起 110% speed
[ 9, 900, 90], [10, 900, 90], [11, 900, 90], [12, 900, 90], [13, 900, 90], [14, 900, 90], [15, 900, 90], [16, 900, 90], [17, 1500, 90],
# #//顺时针全部躺下 120%
[18, 1500, 80],
# #//正序挨个竖起 130%
[20, 700, 70], [21, 700, 70], [22, 700, 70], [23, 700, 70], [24, 700, 70], [25, 700, 70], [26, 700, 70], [27, 700, 70], [28, 1500,70],
# #/////////////////////////////////////////////////////////////////
# #//逆时针正序挨个躺下 但不等前一个完全躺下 下一个就要开始动作 标准速度
[20, 130, 60], [21, 130, 60], [22, 130, 60], [23, 130, 60], [24, 130, 60], [25, 130, 60], [26, 130, 60], [27, 130, 60], [28, 1500, 60],
# #//正序 奇数舵机先竖起 反序偶数舵机后竖起
[17, 800, 80], [15, 800, 80], [13, 800, 80], [11, 800, 80], [ 9, 800, 80],
[10, 800, 80], [12, 800, 80], [14, 800, 80], [16, 800, 80],
# #//反序两两一组依次顺时针躺下
[29, 0, 50], [30, 600, 50], [31, 0, 80], [32, 1000, 80], [33, 0,110], [34, 1200,110], [35, 0, 150], [36, 0, 150], [37,2500,150],
# #//反序逆时针起来且执行速度越来越快
[28, 1000,100], [27, 960, 96], [26, 920, 92], [25, 880, 88], [24, 840, 84], [23, 800, 80], [22, 720, 72], [21, 600, 60], [20, 2500, 50],
# # #//摇晃速度不变 幅度越来越大
[19, 60,200], [18, 120,200], [19, 140,200], [18, 140,200], [19, 160,200], [18, 160,200], [19, 180,200], [18, 180,200], [19, 200,200],
[18, 200,200], [19, 220,200], [18, 220,200], [19, 220,200], [18, 240,200], [19, 260,200], [18, 260,200], [19, 280,200],
[18, 300,200], [19, 300,200], [18, 320,200], [19, 340,200], [18, 340,200], [19, 360,200], [18, 360,200], [19, 360,200],
[18, 400,200], [19, 440,200], [18, 500,200], [19, 500,200], [18, 560,200], [19, 600,200], [18, 600,200], [19, 600,200],
[18, 660,200], [19, 660,200], [18, 720,200], [19, 720,200], [18, 780,200], [19, 780,200], [18, 840,200], [19, 840,200],
[ 0, 0,100], [ 1, 0,100], [ 2, 0,100], [ 3, 0,100], [ 4, 0,100], [ 5, 0,100], [ 6, 0,100], [ 7, 0,100], [ 8, 2000,100],
# # WAVE: T/2 = 1920ms, T_90deg = 960ms, T_delta = 240ms
[17, 360, 96], [16, 360, 96], [15, 360, 96], [14, 360, 96], [13, 360, 96], [12, 360, 96], [11, 360, 96], [10, 360, 96], [ 9, 360, 96],
[20, 360, 96], [21, 360, 96], [22, 360, 96], [23, 360, 96], [24, 360, 96], [25, 360, 96], [26, 360, 96], [27, 360, 96], [28, 360, 96],
[17, 280, 96], [16, 280, 96], [15, 280, 96], [14, 280, 96], [13, 280, 96], [12, 280, 96], [11, 280, 96], [10, 280, 96], [ 9, 280, 96],
[20, 280, 96], [21, 280, 96], [22, 280, 96], [23, 280, 96], [24, 280, 96], [25, 280, 96], [26, 280, 96], [27, 280, 96], [28, 280, 96],
[17, 200, 96], [16, 200, 96], [15, 200, 96], [14, 200, 96], [13, 200, 96], [12, 200, 96], [11, 200, 96], [10, 200, 96], [ 9, 200, 96],
[20, 200, 96], [21, 200, 96], [22, 200, 96], [23, 200, 96], [24, 200, 96], [25, 200, 96], [26, 200, 96], [27, 200, 96], [28, 200, 96],
[17, 120, 96], [16, 120, 96], [15, 120, 96], [14, 120, 96], [13, 120, 96], [12, 120, 96], [11, 120, 96], [10, 120, 96], [ 9, 120, 96],
[20, 120, 96], [21, 120, 96], [22, 120, 96], [23, 120, 96], [24, 120, 96], [25, 120, 96], [26, 120, 96], [27, 120, 96], [28, 120, 96],
[17, 80, 96], [16, 80, 96], [15, 80, 96], [14, 80, 96], [13, 80, 96], [12, 80, 96], [11, 80, 96], [10, 80, 96], [ 9, 80, 96],
[20, 80, 96], [21, 80, 96], [22, 80, 96], [23, 80, 96], [24, 80, 96], [25, 80, 96], [26, 80, 96], [27, 80, 96], [28, 80, 96],
[17, 120, 96], [16, 120, 96], [15, 120, 96], [14, 120, 96], [13, 120, 96], [12, 120, 96], [11, 120, 96], [10, 120, 96], [ 9, 120, 96],
[20, 120, 96], [21, 120, 96], [22, 120, 96], [23, 120, 96], [24, 120, 96], [25, 120, 96], [26, 120, 96], [27, 120, 96], [28, 1500, 96],
#//全部角度归零(横躺) 标准速度
[ 0, 0,100], [ 1, 0,100], [ 2, 0,100], [ 3, 0,100], [ 4, 0,100], [ 5, 0,100], [ 6, 0,100], [ 7, 0,100], [ 8, 3000,100],
# #//顺时针正序挨个竖起 但不等前一个完全竖起 下一个就要开始动作 标准速度
[17, 130,200], [16, 130,200], [15, 130,200], [14, 130,200], [13, 130,200], [12, 130,200], [11, 130,200], [10, 130,200], [9, 130,200],
];
#//舵机对象列表
#//每个列表项代表一个舵机 里面包含了描述舵机关键属性/状态的变量
theServos = [#//设定每个舵机的初始状态
[ 1, 0, 0 ], #//1#舵机
[ 2, 0, 0 ], #//2#舵机
[ 3, 0, 0 ], #//3#舵机
[ 4, 0, 0 ], #//4#舵机
[ 5, 0, 0 ], #//5#舵机
[ 6, 0, 0 ], #//6#舵机
[ 7, 0, 0 ], #//7#舵机
[ 8, 0, 0 ], #//8#舵机
[ 9, 0, 0 ], #//9#舵机
];
def ser_open():
global SerialHdl
if platform.system()=='Linux':
sdev = '/dev/ttyUSB0'
else:
sdev = 'COM14' # windows
bps = 115200 # 9600,19200,38400,57600,115200
timout = 0.008 # timeout (second) / None : waiting forever
try:
SerialHdl = serial.Serial(sdev, bps, bytesize=8, parity='N', stopbits=1, timeout=timout)
except Exception as e:
print('error, can not open [%s]: ', sdev, e)
SerialHdl = None
return SerialHdl
def ser():
global SerialHdl
return SerialHdl
def ser_close():
global SerialHdl
ser().close()
SerialHdl = None
def load_animation_unit(unit):
return {
"mid" : unit[0], #motion id
"delay": unit[1], #ms
"speed": unit[2]
}
def load_servo_motion_lib(motion_id, srvsum):
global servo_motion_lib
return servo_motion_lib[motion_id][:srvsum]
def checksum(cmd):
cksum = 0
for i in range(2,8):
cksum += cmd[i]
return cksum&0xFF
def serial_write_read(send, recv_len):
sz = len(send)
print('['+str(datetime.datetime.now())+'] SEND: '+('%02X '*sz)%tuple(send[:sz]))
ser().write(bytes((send)[:len(send)]))
recv = []
while len(recv)<recv_len:
rbyte = ser().read() #read one byte
if rbyte==b'':
print('read timeout')
break
recv.append(ord(rbyte))
sz = len(recv)
print('['+str(datetime.datetime.now())+'] RECV: '+('%02X '*sz)%tuple(recv[:sz]))
return recv
def send_servo_motion(servo, motion, speed):
#servo[0]#uint8_t id; // 舵机ID
#servo[1]#int16_t angle; // 舵机当前角度 单位: 度
#servo[2]#int16_t adjust; // 角度偏移校正值
#motion[0]#int16_t angle; // 角度调整信息 单位: deg //
#motion[1]#int8_t type; // ABSA or RELA
#motion[2]#int16_t time; // 指定动作完成时间
#舵机命令组包缓冲区
cmd = [0xFA, 0xAF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xED]
srvid = servo[0];
if (RELA==motion[1]) and (0==motion[0]):
return 0 #//在舵机当前角度基础上偏移0度， 即动作为空 直接返回
print('send_servo_motion: '+str(motion))
if RELA==motion[1]:
#相对角度 先读取舵机当前角度
cmd[2] = srvid
cmd[3] = 0x02 #角度回读
cmd[4] = 0x00
cmd[5] = 0x00
cmd[6] = 0x00
cmd[7] = 0x00
cmd[8] = checksum(cmd)
recv = serial_write_read(cmd, 10) #发送10字节 接收10字节
if len(recv)<10:
return 1
servo[1] = recv[7] #保存读回的角度值
servo[1] += motion[0]
else:
servo[1] = motion[0] #直接指定角度值
#发送命令 调整角度
if servo[1]<0:
servo[1] = 0
cmd[2] = srvid
cmd[3] = 0x01 #设置角度
cmd[4] = servo[1]&0xFF #最终的目标角度
cmd[5] = int(motion[2]*speed/(100*20))&0xFF #运动时间
cmd[6] = 0x00
cmd[7] = 0x00
cmd[8] = checksum(cmd)
serial_write_read(cmd, 1) #发送10字节 接收1字节
return 0
def animation_start( mode, srvlst, srvsum, animate, animate_len ):
print('animation_start')
step = 0
stop = False
while not stop:
if mode==DEMO_MODE_ONCE:
stop = True
while step<animate_len:
au = load_animation_unit(animate[step])
srvmts = load_servo_motion_lib(au["mid"], srvsum)
for i in range(0, srvsum):
if send_servo_motion(srvlst[i], srvmts[i], au["speed"])!=0:
return 1; #返回非0值，表示命令发送出错
time.sleep(au["delay"]/1000)
step += 1
def setup():
if ser_open()==None:
exit(0)
else:
print('open success')
def loop():
animation_start(DEMO_MODE, theServos, SERVO_SUM, servo_demo, len(servo_demo))
def main():
setup();
while True:
loop()
if DEMO_MODE==DEMO_MODE_ONCE:
break
ser_close()
return
main()
此处可能存在不合适展示的内容，页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容，可点击提交进行申诉，我们将尽快为您处理。