MFXWW2头像
Vex AIM 机器人开发笔记封面图

Vex AIM 机器人开发笔记

一. 常见概念

1.“回归角”

在机器人每次运行程序时,回归角会被设为0度,这个角度不是机器人的dashboard里边显示的那个。

如果要让dashboard显示回归角,可以在运行前双击角度显示盘将当前角度设为“显示上的”0度,再运行程序,这样就可以大概显示运行时的实时回归角了

2."移动‘正’与‘左’转"

这两个代码块用于“开始移动”与“开始转动”,运行到这里时机器人会开始移动/转动,同时继续执行下一行代码(积木)。

可以用 “移动中?” “转弯中?” 判断当前的运动状态,也可以用 "停止所有移动" 来停止。

3.“有‘运动球’?”

这个条件,我一开始把他当成了“视野里有没有运动球”,其他人也是这么想的。

但是要判断“视野里有没有运动球”应该在 “从AI视觉仪获取‘运动球’数据” 之后,立即使用“AI视觉仪对象存在?”判断视野里有没有。

而这个条件“有‘运动球’?”,实际上指的是机器人有没有拿到球(在自己的磁铁上)。

如果不使用这个条件,就要面临这样的判断条件:

150 < ball_x < 170 and 210 < ball_y < 220

这个条件太长了(不容易看懂),而且没有官方给的条件准。

其实 “有‘运动球’?” 应该是一个翻译错误的说,本意不是这样的

二. 开发技巧

1. 快速扫视

官方给的“右转直到发现运动球”宏对机器人转动的速度有着严苛的要求,转快了会掉识别。经过实测,稳定的速度在32%左右。

但是这个速度太慢了。所以替代方案是转一段 + 扫一下,转的过程中不识别,所以可以全速转。

degree_per_seek = 70
while not vision_data[vision_data_index].exists:
    robot.turn_for(RIGHT, degree_per_seek)
    vision_data = robot.vision.get_data(SPORTS_BALL)
    if robot.has_sports_ball():
        find_goal()
    wait(5, MSEC)
if vision_data[vision_data_index].centerX < 160 - pardon_x:
    
    # Turn right until sports ball is detected
    robot.turn(RIGHT)
    while True:
        vision_data = robot.vision.get_data(SPORTS_BALL)
    
        if vision_data[0].exists:
            # Turn to the object by adding your current heading and the vision bearing offset
            robot.turn_to(robot.inertial.get_heading() + vision_data[0].bearing)
            break
        wait(20, MSEC)
    
elif vision_data[vision_data_index].centerX > 160 + pardon_x:
    
    # Turn left until sports ball is detected
    robot.turn(LEFT)
    while True:
        vision_data = robot.vision.get_data(SPORTS_BALL)
    
        if vision_data[0].exists:
            # Turn to the object by adding your current heading and the vision bearing offset
            robot.turn_to(robot.inertial.get_heading() + vision_data[0].bearing)
            break
        wait(20, MSEC)
    
else:
    pass
robot.move_at(0)
attempt_count = 0
while True:
    vision_data = robot.vision.get_data(SPORTS_BALL)
    if not (vision_data[vision_data_index].exists and (160 - pardon_x) < vision_data[vision_data_index].centerX < (160 + pardon_x)):
        attempt_count = attempt_count + 1
        print(str("LOSE SIGHT OF BALL - ATTEMPT CNT") + str(attempt_count))
        if attempt_count > 100:
            # 丢球,朝着球消失时的方向,再找
            robot.stop_all_movement()
            rightseeking = last_ballX > 160
            break
    elif robot.has_sports_ball():
        # 持球,尝试面向球门同时持续监测是否丢球
        robot.stop_all_movement()
        find_goal()
        break
    else:
        attempt_count = 0
        last_ballX = vision_data[vision_data_index].centerX
        print(str("RECATCH SIGHT :-)") + str(attempt_count))
    wait(5, MSEC)

2. 全场定位

双机可以实现简要的全场定位。一台机器通过视觉计算己方机器人与球相对于自己的坐标,发给场上的机器人之后,场上的机器人就可以直接面向球(的大致方向)而不用全靠自己转圈找球了。

(前提:机器人的初始归位角相同)

测距(球和车不能互换,同距 5 cm 时球框宽只有车的 30%)

队友:d = \frac{1420}{width_{px}} \quad (width \ge 59)

球:d = \frac{446}{centerY - 53.4} \quad (centerY > 61)

机体系坐标(cm,右正)

x = d \cdot (\text{bearing}^\circ \times 0.017453), \qquad y = d

队友算方位(主机的原点就是坐标原点,所以要减去自己的位置)

\Delta x = bx - rx,\quad \Delta y = by - ry\\

\theta = 90^\circ - \operatorname{atan2}(\Delta y,\ \Delta x)\\

\varphi = \big((\theta - h + 180) \bmod 360\big) - 180\\ h^* = (h + \varphi + 360) \bmod 360

\theta 是球在零位航向下的方位角,$h$ 是当前航向,$\varphi$ 是带符号的最短转向量,$h^*$ 交给 turn_to。

主机代码

# ---- ① 球:先取视觉、先上路 ----
vision_data = robot.vision.get_data(SPORTS_BALL)
if vision_data[vision_data_index].exists:
    ball_cy = vision_data[vision_data_index].centerY
    if ball_cy > 61:                                  # 守卫用原始量,不用算出的距离
        ball_dist_cm = 446 / (ball_cy - 53.4)
        ball_bearing = vision_data[vision_data_index].bearing
        ball_x_cm = ball_dist_cm * (ball_bearing * 0.017453)   # sin 的一次近似
        ball_y_cm = ball_dist_cm
    else:
        ball_x_cm = -1; ball_y_cm = -1                # 太远
