/// <summary>
/// 左右のホイールの移動量を、X,Y座標に変換
/// </summary>
/// <param name="resXYWR">右車輪のX,Y座標</param>
/// <param name="resXYWL">左車輪のX,Y座標</param>
/// <param name="resAng">現在の向き</param>
/// <param name="reWR">右車輪のREncoderの値 絶対値</param>
/// <param name="reWL">左車輪のREncoderの値 絶対値</param>
/// <param name="reOldWR">右車輪のREncoderの値 絶対値 (前回値)</param>
/// <param name="reOldWL">左車輪のREncoderの値 絶対値 (前回値)</param>
public static void CalcWheelPlotXY(ref PointD resXYWR, ref PointD resXYWL, ref double resAng, double reWR, double reWL, double reOldWR, double reOldWL)
{
double mov;
double deg = REncodeDataToDegree(reWR, reWL);
double ang = (deg-90.0) * Math.PI / 180.0;
// 右車輪
mov = WheelRotateToLength(reWR - reOldWR);
resXYWR.X += (mov * -Math.Cos(ang));
resXYWR.Y += (mov * Math.Sin(ang));
// 左車輪
mov = WheelRotateToLength(reWL - reOldWL);
resXYWL.X += (mov * -Math.Cos(ang));
resXYWL.Y += (mov * Math.Sin(ang));
resAng = deg * Math.PI / 180.0;
}
/// <summary>
/// 左右のホイールの移動量を、X,Y座標に変換
/// </summary>
/// <param name="xyWR">右車輪のX,Y座標</param>
/// <param name="xyWL">左車輪のX,Y座標</param>
/// <param name="reWR">右車輪のREncoderの値 絶対値</param>
/// <param name="reWL">左車輪のREncoderの値 絶対値</param>
public static void CalcWheelPlotXY(out PointD[] xyWR, out PointD[] xyWL, double[] reWR, double[] reWL )
{
xyWR = new PointD[reWR.Length];
xyWL = new PointD[reWR.Length];
// 初期位置は上向き
// 動力のある左の車輪が0,0基準
xyWR[0] = new PointD();
xyWL[0] = new PointD();
xyWR[0].X = 500; xyWR[0].Y = 0;
xyWL[0].X = 0; xyWL[0].Y = 0;
// MakeLine
for (int i = 1; i < reWR.Length; i++)
{
double mov;
double deg = REncodeDataToDegree(reWR[i], reWL[i]) - 90.0;
double ang = deg * Math.PI / 180.0;
xyWR[i] = new PointD();
xyWL[i] = new PointD();
// 右車輪
mov = WheelRotateToLength(reWR[i] - reWR[i - 1]);
xyWR[i].X = xyWR[i - 1].X + (mov * -Math.Cos(ang));
xyWR[i].Y = xyWR[i - 1].Y + (mov * Math.Sin(ang));
// 左車輪
mov = WheelRotateToLength(reWL[i] - reWL[i - 1]);
xyWL[i].X = xyWL[i - 1].X + (mov * -Math.Cos(ang));
xyWL[i].Y = xyWL[i - 1].Y + (mov * Math.Sin(ang));
}
//x11 = x1 * Math.cos(alpha) - y1 * Math.sin(alpha);
//y11 = x1 * Math.sin(alpha) + y1 * Math.cos(alpha);
}
/// <summary>
/// Limited Slip Plot
/// </summary>
/// <param name="xyWR"></param>
/// <param name="xyWL"></param>
/// <param name="OreWR"></param>
/// <param name="OreWL"></param>
/// <param name="lspLv"></param>
/// <param name="lsp"></param>
public static void CalcWheelPlotXY_LSP(out PointD[] xyWR, out PointD[] xyWL, double[] OreWR, double[] OreWL, int lspLv, double lsp)
{
xyWR = new PointD[OreWR.Length];
xyWL = new PointD[OreWR.Length];
// 初期位置は上向き
// 動力のある左の車輪が0,0基準
xyWR[0] = new PointD();
xyWL[0] = new PointD();
xyWR[0].X = 500; xyWR[0].Y = 0;
xyWL[0].X = 0; xyWL[0].Y = 0;
double[] reWR = new double[OreWR.Length];
double[] reWL = new double[OreWR.Length];
// Copy Work
{
for (int i = 0; i < reWR.Length; i++)
{
reWR[i] = OreWR[i];
reWL[i] = OreWL[i];
}
}
// Limited Slip Pulse
{
int lv = lspLv; // Lv10 , lsp 0.52
for (int i = lv; i < reWR.Length; i++)
{
if ((reWR[i] - reWR[i - lv]) / (reWL[i] - reWL[i - lv]) < lsp)
{
double def = (reWR[i] - reWR[i - lv]);
double odef = ((reWL[i] - reWL[i - lv]) - def) / (double)lv;
/*
for (int n = 1; n < lv; n++)
{
reWL[i - lv + n] = reWL[i - lv] + (def / (double)lv);
}
*/
for (int n = (i - lv); n < reWR.Length; n++)
{
reWL[n] -= odef;
}
}
}
}
// MakeLine
for (int i = 1; i < reWR.Length; i++)
{
double mov;
double deg = REncodeDataToDegree(reWR[i], reWL[i]) - 90.0;
double ang = deg * Math.PI / 180.0;
xyWR[i] = new PointD();
xyWL[i] = new PointD();
// 右車輪
mov = WheelRotateToLength(reWR[i] - reWR[i - 1]);
xyWR[i].X = xyWR[i - 1].X + (mov * -Math.Cos(ang));
xyWR[i].Y = xyWR[i - 1].Y + (mov * Math.Sin(ang));
// 左車輪
mov = WheelRotateToLength(reWL[i] - reWL[i - 1]);
xyWL[i].X = xyWL[i - 1].X + (mov * -Math.Cos(ang));
xyWL[i].Y = xyWL[i - 1].Y + (mov * Math.Sin(ang));
}
//x11 = x1 * Math.cos(alpha) - y1 * Math.sin(alpha);
//y11 = x1 * Math.sin(alpha) + y1 * Math.cos(alpha);
}
