/// <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; }