ビジュアルサーボによる照準

1. limelightとドライブトレインだけで、正確かつ素早くロボットの照準を合わせることができます。
2. これらはすべて1時間以内に実現できます。

高フレームレートのビジョントラッキングを使用することで、ビジョンパイプラインを直接PID制御ループの「センサー」として使用し、ロボットやタレットを誘導することが可能になりました。このアイデアをテストするため、2017年のFRCロボットにlimelightを追加し、ドライブトレインとlimelightが報告するネットワークテーブルデータのみを使用してビジョンターゲットに照準を合わせるようにしました。

この例では、コルソンホイールを使用した6輪ドライブトレインを搭載した2017年のFRCロボットをテスト対象としました。このテストを行うためにロボットに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;
}

この時点で、ほぼ機能しました。ボタンを押している間、ロボットは自動的にターゲットの方向に回転します。ターゲットを動かすと、ロボットはターゲットを追跡して回転します。しかし、ダッシュボードのライブビデオフィードを見ると、大きな問題が1つありました：ロボットが必ずしもターゲットに完全に整列していないのです。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を適切に調整することで、ロボットは非常に正確かつ素早くターゲットに直接照準を合わせることができるようになります。