2.4.1 状态机设计 ​

1、状态定义: ​

本项目为机器人定义了四种状态：找球 —— 捡球 —— 找桶 —— 放球

#代码位置：setup.py

class RobotStatus(Enum):
    SEARCH = "search" # 找球模式
    PICK = "pick" # 捡球模式
    FIND_BUCKET = "find_bucket" # 找桶模式
    PUT_BALL = "put_ball" # 放球模式

2、各状态的行为及代码实现 ​

a. SEARCH（找球） ​

机器人自主搜索网球，使用摄像头检测球的位置。并向网球的位置移动。

#代码位置：main.py

elif get_robot_status() == RobotStatus.SEARCH:
    move_action = move_controller(direction, result)

b. PICK（捡球） ​

根据预设的10步夹球的动作序列 CATCH_ACTION ，进行捡球的动作。

python 
#代码位置：main.py

if get_robot_status() == RobotStatus.PICK:
    if get_control_mode() == RobotControlModel.ACT:
        arm_action = arm_controller(robot)
    else:
        arm_action, current_x, current_y = p_control_loop(CATCH_ACTION[command_step],
                                                          current_x, current_y, current_obs, kp=0.5)
        # ... 执行夹球动作序列 ...

c. FIND_BUCKET（找桶） ​

寻找红色桶，准备放球

python 
#代码位置：main.py

elif get_robot_status() == RobotStatus.FIND_BUCKET:
    move_action = move_controller_for_bucket(direction, result)

d. PUT_BALL（放球） ​

打开夹爪放球，然后重置机器人状态

#代码位置：main.py

elif get_robot_status() == RobotStatus.PUT_BALL:
    arm_action, current_x, current_y = p_control_loop(("gripper", 50),
                                                      current_x, current_y, current_obs, kp=0.5)
    set_robot_status(RobotStatus.SEARCH)
    reset_robot()

3、状态转换流程 ​

SEARCH → (检测到球且稳定) → PICK → (抓取完成) → FIND_BUCKET → (找到桶) → PUT_BALL → SEARCH 通过函数 set_robot_status(RobotStatus.xx状态) 来实现状态转换。

2.4.2 main.py(主函数) ​

1、初始化 ​

(1)初始化配置，并连接机器人

c 
init_app()  #初始化全局配置和状态机  
init_robot()  #初始化机器人  
robot = get_robot()  #获取刚才初始化好的机器人实例  
direction = get_direction()  #获取运动方向控制器  
robot.connect() #连接机器人

(2)机械臂回到预设位置，在终端会打印出初始关节角度

print("Reading initial joint angles...")
start_obs = robot.get_observation()
…………
return_to_start_position(robot, start_obs, get_target_positions(), 0.9, get_fps()) 
…………
command_step = 0    # 当前执行到 CATCH_ACTION 中的第几步

2、主循环 ​

四个阶段：感知、检测、决策、执行

a. 感知 ​

获取观测数据,包括机器人的状态和摄像头画面等

current_obs = robot.get_observation()   #获取机器人当前状态
frame = current_obs["front"]            #获取摄像头画面frame

b. 检测 ​

（1）算法的选择
本项目有两个检测目标——网球和红桶，两种检测的算法不同。在循环开始时要选择执行哪种算法。
判断先找球还是先找桶：判断夹爪是否夹住网球（根据夹爪位置判断）。
如果夹着球，则执行找红桶的检测算法：

result = get_red_bucket_local(frame)

如果没有夹着球，则执行找网球的算法：

result = yolo_infer(frame)

（2）检测结果显示
当 hardware_mode 为 normal 时，把检测结果的画面上弹窗显示；按 q 退出程序。

if get_hardware_mode() == 'normal': 
    for box in result:
        x, y, w, h = box.x, box.y, box.w, box.h
…………
    cv2.imshow("frame", frame)
    key = cv2.waitKey(1) & 0xFF
    if key == ord('q'):
        break

c. 根据状态选择控制策略 ​

d. 执行 ​

发送动作：

robot.send_action({**arm_action, **move_action})

4、FPS控制的实现方式 ​

（1）FPS配置获取
在 setup.py 中定义 get_fps()
FPS值从 config.yaml 配置文件读取：fps: 20

（2）主循环时间控制

# 主循环开始计时
t0 = time.perf_counter()

# ... 执行所有控制逻辑（感知、决策、执行）...

# FPS控制：计算并等待剩余时间
busy_wait(max(1.0 / get_fps() - (time.perf_counter() - t0), 0.0))

2.4.3 配置 ​

（setup.py文件）

1、全局配置的单例模式管理 ​

通过全局状态标志 _is_initialized: bool = False 确保配置只初始化一次。

def init_app():
    global _is_initialized, _hardware_mode, _left, _top, _right, _bottom, _port, _log_level, _target_w, _target_h, _robot_status, _control_mode, _fps
    if _is_initialized:
        return  # 已初始化则直接返回，避免重复初始化
    print("Initializing...")

2、配置项的设置 ​

#代码位置：setup.py > init_app()

_   hardware_mode = config['hardware_mode']     #yolo的硬件运行模式
    height, width = 480, 640

    _target_w = min(height, width) // 3         #目标框参数
    _target_h = min(height, width) // 3
    _left = max(0, (width - _target_w) // 2)
    _top = max(0, (height - _target_h) // 2)
    _right = min(width, _left + _target_w)
    _bottom = min(height, _top + _target_h)

    _port = config['port']                      #串口设备路径
    _log_level = config['log_level']
    _robot_status = RobotStatus.SEARCH          #状态机的状态

    if config['control_mode'] == 'act':         #机械臂控制策略
        _control_mode = RobotControlModel.ACT
    else:
        _control_mode = RobotControlModel.INVERSE
    
    _fps = config['fps']

    _is_initialized = True