# serial_support.py
# copyright 2022- https://robot-creation-diary.com/
# 20230320送信する角度データを小数点第2位まで送るようにした。
import time
import serial
import numpy as np
class SerialSuppot:
def __init__(self, pname, cspeed):
self.port_name = pname
self.com_speed = cspeed
self.comport = None
self.neck_comport = None
self.__connect()
self.debug_on = False
self.read_zero_count = 0
def __del__(self):
# comportを閉じる
if None is not self.comport:
self.comport.close()
if self.neck_comport is not None:
self.neck_comport.close()
def __connect(self):
"""
接続
"""
self.comport = serial.Serial(
self.port_name,
self.com_speed,
timeout=0)
time.sleep(1)
self.comport.timeout = 1
print(self.comport.read(100), "\r\n")
self.comport.timeout = 0
def get_rad_keep_in_range(self, rad, min_rad=0.0, max_rad=np.pi):
"""
角度を指定範囲に収める
"""
if rad < min_rad:
return min_rad
if max_rad < rad:
return max_rad
return rad
# 帰ってきた角度を指定範囲に収める処理
def get_rad_to_deg_0_220(self, rad, min_rad=0.0, max_rad=np.pi):
"""
角度(deg)を0から220度の間に収める.2piより大きいとき240を返す。240は脱力
"""
if np.pi * 2 < rad:
# 2piより大きいとき脱力
return 240
rad = self.get_rad_keep_in_range(rad, min_rad, max_rad)
rad -= min_rad
buf = rad * 180 / np.pi
if 220 < buf:
buf = 220
if buf < 0:
buf = 0
return buf
def get_rad_to_deg_inv_0_220(self, rad, min_rad=0.0, max_rad=np.pi):
"""
角度(deg)を0から220度の間に収める.220から0を反転させる
2piより大きいとき240を返す。240は脱力
"""
buf = self.get_rad_to_deg_0_220(rad, min_rad, max_rad)
if 240 == buf:
return 240
return 220 - buf
return buf
def get_rad_to_deg_0_180(self, rad, min_rad=0.0, max_rad=np.pi):
"""
角度(deg)を0から180度の間に収める。2piより大きいとき240を返す。240は脱力
"""
if np.pi * 2 < rad:
# 2piより大きいとき脱力
return 240
rad = self.get_rad_keep_in_range(rad, min_rad, max_rad)
rad -= min_rad
buf = rad * 180 / np.pi
if 180 < buf:
buf = 180
if buf < 0:
buf = 0
return buf
def get_rad_to_deg_inv_0_180(self, rad, min_rad=0.0, max_rad=np.pi):
"""
角度(deg)を0から180度の間に収める。180から0を反転させる.
。2piより大きいとき240を返す。240は脱力
"""
# 0-180を反転させる。ただし、240は脱力なのでそのまま送る。
buf = self.get_rad_to_deg_0_180(rad, min_rad, max_rad)
if 240 == buf:
return 240
return 180 - buf
def get_deg_to_rad(self, deg):
"""
ラジアンをdegに変換
"""
return deg * np.pi / 180.0
# サーボにデータ送信
def __check_data_length_and_reconnect(self, data):
"""
データの長さが0の回数を数え3回より多くなったら再接続
"""
if 0 == len(data):
self.read_zero_count += 1
else:
# 20240717追加 累積でなく連続で3回データを受け取れないとき再接続する。
self.read_zero_count = 0
if 3 < self.read_zero_count:
self.comport.close()
self.__connect()
self.read_zero_count = 0
print("reconnect")
def read_and_print(self):
"""
データを読んで、データがあればプリント
"""
if None is not self.comport:
bs = self.comport.read(256)
if 0 < len(bs):
print(bs)
def move_servo(self, left_arm_list, left_hand,
right_arm_list, right_hand, neck_list,wheel_list):
"""
角度情報をサーボ用の角度に変更して送信
"""
send_data = []
send_data.append(0xFF)
send_data.append(0)
# 1byteに収まらない数値の場合"bytes"でエラーになる
send_data.append(int(self.get_rad_to_deg_0_180(
right_arm_list[0],
-np.pi * 0.5,
np.pi * 0.5)))
send_data.append(int(self.get_rad_to_deg_0_180(
right_arm_list[0],
-np.pi * 0.5,
np.pi * 0.5) * 100) % 100)
send_data.append(int(self.get_rad_to_deg_0_180(
right_arm_list[1] * -1,
self.get_deg_to_rad(-20.0),
self.get_deg_to_rad(160.0))))
send_data.append(int(self.get_rad_to_deg_0_180(
right_arm_list[1] * -1,
self.get_deg_to_rad(-20.0),
self.get_deg_to_rad(160.0)) * 100) % 100)
# 上腕軸
send_data.append(int(self.get_rad_to_deg_0_180(
right_arm_list[2],
-np.pi * 0.5,
np.pi * 0.5)))
send_data.append(int(self.get_rad_to_deg_0_180(
right_arm_list[2],
-np.pi * 0.5,
np.pi * 0.5) * 100) % 100)
# 肘
send_data.append(int(self.get_rad_to_deg_0_180(
(right_arm_list[3] +
self.get_deg_to_rad(140.0)),
0,
np.pi)))
send_data.append(int(self.get_rad_to_deg_0_180(
(right_arm_list[3] +
self.get_deg_to_rad(140.0)),
0,
np.pi) * 100) % 100)
# 手首回転
send_data.append(int(self.get_rad_to_deg_0_180(
(right_arm_list[4]) * -1,
-np.pi * 0.5,
np.pi * 0.5)))
send_data.append(int(self.get_rad_to_deg_0_180(
(right_arm_list[4]) * -1,
-np.pi * 0.5,
np.pi * 0.5) * 100) % 100)
send_data.append(int(self.get_rad_to_deg_0_180(
(right_arm_list[5]),
self.get_deg_to_rad(-20.0),
self.get_deg_to_rad(160.0))))
send_data.append(int(self.get_rad_to_deg_0_180(
(right_arm_list[5]),
self.get_deg_to_rad(-20.0),
self.get_deg_to_rad(160.0)) * 100) % 100)
send_data.append(int(self.get_rad_to_deg_0_180(
right_arm_list[6],
-np.pi * 0.5,
np.pi * 0.5)))
send_data.append(int(self.get_rad_to_deg_0_180(
right_arm_list[6],
-np.pi * 0.5,
np.pi * 0.5) * 100) % 100)
send_data.append(int(self.get_rad_to_deg_inv_0_220(
right_hand,
0,
self.get_deg_to_rad(220.0))))
send_data.append(int(self.get_rad_to_deg_inv_0_220(
right_hand,
0,
self.get_deg_to_rad(220.0)) * 100) % 100)
# 左手
send_data.append(int(self.get_rad_to_deg_inv_0_180(
left_arm_list[0],
-np.pi * 0.5,
np.pi * 0.5)))
send_data.append(int(self.get_rad_to_deg_inv_0_180(
left_arm_list[0],
-np.pi * 0.5,
np.pi * 0.5) * 100) % 100)
send_data.append(int(self.get_rad_to_deg_0_180(
left_arm_list[1] * -1,
self.get_deg_to_rad(-160.0),
self.get_deg_to_rad(20.0))))
send_data.append(int(self.get_rad_to_deg_0_180(
left_arm_list[1] * -1,
self.get_deg_to_rad(-160.0),
self.get_deg_to_rad(20.0)) * 100) % 100)
# 上腕軸
send_data.append(int(self.get_rad_to_deg_0_180(
left_arm_list[2],
-np.pi * 0.5,
np.pi * 0.5)))
send_data.append(int(self.get_rad_to_deg_0_180(
left_arm_list[2],
-np.pi * 0.5,
np.pi * 0.5) * 100) % 100)
# 肘
send_data.append(int(self.get_rad_to_deg_inv_0_180(
(left_arm_list[3] +
self.get_deg_to_rad(140.0)),
0,
np.pi)))
send_data.append(int(self.get_rad_to_deg_inv_0_180(
(left_arm_list[3] +
self.get_deg_to_rad(140.0)),
0,
np.pi) * 100) % 100)
# 手首回転
send_data.append(int(self.get_rad_to_deg_0_180(
left_arm_list[4],
-np.pi * 0.5,
np.pi * 0.5)))
send_data.append(int(self.get_rad_to_deg_0_180(
left_arm_list[4],
-np.pi * 0.5,
np.pi * 0.5) * 100) % 100)
send_data.append(int(self.get_rad_to_deg_0_180(
(left_arm_list[5]),
self.get_deg_to_rad(-160.0),
self.get_deg_to_rad(20.0))))
send_data.append(int(self.get_rad_to_deg_0_180(
(left_arm_list[5]),
self.get_deg_to_rad(-160.0),
self.get_deg_to_rad(20.0)) * 100) % 100)
send_data.append(int(self.get_rad_to_deg_0_180(
left_arm_list[6],
-np.pi * 0.5,
np.pi * 0.5)))
send_data.append(int(self.get_rad_to_deg_0_180(
left_arm_list[6],
-np.pi * 0.5,
np.pi * 0.5) * 100) % 100)
send_data.append(int(self.get_rad_to_deg_0_220(
left_hand,
0,
self.get_deg_to_rad(220.0))))
send_data.append(int(self.get_rad_to_deg_0_220(
left_hand,
0,
self.get_deg_to_rad(220.0)) * 100) % 100)
# 首
send_data.append(int(neck_list[0]))
send_data.append(int(neck_list[0] * 100) % 100)
send_data.append(int(neck_list[1]))
send_data.append(int(neck_list[1] * 100) % 100)
# 左右の車輪
send_data.append(int((wheel_list[0] + 1) * 100))
send_data.append(0)
send_data.append(int((wheel_list[1] + 1) * 100))
send_data.append(0)
# 1byteに収まらない数値の場合"bytes"でエラーになる
x = 0
if self.debug_on:
send_data = [255, 19, 90, 20, 90, 40, 90, 20, 0, 90,
142, 164, 96, 56, 90, 151, 00, 90, 90, 90]
for i in range(2, len(send_data)):
x = x ^ send_data[i]
send_data.append(x)
send_data.append(0xFE)
send_data[1] = len(send_data)
print("send_data len", len(send_data))
print("b_send_data len", len( bytes(send_data)))
print("send_data", send_data)
# 0-255に収まっていないときはエラーデータを表示
for data in send_data:
if data < 0 or 255 < data:
print("send_data", send_data)
print("left_arm_list, left_hand, right_arm_list, right_hand",
left_arm_list, left_hand, right_arm_list, right_hand)
bs = bytes(send_data)
self.comport.write(bs)
time.sleep(0.05) # 受信データを受け取るためのスリープ
bs = self.comport.read(256)
self.__check_data_length_and_reconnect(bs)
print("read data len|", len(bs))
print("read data bytes|", bs) # データ内容表示これでscrialのデータを読み出しでバッファデータから消す。
def open_neck_comport(self, port, bps):
self.neck_comport = serial.Serial(port, bps)
def move_neck_servo(self, servo1, servo2):
test = []
test.append(0xFD)
test.append(0x09)
test.append(0x06)
test.append(0x00)
test.append(0x0F)
# 1byteに収まらない数値の場合"bytes"でエラーになる
test.append(int(servo1))
test.append(0x0)
# 1byteに収まらない数値の場合"bytes"でエラーになる
test.append(int(servo2))
test.append(0xFE)
bs = bytes(test)
if None is not self.neck_comport:
self.neck_comport.write(bs)
