public void CalcTargetVelocity(double[] tagVelo, ref double tagAngVelo, double[] nowPosition, double nowAngle)
{
float turningRadius = (float)Math.Sqrt(axisCenterFromRear * axisCenterFromRear + Constants.Wc * Constants.Wc);
if (AutoRunMode == AutoRunModeEnum.Stop)
{
targetVelocity_vec = Vector3.Zero;
}
else
{
targetPosition = selectedPathData[nowIndex];
diffPosition_vec.X = (float)(targetPosition[0] - nowPosition[0]);
diffPosition_vec.Y = (float)(targetPosition[1] - nowPosition[1]);
diffPosition_vec.Z = (float)(targetPosition[2] - nowAngle) * turningRadius;
targetUnitVector_vec = Vector3.Normalize(diffPosition_vec);
if (AutoRunMode == AutoRunModeEnum.RunWayPoint)
{
if (diffPosition_vec.Length() <= errorRadius)
{
if (nowIndex < selectedPathData.Count - 2)
{
nowIndex++;
}
else
{
AutoRunMode = AutoRunModeEnum.RunEndPoint;//目標が最後の点になったら
}
System.Console.WriteLine(nowIndex + " " + diffPosition_vec.Length());
return;
}
targetVelocity_vec = Vector3.Multiply((float)Constants.MaxVelocity, targetUnitVector_vec);
}
else if (AutoRunMode == AutoRunModeEnum.RunEndPoint)
{
targetVelocity_vec = Vector3.Multiply((float)Constants.MaxVelocity * (float)diffPosition_vec.Length() / (float)interval, targetUnitVector_vec);
//targetVelocity_vec = Vector3.Multiply((float)Constants.MaxVelocity, targetUnitVector_vec);
if (diffPosition_vec.Length() <= LastErrorRadius)
{
AutoRunMode = AutoRunModeEnum.Stop;
isEndPoint = false;
nowIndex = 0;
}
}
}
targetVelocityFilter_vec = Vector3.Multiply((float)(1.0 - filterConst), targetVelocityFilter_vec)
+ Vector3.Multiply((float)(filterConst), targetVelocity_vec);//ローパスフィルタ
tagVelo[0] = targetVelocityFilter_vec.X;
tagVelo[1] = targetVelocityFilter_vec.Y;
tagAngVelo = targetVelocityFilter_vec.Z / turningRadius;
}
private void button_startDriving_Click(object sender, EventArgs e)
{
if (AutoRunMode == AutoRunModeEnum.Stop)
{
AutoRunMode = AutoRunModeEnum.RunWayPoint;
}
else
{
AutoRunMode = AutoRunModeEnum.Stop;
}
updateButtonStatus_startDriving();
}
