def handle_body_up_down(srv):
if srv.up_down_status < 0: # 降低
key_byte = 0xf0
elif srv.up_down_status > 0: # 抬高
key_byte = 0x0f
else:
key_byte = 0x00
# 设置16路IO全为输出模式
buf = [0x55, 0xaa, 0x71, 0x02, 0x03]
buf.extend([0xff, 0xff])
buf.append(solve_sum(buf))
if link.write(buf):
# IO输出，4~7位为1,其余位为0
buf = [0x55, 0xaa, 0x71, 0x02, 0x04]
buf.extend([0x00, key_byte])
buf.append(solve_sum(buf))
if link.write(buf):
return True
return False
return False
def handle_servo_control(srv):
"""
设置舵机的目标角度和运动速度
:param srv: 头, 颈, 右爪, 左爪, 右肩, 左肩, 右肘, 左肘, 右腕, 左腕
"""
speed = constrain(srv.speed, 0, 0xff)
buf = [0x55, 0xaa, 0x71, 0x18, 0x01]
data = [constrain(srv.head, -28, 22) + 128, srv.speed,
constrain(srv.neck, -50, 50) + 77, srv.speed,
constrain(srv.r_claw, -30, 0) + 100, srv.speed,
constrain(srv.l_claw, -45, 0) + 120, srv.speed,
90, srv.speed,
90, srv.speed,
constrain(srv.r_shoulder, 0, 70) + 90, srv.speed,
constrain(srv.l_shoulder, -70, 0) + 90, srv.speed,
constrain(srv.r_elbow, -75, 0) + 117, srv.speed,
constrain(srv.l_elbow, -75, 0) + 134, srv.speed,
constrain(srv.r_wrist, -60, 60) + 85, srv.speed,
constrain(srv.l_wrist, -60, 60) + 80, srv.speed]
buf.extend(data)
buf.append(solve_sum(buf))
if link.write(buf):
return True
return False