Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

test(task_1): adjust go_straight parameters and comment out angle correction

Browse Source 
- Comment out angle correction logic in go_straight function
- Adjust go_straight parameters in task_1
- Add success message for horizontal line calibration
- Update main function to include task_test
- Modify marker request response format
This commit is contained in:
havoc420ubuntu 2025-05-22 04:27:22 +00:00
parent 1ff2d22d4f
commit 1d18ec1e56
6 changed files with 54 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
base_move/go_straight.py
View File

@ -134,24 +134,24 @@ def go_straight(ctrl, msg, distance, speed=0.5, observe=False):
ctrl.Send_cmd(msg)
# 如果偏离初始方向，进行角度修正
if abs(yaw_error) > angle_correction_threshold:
# 计算角速度修正值，偏差越大修正越强
angular_correction = -yaw_error * 0.5 # 比例系数可调整
# 限制最大角速度修正
angular_correction = max(min(angular_correction, 0.2), -0.2)
# if abs(yaw_error) > angle_correction_threshold:
# # 计算角速度修正值，偏差越大修正越强
# angular_correction = -yaw_error * 0.5 # 比例系数可调整
# # 限制最大角速度修正
# angular_correction = max(min(angular_correction, 0.2), -0.2)
if observe and abs(angular_correction) > 0.05:
warning(f"方向修正: 偏差 {math.degrees(yaw_error):.1f}度，应用角速度 {angular_correction:.3f}rad/s", "角度")
# if observe and abs(angular_correction) > 0.05:
# warning(f"方向修正: 偏差 {math.degrees(yaw_error):.1f}度，应用角速度 {angular_correction:.3f}rad/s", "角度")
# 应用角速度修正
msg.vel_des[2] = angular_correction
msg.life_count += 1
ctrl.Send_cmd(msg)
elif msg.vel_des[2] != 0:
# 如果方向已修正，重置角速度
msg.vel_des[2] = 0
msg.life_count += 1
ctrl.Send_cmd(msg)
# # 应用角速度修正
# msg.vel_des[2] = angular_correction
# msg.life_count += 1
# ctrl.Send_cmd(msg)
# elif msg.vel_des[2] != 0:
# # 如果方向已修正，重置角速度
# msg.vel_des[2] = 0
# msg.life_count += 1
# ctrl.Send_cmd(msg)
if observe and current_time - start_time > 1 and (current_time % 0.5 < position_check_interval):
debug(f"已移动: {distance_moved:.3f}米, 目标: {abs_distance:.3f}米 (完成: {completion_ratio*100:.1f}%)", "距离")

2
base_move/move_base_hori_line.py
View File

@ -296,6 +296,8 @@ def go_straight_by_hori_line(ctrl, msg, distance=0.5, speed=0.5, observe=False):
if not aligned:
error("无法校准到横向线水平，停止移动", "停止")
return False
else:
success("校准完成", "完成")
go_straight(ctrl, msg, distance=distance, speed=speed, observe=observe)

12
main.py
View File

@ -23,6 +23,8 @@ from utils.base_msg import BaseMsg
from task_1.task_1 import run_task_1
from task_5.task_5 import run_task_5
from task_test.task_test import run_task_test
pass_marker = True
def main():
@ -34,13 +36,15 @@ def main():
try:
print("Recovery stand")
Ctrl.base_msg.stand_up()
Ctrl.base_msg.stop() # BUG 垃圾指令 for eat
# time.sleep(100) # TEST
run_task_1(Ctrl, msg)
# run_task_5(Ctrl, msg)
# run_task_test(Ctrl, msg)
# time.sleep(100)
except KeyboardInterrupt:
@ -104,14 +108,14 @@ class Robot_Ctrl(object):
self.calibration_offset = self.odo_msg.xyz
self.is_calibrated = True
print(f"校准完成，基准值: {self.calibration_offset}")
# 将接收到的 odo 数据减去校准基准值
# # 将接收到的 odo 数据减去校准基准值
self.odo_msg.xyz = [
self.odo_msg.xyz[0] - self.calibration_offset[0],
self.odo_msg.xyz[1] - self.calibration_offset[1],
self.odo_msg.xyz[2] - self.calibration_offset[2]
]
# if self.odo_msg.timestamp % 100 == 0:
# print(self.odo_msg.xyz)
# if self.odo_msg.timestamp % 50 == 0:
# print(self.odo_msg.rpy)
def odo_reset(self):
self.calibration_offset = self.odo_msg.xyz

17
task_1/task_1.py
View File

@ -25,9 +25,10 @@ def run_task_1(ctrl, msg):
info('开始执行任务1...', "启动")
# TEST better align
aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
print(aligned)
return
# aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
# print(aligned)
# go_straight(ctrl, msg, -10, observe=observe)
# return
# v2
section('任务1-1：转弯并扫描QR码', "移动")
@ -57,7 +58,7 @@ def run_task_1(ctrl, msg):
ctrl=ctrl,
msg=msg,
angle_deg=-170, # 负值表示右转
target_distance=0.5,
target_distance=0.8,
#
pass_align=True,
observe=observe,
@ -72,11 +73,11 @@ def run_task_1(ctrl, msg):
return
section('任务1-4：直线移动', "移动")
move_distance = 0.3
move_distance = 0.5
move_speed = 0.5
if not direction:
# TODO to check
go_straight_by_hori_line(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)
go_straight_by_hori_line(ctrl, msg, distance=move_distance, speed=move_distance, observe=observe)
else:
go_straight(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)
@ -92,7 +93,7 @@ def run_task_1(ctrl, msg):
ctrl.base_msg.stand_up()
section('任务1-6：返回', "移动")
go_straight(ctrl, msg, distance=-(move_distance + res['radius'] * 2), speed=0.3, observe=observe)
go_straight(ctrl, msg, distance=-(move_distance + res['radius'] + 0.2), speed=0.3, observe=observe)
# turn and back
turn_degree(ctrl, msg, degree=-90)
@ -102,7 +103,7 @@ def run_task_1(ctrl, msg):
ctrl=ctrl,
msg=msg,
radius=res['radius'] * 2,
angle_deg=90,
angle_deg=85,
#
pass_align=True,
observe=observe

15
task_test/task_test.py Normal file
View File

@ -0,0 +1,15 @@
import time
import sys
import os
# 添加父目录到路径，以便能够导入utils
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from task_5.detect_arrow_direction import ArrowDetector
from base_move.turn_degree import turn_degree
def run_task_test(ctrl, msg):
ctrl.base_msg.stop()
turn_degree(ctrl, msg, 90)
turn_degree(ctrl, msg, 90)

5
utils/marker_client.py
View File

@ -36,7 +36,10 @@ class MarkerRunner:
def send_request(self, x, y, z, color, observe=False):
if self.pass_flag:
return "pass flag."
return {
'success': True,
'message': "Pass"
}
response = self.marker_client.send_request(x, y, z, color)
if observe:
self.marker_client.get_logger().info(