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

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

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

この例では、テスト対象は2017年のFRCロボットで、コルソンホイールを使用した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;
}

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