havoc420ubuntu
214cc13c65
- Set msg.duration to 0 in move_to_hori_line to wait for next command - Update task_1 execution flow based on QR code result - Modify BaseMsg.stop() to use gait_id 27 for stopping - Comment out smooth stop and use normal stop in move_to_hori_line
64 lines
2.1 KiB
Python
64 lines
2.1 KiB
Python
import time
|
|
|
|
class BaseMsg:
|
|
def __init__(self, ctrl, msg):
|
|
self.ctrl = ctrl
|
|
self.msg = msg
|
|
|
|
def stop_force(self):
|
|
"""
|
|
强制停止，但是大部分场景不好用，容易导致 robot 崩坏。
|
|
"""
|
|
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=200):
|
|
self.msg.mode = 11
|
|
self.msg.gait_id = 27
|
|
self.msg.vel_des = [0, 0, 0]
|
|
self.msg.duration = wait_time
|
|
self.msg.life_count += 1
|
|
self.ctrl.Send_cmd(self.msg)
|
|
if wait_time:
|
|
time.sleep(wait_time / 1000)
|
|
|
|
def stop_smooth(self, wait_time=300):
|
|
"""平滑停止，采用逐渐减速的方式实现更柔和的停止过程"""
|
|
# 获取当前速度
|
|
current_vel = self.msg.vel_des.copy() if hasattr(self.msg, 'vel_des') and self.msg.vel_des else [0, 0, 0]
|
|
|
|
# 如果当前速度已经为零，直接调用普通停止
|
|
if all(abs(v) < 0.01 for v in current_vel):
|
|
self.stop(wait_time)
|
|
return
|
|
|
|
# 计算减速步数和每步减速量
|
|
steps = 5 # 减速分5步完成
|
|
step_time = wait_time / steps
|
|
vel_step = [v / steps for v in current_vel]
|
|
|
|
# 逐步减速
|
|
for i in range(steps):
|
|
# 计算当前步骤的目标速度
|
|
target_vel = [current_vel[j] - vel_step[j] * (i + 1) for j in range(len(current_vel))]
|
|
|
|
# 发送减速命令
|
|
self.msg.mode = 11
|
|
self.msg.gait_id = 26
|
|
self.msg.vel_des = target_vel
|
|
self.msg.duration = step_time
|
|
self.msg.life_count += 1
|
|
self.ctrl.Send_cmd(self.msg)
|
|
time.sleep(step_time / 1000)
|
|
|
|
# 最后确保完全停止
|
|
self.msg.vel_des = [0, 0, 0]
|
|
self.msg.duration = step_time
|
|
self.msg.life_count += 1
|
|
self.ctrl.Send_cmd(self.msg)
|
|
time.sleep(step_time / 1000)
|
|
|