一. 常见概念
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%)
队友:
球:
机体系坐标(cm,右正)
队友算方位(主机的原点就是坐标原点,所以要减去自己的位置)
是球在零位航向下的方位角,
是当前航向,
是带符号的最短转向量,
交给 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





