virtual double operator()(double time,
double rest_length,
double length,
double vel,
ChLinkSpringCB* link) override {
// Clip the velocity to within +/- 0.254 m/s [10 in/s]
ChClampValue(vel, -0.254, 0.254);
// Velocity is positive in extension.
// Linear interpolate between 500lbf in extension at 10 in/s and
// -2000lbf at -10in/s (converted to N)
return vel >= 0 ? (-8756.341762 * vel) : (-35025.36705 * vel);
}
void ChPathFollowerDriver::Advance(double step) {
// Set the throttle and braking values based on the output from the speed controller.
double out_speed = m_speedPID.Advance(m_vehicle, m_target_speed, step);
ChClampValue(out_speed, -1.0, 1.0);
if (out_speed > 0) {
// Vehicle moving too slow
m_braking = 0;
m_throttle = out_speed;
} else if (m_throttle > m_throttle_threshold) {
// Vehicle moving too fast: reduce throttle
m_braking = 0;
m_throttle = 1 + out_speed;
} else {
// Vehicle moving too fast: apply brakes
m_braking = -out_speed;
m_throttle = 0;
}
// Set the steering value based on the output from the steering controller.
double out_steering = m_steeringPID.Advance(m_vehicle, step);
ChClampValue(out_steering, -1.0, 1.0);
m_steering = out_steering;
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChPac89Tire::Advance(double step) {
// Return now if no contact. Tire force and moment are already set to 0 in Synchronize().
if (!m_data.in_contact)
return;
if (m_states.vx != 0) {
m_states.cp_long_slip = -m_states.vsx / m_states.vx;
} else {
m_states.cp_long_slip = 0;
}
if (m_states.omega != 0) {
m_states.cp_side_slip = std::atan(m_states.vsy / std::abs(m_states.omega * (m_unloaded_radius - m_data.depth)));
} else {
m_states.cp_side_slip = 0;
}
// Ensure that cp_lon_slip stays between -1 & 1
ChClampValue(m_states.cp_long_slip, -1.0, 1.0);
// Ensure that cp_side_slip stays between -pi()/2 & pi()/2 (a little less to prevent tan from going to infinity)
ChClampValue(m_states.cp_side_slip, -CH_C_PI_2 + 0.001, CH_C_PI_2 - 0.001);
// Calculate the new force and moment values (normal force and moment have already been accounted for in
// Synchronize()).
// Express Fz in kN (note that all other forces and moments are in N and Nm).
// See reference for details on the calculations.
double Fx = 0;
double Fy = 0;
double Fz = m_data.normal_force / 1000;
double Mx = 0;
double My = 0;
double Mz = 0;
// Express alpha and gamma in degrees. Express kappa as percentage.
// Flip sign of alpha to convert to PAC89 modified SAE coordinates.
m_gamma = 90.0 - std::acos(m_states.disc_normal.z()) * CH_C_RAD_TO_DEG;
m_alpha = -m_states.cp_side_slip * CH_C_RAD_TO_DEG;
m_kappa = m_states.cp_long_slip * 100.0;
// Clamp |gamma| to specified value: Limit due to tire testing, avoids erratic extrapolation.
double gamma = ChClamp(m_gamma, -m_gamma_limit, m_gamma_limit);
// Longitudinal Force
{
double C = m_PacCoeff.B0;
double D = (m_PacCoeff.B1 * std::pow(Fz, 2) + m_PacCoeff.B2 * Fz);
double BCD = (m_PacCoeff.B3 * std::pow(Fz, 2) + m_PacCoeff.B4 * Fz) * std::exp(-m_PacCoeff.B5 * Fz);
double B = BCD / (C * D);
double Sh = m_PacCoeff.B9 * Fz + m_PacCoeff.B10;
double Sv = 0.0;
double X1 = (m_kappa + Sh);
double E = (m_PacCoeff.B6 * std::pow(Fz, 2) + m_PacCoeff.B7 * Fz + m_PacCoeff.B8);
Fx = (D * std::sin(C * std::atan(B * X1 - E * (B * X1 - std::atan(B * X1))))) + Sv;
}
// Lateral Force
{
double C = m_PacCoeff.A0;
double D = (m_PacCoeff.A1 * std::pow(Fz, 2) + m_PacCoeff.A2 * Fz);
double BCD =
m_PacCoeff.A3 * std::sin(std::atan(Fz / m_PacCoeff.A4) * 2.0) * (1.0 - m_PacCoeff.A5 * std::abs(gamma));
double B = BCD / (C * D);
double Sh = m_PacCoeff.A9 * Fz + m_PacCoeff.A10 + m_PacCoeff.A8 * gamma;
double Sv = m_PacCoeff.A11 * Fz * gamma + m_PacCoeff.A12 * Fz + m_PacCoeff.A13;
double X1 = m_alpha + Sh;
double E = m_PacCoeff.A6 * Fz + m_PacCoeff.A7;
// Ensure that X1 stays within +/-90 deg minus a little bit
ChClampValue(X1, -89.5, 89.5);
Fy = (D * std::sin(C * std::atan(B * X1 - E * (B * X1 - std::atan(B * X1))))) + Sv;
}
// Self-Aligning Torque
{
double C = m_PacCoeff.C0;
double D = (m_PacCoeff.C1 * std::pow(Fz, 2) + m_PacCoeff.C2 * Fz);
double BCD = (m_PacCoeff.C3 * std::pow(Fz, 2) + m_PacCoeff.C4 * Fz) * (1 - m_PacCoeff.C6 * std::abs(gamma)) *
std::exp(-m_PacCoeff.C5 * Fz);
double B = BCD / (C * D);
double Sh = m_PacCoeff.C11 * gamma + m_PacCoeff.C12 * Fz + m_PacCoeff.C13;
double Sv =
(m_PacCoeff.C14 * std::pow(Fz, 2) + m_PacCoeff.C15 * Fz) * gamma + m_PacCoeff.C16 * Fz + m_PacCoeff.C17;
double X1 = m_alpha + Sh;
double E = (m_PacCoeff.C7 * std::pow(Fz, 2) + m_PacCoeff.C8 * Fz + m_PacCoeff.C9) *
(1.0 - m_PacCoeff.C10 * std::abs(gamma));
// Ensure that X1 stays within +/-90 deg minus a little bit
ChClampValue(X1, -89.5, 89.5);
Mz = (D * std::sin(C * std::atan(B * X1 - E * (B * X1 - std::atan(B * X1))))) + Sv;
}
// Overturning Moment
{
double deflection = Fy / m_lateral_stiffness;
Mx = -(Fz * 1000) * deflection;
Mz = Mz + Fx * deflection;
}
// Rolling Resistance
{
double Lrad = (m_unloaded_radius - m_data.depth);
// Smoothing interval for My
const double vx_min = 0.125;
const double vx_max = 0.5;
// Smoothing factor dependend on m_state.abs_vx, allows soft switching of My
double myStartUp = ChSineStep(std::abs(m_states.vx), vx_min, 0.0, vx_max, 1.0);
My = myStartUp * m_rolling_resistance * m_data.normal_force * Lrad * ChSignum(m_states.omega);
}
// std::cout << "Fx:" << Fx
// << " Fy:" << Fy
// << " Fz:" << Fz
// << " Mx:" << Mx
// << " My:" << My
// << " Mz:" << Mz
// << std::endl
// << " G:" << gamma
// << " A:" << alpha
// << " K:" << kappa
// << " O:" << m_states.omega
// << std::endl;
// Compile the force and moment vectors so that they can be
// transformed into the global coordinate system.
// Convert from SAE to ISO Coordinates at the contact patch.
m_tireforce.force = ChVector<>(Fx, -Fy, m_data.normal_force);
m_tireforce.moment = ChVector<>(Mx, -My, -Mz);
// Rotate into global coordinates
m_tireforce.force = m_data.frame.TransformDirectionLocalToParent(m_tireforce.force);
m_tireforce.moment = m_data.frame.TransformDirectionLocalToParent(m_tireforce.moment);
// Move the tire forces from the contact patch to the wheel center
m_tireforce.moment +=
Vcross((m_data.frame.pos + m_data.depth * m_data.frame.rot.GetZaxis()) - m_tireforce.point, m_tireforce.force);
}
