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(); }