移動距離の計算について
『移動距離に関する制御のコード』では「他のファイルで計算された」と表記したが、その箇所に関しての説明をここで行う。
移動距離の計算は「TelloRequestCalculation02.cs」で行われており、前後移動、左右移動、旋回の3つについての移動距離の計算を行う。
前提の説明
「TelloRequestCalculation02.cs」で値を送る前に、「TelloController1122.cs」ではARマーカーとの距離の値を取得する。
円中心の三角形がARマーカーを付けた追従対象で、左下にあるものがドローンとする。この時、下のコードから3つのパラメーターを受け取る。
double targetPosX = textureSender.estimater.position[0]; // 左右方向の距離
double targetPosZ = textureSender.estimater.position[2]; // 前後方向の距離
double targetRotY = textureSender.estimater.rotation[1]; // ARマーカーとの角度
targetPosX → ドローンとARマーカーまでの横方向の距離
targetPosZ → ドローンとARマーカーまでの前方向の距離
targetRotY → ドローンから見たARマーカーの角度(右方向に+、左方向に-)
この時、取得できる単位は X,Y は[m]、 Y は[°]であることに気を付ける。
「TelloRequestCalculation02.cs」内の説明
まず、TelloとARマーカーがどれだけ離れて追従するかを設定する。
// 単位は[cm]
private static double targetDistance = 2;
この場合2m離れて追従する設定となり、この値が基準値となって以下の計算を行う。
単位は[cm] と書いてあるが本当は 単位は[m]であることに気を付けて下さい！
なお、これより以下の計算では、角度はラジアンで扱う点に注意する。
-
x軸方向(左右)の計算
式：$MoveX=targetPosX-targetDistance×\sin(targetRotY)$
//x軸方向(左右)の計算
public static double CalculateRequestMoveX(double targetPosX, double targetRotY)
{
// 引数は単位が[m]
return targetPosX - targetDistance * Math.Sin(targetRotY * Mathf.Deg2Rad);
}
-
y軸方向(前後)の計算
式：$MoveZ=targetPosZ-targetDistance×\cos(targetRotY)$
//y軸方向(前後)の計算
public static double CalculateRequestMoveY(double targetPosZ, double targetRotY)
{
// 引数は単位が[m]
return targetPosZ - targetDistance * Math.Cos(targetRotY * Mathf.Deg2Rad);
}
-
旋回の計算
Yの大きさによって旋回の向きを変える
public static double CalculateRequestRoteY(double targetRotY)
{
return (targetRotY <= 180) ? targetRotY * Mathf.Deg2Rad : (targetRotY - 360) * Mathf.Deg2Rad;
}