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

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

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

この例では、テスト対象は2017年のFRCロボットで、コルソンホイールを使用した6輪ドライブトレインを採用しています。以下は、このテストを行うためにロボットにライムライトを追加している様子です。

次に、ドライバーがジョイスティックのボタンを押している間に実行されるコードをロボットに追加しました。このロボットは「タンク」スタイルの駆動を使用していたため、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を適切に調整することで、ロボットが非常に正確かつ素早くターゲットに直接照準を合わせることができるはずです。