80 lines
2.6 KiB
Python
80 lines
2.6 KiB
Python
import time
|
||
|
||
class BaseMsg:
|
||
def __init__(self, ctrl, msg):
|
||
self.ctrl = ctrl
|
||
self.msg = msg
|
||
|
||
def stop_force(self):
|
||
self.msg.mode = 0
|
||
self.msg.gait_id = 0
|
||
self.msg.duration = 0
|
||
self.msg.life_count += 1
|
||
self.ctrl.Send_cmd(self.msg)
|
||
self.ctrl.Wait_finish(0, 0)
|
||
|
||
def stop(self, wait_time=0):
|
||
self.msg.mode = 11
|
||
self.msg.gait_id = 26
|
||
self.msg.vel_des = [0, 0, 0]
|
||
self.msg.duration = 10
|
||
self.msg.life_count += 1
|
||
self.ctrl.Send_cmd(self.msg)
|
||
if wait_time:
|
||
time.sleep(wait_time)
|
||
|
||
def stop_smooth(self, current_vel=None, steps=3, delay=0.2):
|
||
"""
|
||
平滑停止移动，通过逐步减小速度而不是强制停止
|
||
|
||
参数:
|
||
current_vel: 当前速度列表 [x, y, z]，如果为None则使用当前消息中的速度
|
||
steps: 减速步骤数，默认为3步
|
||
delay: 每步之间的延迟时间(秒)，默认为0.2秒
|
||
"""
|
||
|
||
# 如果没有提供当前速度，使用消息中的当前速度
|
||
if current_vel is None:
|
||
current_vel = self.msg.vel_des.copy() if hasattr(self.msg, 'vel_des') else [0, 0, 0]
|
||
|
||
# 保存当前消息的一些参数
|
||
current_mode = self.msg.mode
|
||
current_gait_id = self.msg.gait_id
|
||
|
||
# 检查是否有速度需要减小
|
||
if all(abs(v) < 0.01 for v in current_vel):
|
||
# 速度已经很小，直接停止
|
||
self.stop_force()
|
||
return
|
||
|
||
# 逐步减小速度
|
||
for i in range(1, steps + 1):
|
||
# 计算这一步的减速比例
|
||
ratio = (steps - i) / steps
|
||
|
||
# 计算新的速度
|
||
new_vel = [v * ratio for v in current_vel]
|
||
|
||
# 更新消息
|
||
self.msg.mode = current_mode
|
||
self.msg.gait_id = current_gait_id
|
||
self.msg.vel_des = new_vel
|
||
self.msg.duration = int(delay * 1000) # 毫秒
|
||
self.msg.life_count += 1
|
||
|
||
# 发送命令
|
||
self.ctrl.Send_cmd(self.msg)
|
||
|
||
# 等待这一步完成
|
||
time.sleep(delay)
|
||
|
||
# 最后发送一个零速度命令，确保完全停止
|
||
self.msg.mode = current_mode
|
||
self.msg.gait_id = current_gait_id
|
||
self.msg.vel_des = [0, 0, 0]
|
||
self.msg.duration = int(delay * 1000)
|
||
self.msg.life_count += 1
|
||
self.ctrl.Send_cmd(self.msg)
|
||
|
||
# 等待最后的命令完成
|
||
time.sleep(delay) |