我們如何通過外部去控制樹莓派序列槽總線舵機和PWM舵機。
網絡無疑是一個很好的選擇。
這一部分是在lsc.py檔案中
關于socketserver部分參考
socketserver
文章目錄
- 調試工具
-
- 1.網絡調試助手
- 2.樹莓派
- 一、控制指令
-
- 1.設定PWM舵機位置
- 2.運作動作組
- 3.複位兩個PWM舵機
- 4.讀出舵機的電壓值
- 二、代碼
調試工具
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() # 關閉服務