在这一步又对舵机的命令做了一层封装，每一个指令对应的就是一种功能，方便后续进行处理。
这一部分在LeCmd里
文章目录
前提：获取全部动作组文件
1.动作组文件夹
这个文件夹中存放的是所有的动作组文件
2.获取所有动作组文件
获取所有的动作组文件，并且文件是以.d6a结尾的，全部存放一个列表返回。
代码
#获取全部.d6a文件
#!/usr/bin/python3
# encoding: utf-8
import os
# 获取全部.d6a文件
def listActions(path):
if not os.path.exists(path): # 如果path文件不存在 path可以为文件路径
os.mkdir(path) # 创建这个目录 path 为文件路径
pathlist = os.listdir(path) # 返回指定文件夹包含的文件或文件夹
actList = [] # 创建一个列表
for f in pathlist:
#print(f) # 输出这个路径下的所有文件
if f[0] == '.': # 以.开头的文件
pass # 不需要
else:
if f[-4:] == '.d6a': # 后缀是.d6a的文件
#print(f) # 输出所有.d6a的文件
f.replace('-', '') # 把-替换为空格
if f:
actList.append(f) # 添加到actList列表中
else:
pass
return actList
if __name__ == '__main__':
path = '/home/pi/Desktop/Serial_SteerEngine/ActionGroups'
listActions(path)
一、各个指令的作用
1.cmd_i000 空指令
这部分啥也不干，一点用也没有
2.cmd_i001 设置16个舵机的位置
通过这个函数可以设置16个串口舵机的转动位置，必须是16个舵机一起，也就是相当于一个动作组
参数 par：
par[0]:串口舵机运行时间
par[1]:运行指令的长度
par[2]:id1 舵机的ID
par[3]:pos1 舵机的运行位置
par[4]:id2
par[5]:pos2
。。。
3.cmd_i002 停止动作组
通过这个命令可以停止全部舵机的运动。
参数 par：为空
4.cmd_i003 运行动作组
参数 sock：网络对象，此处没有使用。
data=["",""]：存放字符串的列表
data[0]:动作组的命令
data[1]:动作组运行的次数
调用的是runAction方法，此动作只执行一次，也就是说次数被屏蔽了。
5.cmd_i004 查询动作组
通过这个指令，查找所有的动作文件夹里的动作组，并且通过网络发送给请求者，有固定的帧格式
参数 sock：网络对象，需要给出一个要接收数据的对象
data=[]：这个没有用到
注释1
列出所有动作组的名称
注释2
列出每个动作组的开头
注释3
列出所有动作组
注释4
每10个动作组连成一个字符串
注释5
可以看出，这些数据格式是
数据头：I004
数据长度：44
这一帧的数据范围 是1-10 11-20.。。。40-44
6.cmd_i005 删除动作组
通过这个命令可以删除动作组，这个列表中有几个动作组的名字，就删除几个动作组
参数 sock：数据没有使用
data =[]:这个是动作组的名字的列表，有几个删几个
7.cmd_i006 删除所有动作组
参数 sock：数据没有使用
data =[]:数据没有使用
8.cmd_i007 设置任意舵机位置
通过这个命令可以设置任意舵机的位置，可以设置任意个数，可以不按顺序，只要id对应即可。
参数 sock：数据没有使用
data =[]:
data[0]: 舵机运行时间
data[1]:运行舵机的数目
data[2]:id1
data[3]:pos1
data[4]:id2
data[5]:pos2
9.cmd_i008 修改舵机偏差
参数 sock：一个网络请求对象
data =[]:32个数据
data[0]:id1
data[1]:偏差1
data[2]:id2
data[3]:偏差2
。。。
调用setDeviation函数设置偏差值
10.cmd_i009 读取舵机内部偏差
参数 sock：一个网络请求对象
data =[]:没有使用
返回一帧数据给上位机 包括舵机ID和对应的偏差值
二、对应的代码
# 舵机控制命令
#!/usr/bin/env python3
# encoding: utf-8
import os # 导入标准库OS os--操作系统接口模块
import LeActList # 列出目录下的所有.d6a文件，以列表形式返回
import threading # 导入线程
import sys # 导入包含了与Python解释器和它的环境有关的函数
import Serial_Servo_Running # 串口舵机运行
import time # 导入时间模块
import config_serial_servo # 配置串口舵机
actdir = "/home/pi/Desktop/Serial_SteerEngine/ActionGroups" # 动作组文件目录
# 校准值 16个串口舵机的值
Calibration = (500, 500, 303, 500, 500, 500, 500, 725, 500, 500, 697, 500, 500, 500, 500, 275)
# 错误类 继承 Exception
class LeError(Exception):
def __init__(self, data=(), msg="LeError"):
self.data = data
self.msg = msg
def cmd_i000(par): # 指令000 空指令
pass
# 数据格式 时间 舵机数目 id1 pos1 id2 pos2.。。
def cmd_i001(par): # 指令001 舵机运动
global Deviation # 偏移量
if par[0] > 30000: # 时间限制
par[0] = 30000
if par[0] < 20:
par[0] = 20
# len(par) 不等于 par[1]*2+2 或者 len(par)<4
if not par[1] * 2 + 2 == len(par) or not len(par) >= 4:
raise LeError(tuple(par), "舵机运动指令长度错误") # 抛出异常
Servos = par[2:] #去掉运行时间和数据指令长度
for i in range(0, len(Servos), 2): # 遍历16个舵机
if Servos[i] > 16 or Servos[i] < 1 or Servos[i+1] > 1000 or Servos[i+1] < 0:
raise LeError((Servos[i], Servos[i+1]), "舵机运动参数错误")
# 串口舵机
Serial_Servo_Running.serial_setServo(Servos[i], Servos[i + 1], par[0])
def cmd_i002(par): # 指令002 停止运动
Serial_Servo_Running.stop_action_group() # 停止动作组
def cmd_i003(sock, data=["", ""]): # 指令003 运行动作组
# len(data) 不等于2 data有数据 ata[0]不为0 data[1]不为0
if (not len(data) == 2) or (not data) or (not data[0]) or (not data[1]) :
raise LeError(tuple(data), "运行动作组指令错误")
par = None
try:
par = int(data[1]) # 转为整形 data[1] 动作组次数
except:
raise LeError((data[1],),"动作组运行次数错误")
# print(data[0])
# print(par)
if not par is None: # 动作组次数有数据
try:
Serial_Servo_Running.runAction(data[0]) # data[0] 动作组名字
except Exception as e:
print(e)
def cmd_i004(sock, data=[]): # 指令 004查询动作组
actList = LeActList.listActions(actdir) # 列出所有的动作组的动作文件
actList.sort() # 排序
#for i in actList: # 注释1
# print(i)
#print('\n')
if not len(actList) is 0: # 有数据
for i in range(0, len(actList), 10): # 步进是10,隔10个执行一个动作
str_head = "I004-" + str(len(actList))
str_tial = "-" + str(i+1) + "-" # -1- -11- -21-
#print(str_tial) # 注释2
str_tial1 = ""
t = 10
for j in range(0, 10, 1): # 对前面取10个的再次处理
if i+j < len(actList):
#print(actList[i+j]) # 注释3
str_tial1 += "-" + actList[i+j][:-4] # 去掉后缀 并将所有的后缀
#print(str_tial1) # 注释4
else:
if t == 10:
t = j
if str_tial1:
str_head = str_head + str_tial + str(i+t) + str_tial1 + "\r\n"
# print(str_head) # 注释5
sock.sendall(str_head.encode()) # 10个10个的发送动作组
else: # 没有数据
s = "I004-0-0-0\r\n"
ock.sendall(s.encode()) # 通过网络发送
# print(len(actList))
# print(actList)
def cmd_i005(sock, data=[]): # 指令 005 删除一个动作组
if data: # 数据不为空
for d in data: # 遍历这个列表
if d: # 有数据
os.remove(actdir + d + ".d6a") # actdir存放动作组文件目录 d 为动作组的名字
def cmd_i006(sock, data=[]): # 指令 006 删除所有动作组
actList = LeActList.listActions(actdir)
for d in actList:
os.remove(actdir + d) # d为动作组的名字
# 数据格式 time servo_num ID1 pos1 ID2 pos2...
def cmd_i007(sock, data=[]): # 手柄控制
try:
time1 = int(data[0]) # 转为整形 时间
servo_num = int(data[1]) # 舵机的数目
# print(time,servo_num)
servo_data = []
for i in range(servo_num): # 遍历舵机
servo_id = int(data[2 + i * 2]) # data[2] data[4]
servo_pos = int(data[3 + i * 2])# data[3] data[5]
servo_data.append((servo_id, servo_pos-10000))
# print(servo_data)
except:
raise LeError(tuple(data), "参数错误")
try:
for d in servo_data: # 遍历数据列表
Serial_Servo_Running.serial_setServo(d[0], d[1], time1) # 设置串口舵机移动的id 位置 时间
time.sleep(0.5)
except Exception as e:
print(e)
def read_deviation(): # 读取舵机内部偏差
d = []
for i in range(1, 17, 1): # 16个总线串口舵机
zf_d = config_serial_servo.serial_servo_read_deviation(i)
if zf_d > 127: # 负数
zf_d = -(0xff - (zf_d - 1))
d.append(zf_d)
return d # 返回一个偏差值列表
def cmd_i008(sock, data=[]): # 修改偏差
# 1、读取界面舵机P值
if not 32 == len(data):
raise LeError(tuple(data), "1、舵机运动指令长度错误")
Servos = data[:] # 复制数据
j = 0
upper_d = [] # 界面的舵机值
for i in range(0, len(Servos), 2): # 0-32 每隔一个执行一次
j += 1 # 0 2 4 6 ... 30 舵机ID
upper_d.append(int(Servos[i+1])) # 1 3 5 7 。。。31 舵机的偏差值
########################################################
if not len(Calibration) == 16 and not len(upper_d) == 16:
print("偏差数量错误")
s = "I008-dev-no\r\n"
sock.sendall(s.encode()) # 发送错误信息
sys.exit()
else: # 数据正确
new_d = [] # 新的偏差值
for i in range(0, len(upper_d), 1): # 16个ID
if -125 > (upper_d[i] - Calibration[i]) > 125:
print("偏差值超出范围-125~125")
s = "I008-dev-error-range\r\n"
sock.sendall(s.encode())
sys.exit()
else:
# 界面值 - 校准值
new_d.append(upper_d[i] - Calibration[i])
# 配置偏差
for i in range(0, len(new_d), 1):
Serial_Servo_Running.setDeviation(i+1, new_d[i]) # 设置新的偏差值
time.sleep(0.05)
s = "I008-dev-ok\r\n"
sock.sendall(s.encode()) # 发送修改偏差成功
def cmd_i009(sock, data=[]): # 读取舵机内部偏差
global Deviation
# 1、读取舵机内部偏差值 所有舵机 返回一个列表
Deviation = read_deviation()
# 2、清零舵机内部偏差
for i in range(0, 16, 1):
Serial_Servo_Running.setDeviation(i+1, 0)
time.sleep(0.05)
# 3、计算校准值 + 偏差值，生成字符串命令
str_head = "I009-"
str_head += str(len(Deviation))
str_tial = ''
for i in range(0, len(Deviation), 1):
str_tial += "-" + str(i+1) + "-" + str(Calibration[i] + Deviation[i]) # 舵机ID和对应的偏差值
str_head += str_tial
#print(str_head)
# 4、发送数据给上位机
sock.sendall(str_head.encode())
# 命令列表
cmd_list = [cmd_i000, cmd_i001, cmd_i002, cmd_i003, cmd_i004, cmd_i005, cmd_i006,
cmd_i007, cmd_i008, cmd_i009]
#if __name__ == '__main__':
#cmd_i009(None)
350
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
