Make C++ more consistent with Java

Gold856 · Sep 26, 2024 · e943fb2 · e943fb2
1 parent 2eb3b4e
commit e943fb2
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 6 deletions.
diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/cpp/subsystems/Drive.cpp
@@ -24,6 +24,14 @@ Drive::Drive() {
   // Sets the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(DriveConstants::kEncoderDistancePerPulse);
+
+  // Set the controller to be continuous (because it is an angle controller)
+  m_controller.EnableContinuousInput(-180_deg, 180_deg);
+  // Set the controller tolerance - the delta tolerance ensures the robot is
+  // stationary at the setpoint before it is considered as having reached the
+  // reference
+  m_controller.SetTolerance(DriveConstants::kTurnTolerance,
+                            DriveConstants::kTurnRateTolerance);
 }
 
 frc2::CommandPtr Drive::ArcadeDriveCommand(std::function<double()> fwd,
@@ -54,10 +62,10 @@ frc2::CommandPtr Drive::DriveDistanceCommand(units::meter_t distance,
 
 frc2::CommandPtr Drive::TurnToAngleCommand(units::degree_t angle) {
   return StartRun(
-             [this] { m_controller.Reset(m_gyro.GetRotation2d().Radians()); },
+             [this] { m_controller.Reset(m_gyro.GetRotation2d().Degrees()); },
              [this, angle] {
                m_drive.ArcadeDrive(
-                   0, m_controller.Calculate(m_gyro.GetRotation2d().Radians(),
+                   0, m_controller.Calculate(m_gyro.GetRotation2d().Degrees(),
                                              angle) +
                           // Divide feedforward voltage by battery voltage to
                           // normalize it to [-1, 1]

diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/Constants.h
@@ -44,8 +44,8 @@ inline constexpr auto kMaxTurnAcceleration = 300_deg_per_s / 1_s;
 // theoretically for *your* robot's drive. The SysId tool provides a convenient
 // method for obtaining these values for your robot.
 inline constexpr auto ks = 1_V;
-inline constexpr auto kv = 0.8_V * 1_s / 1_rad;
-inline constexpr auto ka = 0.15_V * 1_s * 1_s / 1_rad;
+inline constexpr auto kv = 0.8_V * 1_s / 1_deg;
+inline constexpr auto ka = 0.15_V * 1_s * 1_s / 1_deg;
 }  // namespace DriveConstants
 
 namespace IntakeConstants {

diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h
@@ -70,11 +70,11 @@ class Drive : public frc2::SubsystemBase {
 
   frc::ADXRS450_Gyro m_gyro;
 
-  frc::ProfiledPIDController<units::radians> m_controller{
+  frc::ProfiledPIDController<units::degrees> m_controller{
       DriveConstants::kTurnP,
       DriveConstants::kTurnI,
       DriveConstants::kTurnD,
       {DriveConstants::kMaxTurnRate, DriveConstants::kMaxTurnAcceleration}};
-  frc::SimpleMotorFeedforward<units::radians> m_feedforward{
+  frc::SimpleMotorFeedforward<units::degrees> m_feedforward{
       DriveConstants::ks, DriveConstants::kv, DriveConstants::ka};
 };