使用视觉伺服进行瞄准

1. 您可以仅使用 Limelight 和底盘来准确快速地瞄准您的机器人。
2. 这一切可以在不到 1 小时内完成。

使用高帧率视觉追踪，现在可以直接将视觉管道作为 PID 控制回路中的"传感器"来引导您的机器人或炮塔。为了测试这个想法，我们在 2017 年的 FRC 机器人上添加了一个 Limelight，并仅使用底盘和 Limelight 报告的网络表数据来使其瞄准视觉目标。

在这个例子中，我们的测试对象是一台 2017 年的 FRC 机器人，它使用带有 Colson 轮的 6 轮底盘。这是我们为进行此测试而在机器人上添加 Limelight 的照片。

接下来，我们在机器人上添加了一些代码，每当驾驶员按住操纵杆上的按钮时就会运行。这台机器人使用"坦克"式驾驶，因此 OperatorControl 函数会生成一个 'left_command' 值和一个 'right_command' 值来控制底盘的左右两侧。在正常控制代码之后，我们添加了这样一段代码：

float Kp = -0.1f;  // 比例控制常数

std::shared_ptr<NetworkTable> table = NetworkTable::GetTable("limelight");
float tx = table->GetNumber("tx");

if (joystick->GetRawButton(9))
{
	float heading_error = tx;
    steering_adjust = Kp * tx;
    
	left_command+=steering_adjust;
    right_command-=steering_adjust;
}

一开始，这基本上就能工作了。每当您按住按钮时，机器人会自动转向目标方向。如果您移动目标，机器人会转向跟随目标。然而，通过仪表板上的实时视频画面，我们可以看到有一个大问题：机器人并不总是能完全对准目标。在一些目标较小的比赛中（如 2016 年和 2017 年），这还不够好。

到目前为止，我们实现的是一个简单的比例控制回路。我们计算了航向误差并将其乘以一个常数，从而产生一个与误差成比例的电机命令。当误差趋近于零时，我们的命令也会趋近于零。问题是机器人尝试转向时存在很大的摩擦力。非常小的命令根本无法转动机器人。在小角度时，命令可能变得太小而无法实际移动机器人。您可能会发现，当您从较大的瞄准误差开始时，机器人能很好地到达目标，但如果您从非常接近的位置开始，它就根本无法瞄准。

有几种方法可以解决这个问题，但这里有一个非常简单的解决方案。我们使用了"最小命令"的概念。如果误差大于某个阈值，只需在电机命令中添加一个常数，该常数大致代表机器人实际移动所需的最小功率（实际上您需要使用比这稍小一点的值）。新代码如下所示：

float Kp = -0.1f;
float min_command = 0.05f;

std::shared_ptr<NetworkTable> table = NetworkTable::GetTable("limelight");
float tx = table->GetNumber("tx");

if (joystick->GetRawButton(9))
{
	float heading_error = -tx;
	float steering_adjust = 0.0f;
	if (Math.abs(heading_error) > 1.0) 
	{
		if (heading_error < 0) 
		{
			steering_adjust = Kp*heading_error + min_command;
		} 
		else 
		{
			steering_adjust = Kp*heading_error - min_command;
		}
	} 
	left_command += steering_adjust;
	right_command -= steering_adjust;
}

注意，如果您将 Kp 或 min_command 设置得太高，您的机器人可能会变得不稳定，并且在超过目标时来回振荡：

经过对 Kp 和 min_command 的一些调整，您的机器人应该能够非常准确和快速地直接瞄准目标。