havoc420ubuntu
f484dab840
- Simplified arc_turn_around_hori_line function: - Removed smooth stop method call - Added print statements for debugging - Removed unused arc_turn_precise and arc_turn_around_hori_line_precise functions - Updated task_1.py to use simplified arc_turn_around_hori_line -
166 lines
6.0 KiB
Python
166 lines
6.0 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)
|
||
|
||
"""
|
||
专为旋转动作设计的平滑停止方法，通过更精细的步骤减小速度
|
||
|
||
参数:
|
||
current_vel: 当前速度列表 [x, y, w]，如果为None则使用当前消息中的速度
|
||
steps: 减速步骤数，默认为5步（比普通停止更多步骤）
|
||
delay: 每步之间的延迟时间(秒)，默认为0.1秒（更快的响应）
|
||
final_steps: 最后完全停止前的额外零速度指令次数，增强稳定性
|
||
"""
|
||
|
||
# 如果没有提供当前速度，使用消息中的当前速度
|
||
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
|
||
|
||
# 1. 先针对角速度进行单独减速，确保旋转先停稳
|
||
if len(current_vel) >= 3 and abs(current_vel[2]) > 0.05: # 角速度
|
||
# 首先只减小角速度，保持线速度
|
||
for i in range(1, steps + 1):
|
||
# 计算这一步的减速比例 - 角速度减速更快
|
||
ratio = (steps - i) / steps
|
||
ratio_w = ratio * ratio # 角速度使用平方递减，减速更快
|
||
|
||
# 计算新的速度，只改变角速度
|
||
new_vel = current_vel.copy()
|
||
new_vel[2] = current_vel[2] * ratio_w
|
||
|
||
# 更新消息
|
||
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)
|
||
|
||
# 2. 再减小整体速度
|
||
current_vel_no_w = current_vel.copy()
|
||
if len(current_vel) >= 3:
|
||
current_vel_no_w[2] = 0 # 角速度已减为0
|
||
|
||
for i in range(1, steps + 1):
|
||
# 计算这一步的减速比例
|
||
ratio = (steps - i) / steps
|
||
|
||
# 计算新的速度
|
||
new_vel = [v * ratio for v in current_vel_no_w]
|
||
if len(current_vel) >= 3:
|
||
new_vel[2] = 0 # 确保角速度为0
|
||
|
||
# 更新消息
|
||
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)
|
||
|
||
# 3. 发送多次零速度命令，确保完全停止
|
||
for _ in range(final_steps):
|
||
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) |