SetDesiredState

gistfile1.txt

void SwerveModule::SetDesiredState(const frc::SwerveModuleState &referenceState)
{
  // Takes a reference state that feeds two talon PID controler loops for speed and angle.
  // Optimize the reference state to avoid spinning further than 90 degrees
  
  const auto state = frc::SwerveModuleState::Optimize(
      referenceState, frc::Rotation2d(GetSwerveTurningFalconInternalRadians()));
      
  // Need velocity in units per 100ms,
  double speedInMetresPerSecond = state.speed.value();
  double speedInUnitsPer100mS = speedInMetresPerSecond / 10.0 / ModuleConstants::kDriveEncoderMetresPerPulse;

  m_driveMotor.Set(ControlMode::Velocity, speedInUnitsPer100mS);

  // New Code added to handle the 180 to -180 and -180 t0 180 boundary condition
 if (m_prevModuleAngleDegrees > 90)
  {
    // Our previous angle was in an area where a wrap could oocur (SW Corner)
    if (state.angle.Degrees().value() < -90)
    {
      // If we are here it means that target setpoint needs to be adjusted to prevent the
      // module from taking the long way to reach the target in the SE Corner.
      m_encoderWindUpRevolutions++;
    }
  }
  
  if (m_prevModuleAngleDegrees < -90)
  {
    // Our previous angle was in an area where a wrap could occur (SE Corner)
    if (state.angle.Degrees().value() > 90)
    {
      // If we are here it mean that target setpoint needs to be adjusted to prevent the
      // module from taking the long way to reach the target in the SW Corner.
      m_encoderWindUpRevolutions--;
    }
  }
  
  //Update the last setpoint to the current setpoint
  m_prevModuleAngleDegrees = state.angle.Degrees().value();

  // Calculate the setpoint to turn to
  double steeringModuleNativeUnitsSetPoint = (state.angle.Degrees().value() / 360 + m_encoderWindUpRevolutions) * 2048.0 * kSteeringRatio;
  m_turningMotor.Set(ControlMode::Position, steeringModuleNativeUnitsSetPoint);
}  //SetDiresedState