我们如何通过外部去控制树莓派串口总线舵机和PWM舵机。
网络无疑是一个很好的选择。
这一部分是在lsc.py文件中
关于socketserver部分参考
socketserver
调试工具
1.网络调试助手
2.树莓派
一、控制命令
1.设置PWM舵机位置
设置PWM舵机的位置,一次设置一个舵机
指令格式
55 55 08 3 xx xx xx id posl posh
id = 6 或者是等于7
7是控制摄像头上下的舵机
6是控制摄像头左右的舵机
posl :位置的低位
posh:位置的高位
示例
55 55 08 3 0 0 0 7 0 0
这是设置舵机ID7设置位置为3000-1200 = 1800
说明:
位置小于1200,就是1200,
位置大于1900,就是1900
测试
这里的0是传进去的参数,实际的值没有输出,懒的改了
2.运行动作组
发送这个指令可以运行一组动作
指令格式
55 55 05 06 action_num action_timel action_timeh
action_num :运行的动作组的名字
action_timel :运行的次数的低位
action_timeh:运行的次数的高位
示例
55 55 05 06 0 0 2
运行立正动作 2次
测试
3.复位两个PWM舵机
运行这个指令,可以使两个PWM舵机复位到中位的位置
指令格式
55 55 02 03
没有参数
示例
55 55 02 03
将两个PWM舵机复位到1500的位置
测试
4.读出舵机的电压值
运行这个指令可以读出舵机的电压值
指令格式
55 55 03 0f id
id:是舵机的ID
示例
55 55 03 0f 6
这里读取的是ID号为6的舵机的电压值
测试
树莓派运行结果:
网络助手运行结果:
二、代码
# 网络控制
#!/usr/bin/python3
# -*- coding: UTF-8 -*-
import socketserver # 提供了服务器中心类,可以简化网络服务器的开发
import PWMServo # 导入PWM舵机二次封装
import Serial_Servo_Running as SSR # 动作组调用
import threading # 线程
import get_data # 从文件中获取PID的值
import math # 导入数学库
from config_serial_servo import serial_servo_read_vin #导入读取舵机的电压
import time # 导入时间模块
import os # 导入系统模块
DEBUG = False # 开启调试模式
client_socket = [] # 创建一个客户端列表
# 开启动作组线程,动作一发生变化,就会去执行动作
SSR.start_action_thread() # 开启动作组线程
# TCPServer是接收到请求后执行handle方法,如果前一个的handle没有结束,那么其他的请求将不会受理,新的客户端也无法加入。
# 而ThreadingTCPServer和ForkingTCPServer则允许前一连接的handle未结束也可受理新的请求和连接新的客户端,
# 区别在于前者用建立新线程的方法运行handle,后者用新进程的方法运行handle
# 机器人的服务类 负责绑定,监听,运行
class LobotServer(socketserver.ThreadingTCPServer): # 继承ThreadingTCPServer
allow_reuse_address = True # 允许地址重用
# 机器人的请求处理类 负责处理用户所发送的数据 定义一个类,继承socketserver.BaseRequestHandler
class LobotServerHandler(socketserver.BaseRequestHandler):
global client_socket # 全局变量 客户端列表
ip = "" # ip地址
port = None # 端口号
# setup在handle()之前被调用,主要的作用就是执行请求之前的初始化相关的工作。
def setup(self):
# strip()方法用于移除字符串头尾指定的字符(默认为空格或换行符)或字符序列
# client_address获取的是一个元组 包含ip地址和端口
self.ip = self.client_address[0].strip() # 获取客户端1的IP地址
self.port = self.client_address[1] # 获取端口号
print("connected\tIP:"+self.ip+"\tPort:"+str(self.port))
client_socket.append(self.request) # 将此连接加入客户端列表
#self.request.settimeout(6) # 超时时间为6秒
# 这个方法必须有,每一个客户端链接之后都会执行这个函数
# 工作就是做那些所有与处理请求相关的工作
def handle(self):
global action_num, action_times, inf_flag
conn = self.request # 获取连接的客户端的对象
recv_data = b'' # 接收数据都是字节类型的
Flag = True # 循环标志 循环
stop = False # 结束标志
t2 = 0
while Flag: # 循环
try:
buf = conn.recv(128) # 接收128个字节数据 可以小于这个长度
# Python3的字符串的编码语言用的是unicode编码,由于Python的字符串类型是str,在内存中以Unicode表示,
# 一个字符对应若干字节,如果要在网络上传输,或保存在磁盘上就需要把str变成以字节为单位的bytes
if buf == b'': # 空字符串 没有接收到数据
Flag = False # 退出循环
else: # 接收到数据
# 打印出数据
print('收到的数据是:',(buf))
recv_data = recv_data + buf # 将字符串转化为字节类型
if len(recv_data) <= 13: # 接收数据长度小于等于13
print('收到%d个字节' %(len(recv_data)))
# 解决断包问题,接收到完整命令后才发送到串口,防止出错
while True: # 死循环
try:
index = recv_data.index(b'\x55\x55') # 搜索数据中的0x55 0x55的下标
print('index is :',index)
# 加3 index返回的是第一个0x55的位置,两个0x55 2个字节 加3表示后面还有数据
if len(recv_data) >= index+3: # 缓存中的数据长度是否足够
# 判断下面接受的指令
recv_data = recv_data[index:] # 截取完整的指令 包含帧头
# recv_data[2]是去掉帧头的第一个数据 也就是cmd数据的长度 -2
# 下面是判断数据是否正确 cmd小于一帧数据
if recv_data[2] + 2 <= len(recv_data): # 缓存中的数据长度是否足够
cmd = recv_data[0:(recv_data[2]+2)] # 取出命令,包含帧头
#print(cmd)
recv_data = recv_data[(recv_data[2]+3):] # 去除已经取出的命令
# 0x55 0x55 08 03 xx xx xx id posl posh # 设置PWM舵机的位置 id=6/7
# 55 55 03 0F id # 读舵机的电压值
# 55 55 05 06 action_num action_timesl action_timesh # 运行动作组
# 55 55 02 03 # 复位两个PWM舵机
if cmd[0] and cmd[1] is 0x55:
if cmd[2] == 0x08 and cmd[3] == 0x03: # 数据长度和控制单舵机命令
id = cmd[7]
pos = 0xffff & cmd[8] | 0xff00 & (cmd[9] << 8)
# print('id:', cmd[7], 'pos:', pos)
print('设置舵机%d的位置为%d:' %(id,pos))
if id == 7:
if 1900 < pos:
pos = 1900
elif pos < 1200:
pos = 1200
PWMServo.setServo(1, 3000 - pos, 20)
elif id == 6:
PWMServo.setServo(2, 3000 - pos, 20)
else:
pass
# 55 55 05 06 action_num action_timesl action_timesh
elif cmd[2] == 0x05 and cmd[3] == 0x06: # 数据长度和控制舵机动作组命令
action_num = cmd[4]
action_times = 0xffff & cmd[5] | 0xff00 & cmd[6] << 8
print('运行动作组')
# 站立动作
if action_num == 0:
SSR.stop_action_group()
# 缓慢站立 运行一次
SSR.change_action_value('stand_slow', 1)
try:
# 读取pid文件
data = get_data.read_data()
if data[0] != 'K':
os.system('sudo kill -9 ' + str(data[0]))
PWMServo.setServo(1, 1500, 300)
PWMServo.setServo(2, 1500, 300)
get_data.write_data("K", "0")
except BaseException as e:
print(e)
# 无限次
elif action_times == 0: # 无限次
print('action', action_num, 'times', action_times)
SSR.change_action_value(str(action_num), action_times)
stop = True
else:
print('action', action_num, 'times', action_times)
if stop:
SSR.stop_action_group()
# 缓慢站立 执行一次
SSR.change_action_value('stand_slow', 1)
stop = False
else:
print('action', action_num, 'times', action_times)
SSR.change_action_value(str(action_num), action_times)
# 55 55 02 03
# 复位两个PWM舵机
elif cmd[2] == 0x02 and cmd[3] == 0x03:
print('复位两个PWM舵机')
PWMServo.setServo(1, 1500, 100)
PWMServo.setServo(2, 1500, 100)
time.sleep(0.1)
# 55 55 03 0F id
# 读舵机的电压值
elif cmd[2] == 0x03 and cmd[3] == 0x0F:
# 如果动作完成了
print('读取舵机的电压值')
if SSR.action_finish():
try:
time.sleep(0.05)
# ath.ceil 返回数字的上入整数
v = math.ceil(serial_servo_read_vin(cmd[4]))
print('电压值是:',v)
buf = [0x55, 0x55, 0x04, 0x0F, 0x00, 0x00]
buf[4] = v & 0xFF
buf[5] = (v >> 8) & 0xFF
# sendall 发送全部的数据
conn.sendall(bytearray(buf))
print('读取电压值成功')
except BaseException as e:
print(e)
else:
print('正在运行动作,请稍后')
# 调试,打印命令cmd
if DEBUG is True:
for i in cmd:
print (hex(i))
print('*' * 30)
else: # 接收数据不正确
break
else: # 数据长度不够
break
except Exception as e: # 在recv_data 中搜不到 '\x55\x55'子串
break
recv_data = b'' # 把数据缓冲区清空
action_times = None # 动作次数
action_num = None # 运行动作组名字
else: # 接收数据大于13个 是错误的数据
recv_data = b'' # 把数据缓冲区清空
pass
except Exception as e: # 接收数据错误
print(e)
Flag = False # 退出循环
break
# 数据发送完成,并且不再使用此客户端 断开此客户端连接
# 在handle()方法之后被调用,执行当处理完请求后的清理工作,默认不会做任何事
def finish(self):
client_socket.remove(self.request) # 从客户端列表中剔除此连接
print("disconnected\tIP:"+self.ip+"\tPort:"+str(self.port))
if __name__ == "__main__":
server = LobotServer(("", 9029), LobotServerHandler) # 建立服务器
try:
server.serve_forever() # 开始服务器循环
except Exception as e:
print(e)
server.shutdown() # 关闭服务器
server.server_close() # 关闭服务