else:
    ball_x_cm = -1; ball_y_cm = -1                    # 没识别
robot.link.send_message("BX", ball_x_cm)
robot.link.send_message("BY", ball_y_cm)              # BY = 帧终结标记

# ---- ② 队友:近区 AIM_ROBOT → 远区 BLUE_HAT 回退 ----
vision_data = robot.vision.get_data(AIM_ROBOT)
rob_src = 0
if vision_data[vision_data_index].exists:
    rob_w = vision_data[vision_data_index].width
    if rob_w >= 59:                                   # 5~25 cm
        rob_dist_cm = 1420 / rob_w
        rob_bearing = vision_data[vision_data_index].bearing
        rob_src = 1
if rob_src == 0:
    vision_data = robot.vision.get_data(BLUE_HAT)
    if vision_data[vision_data_index].exists:
        rob_w = vision_data[vision_data_index].width
        if rob_w >= 20 and rob_w <= 46:               # 31~71 cm,26~38 cm 是死区
            rob_dist_cm = 1420 / rob_w
            rob_bearing = vision_data[vision_data_index].bearing
            rob_src = 2

# ---- ③ 去噪:野值门 → 一阶低通 → 连续 3 帧被拒则重置 ----
if rob_src > 0:
    rob_x_cm = rob_dist_cm * (rob_bearing * 0.017453)
    rob_y_cm = rob_dist_cm
    if my < 0:                                        # 首帧直接吸收
        mx = rob_x_cm; my = rob_y_cm; grev = 0; pok = 1
    elif (rob_y_cm - my) < 25 and (rob_y_cm - my) > -25:
        mx = mx + 0.6 * (rob_x_cm - mx)
        my = my + 0.6 * (rob_y_cm - my)
        grev = 0; pok = 1
    else:                                             # 超出野值门
        grev = grev + 1
        if grev >= 3:                                 # 不是野值,是真的挪了
            mx = rob_x_cm; my = rob_y_cm; grev = 0; pok = 1
        else:
            pok = 0                                   # 本帧当「没看见」
    if pok == 1:
        rob_x_cm = mx; rob_y_cm = my
    else:
        rob_x_cm = -1; rob_y_cm = -1
else:
    rob_x_cm = -1; rob_y_cm = -1; grev = 0

# ---- ④ 帧尾:发 PX/PY → 等 ACK 50 ms → 静默 20 ms ----
t0 = timer.time()
robot.link.send_message("PX", rob_x_cm)
robot.link.send_message("PY", rob_y_cm)
while got == 0 and (timer.time() - t0) < 0.05:
    if robot.link.is_message_available():
        robot.link.get_message_and_data()
        if robot.link.latest_message == "ACK":
            got = 1; ack = ack + 1
robot.wait(0.02, SECONDS)

从机代码

# ---- ① 一次收完积压(每帧 4 条,40 条/秒)----
while robot.link.is_message_available():
    robot.link.get_message_and_data()
    if robot.link.latest_message == "PX":
        rx_cm = robot.link.latest_data
    elif robot.link.latest_message == "PY":
        ry_cm = robot.link.latest_data
    elif robot.link.latest_message == "BX":
        bx_cm = robot.link.latest_data
    elif robot.link.latest_message == "BY":
        by_cm = robot.link.latest_data
        frames = frames + 1
        robot.link.send_message("ACK", frames)        # 先回 ACK 再转向
        # ---- ② 位置互补滤波:只在有 PX/PY 时修 ----
        if rx_cm > 0 and ry_cm > 0:
            ox = robot.get_x_position(); oy = robot.get_y_position()
            ex = rx_cm * 10 - ox; ey = ry_cm * 10 - oy   # cm → mm
            if abs(ex) < 400 and abs(ey) < 400 and (abs(ex) > 30 or abs(ey) > 30):
                robot.set_xy_position(ox + 0.6 * ex, oy + 0.6 * ey)
                ox = ox + 0.6 * ex; oy = oy + 0.6 * ey   # 本地同步,不重读里程计
        # ---- ③ 算球方位 → 最短转向量 ----
        if by_cm > 0 and ry_cm > 0:
            ball_dx = bx_cm - rx_cm
            ball_dy = by_cm - ry_cm
            ball_d2 = ball_dx * ball_dx + ball_dy * ball_dy
            if ball_d2 > 9:                           # <3 cm 时角度没意义
                tgt_deg = 90 - math.atan2(ball_dy, ball_dx) / math.pi * 180
                hdg_deg = robot.inertial.get_heading()
                turn_deg = ((tgt_deg - hdg_deg) + 180) % 360 - 180
                aim_deg  = ((hdg_deg + turn_deg) + 360) % 360
                if not robot.is_turn_active():
                    if turn_deg > 10 or turn_deg < -10:    # 10° 死区吞掉测距抖动
                        robot.turn_to(aim_deg, wait=False)  # 不等待 → 不堵收消息

转载自 CSDN-专业IT技术社区

原文链接:https://blog.csdn.net/2504_92903686/article/details/171108278

文章来源转载

评论

评论列表

微信小程序
QQ小程序

关于作者

点赞数:0
关注数:0
粉丝:0
文章:0
关注标签:0
加入于:--