/// <summary> Motor Angle Calculation: find out how to control wheel direction </summary>
/// <param name="a_eDeltaCarAngle">[IN] Car Angle offset to wanted direction </param>
/// <param name="a_eCarSpeed">[IN] Car Speed </param>
/// <param name="a_tPID_MotorAngleCoe">[IN] PID Coe of dorection control </param>
/// <returns> Motor Angle </returns>
public static double MotorAngleCal(double a_eDeltaCarAngle, double a_eCarSpeed, rtPID_Coefficient a_tPID_MotorAngleCoe)
{ // 目前不考慮 車速的因素
double MotorAngle = 0;
if (a_eDeltaCarAngle > DELTA_CAR_ANGLE_LIMIT)
{ // 超過角度限制 最多轉70度
MotorAngle = (a_eDeltaCarAngle > 0) ? MAX_ANGLE_OFFSET_MOTOR : -MAX_ANGLE_OFFSET_MOTOR;
}
else
{ // 按系數計算
MotorAngle = a_eDeltaCarAngle * a_tPID_MotorAngleCoe.eKp;
}
return MotorAngle;
}
/// <summary> Path Angle Offset Calculation </summary>
/// <param name="a_eDistance">[IN] distance to the path </param>
/// <param name="a_tPID_ThetaOffsetCoe">[IN] PID Coe </param>
/// <returns> angle Offset </returns>
public static double PathAngleOffsetCal(double a_eDistance, rtPID_Coefficient a_tPID_ThetaOffsetCoe)
{
double eThetaOffset = 0;
if (Math.Abs(a_eDistance) > PATH_ERROR_LIMT)
{ // 超過距離限制 最多跟路徑差70度
eThetaOffset = (a_eDistance > 0) ? MAX_ANGLE_OFFSET_PATH : -MAX_ANGLE_OFFSET_PATH;
}
else
{ // 按系數計算
eThetaOffset = a_eDistance * a_tPID_ThetaOffsetCoe.eKp;
}
if (Math.Abs(eThetaOffset) > MAX_ANGLE_OFFSET_PATH)
{ // 超過距離限制 最多跟路徑差70度
eThetaOffset = (eThetaOffset > 0) ? MAX_ANGLE_OFFSET_PATH : -MAX_ANGLE_OFFSET_PATH;
}
return eThetaOffset;
}
