Add setVoltage method to SpeedController (#1997)

Add a voltage-compensated setVoltage method to SpeedController, which is sorely needed to help teams use feedforward-based controls effectively. Also uses correct units on the cpp side.

Also update relevant examples.

wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp

@@ -18,13 +18,14 @@ ShooterSubsystem::ShooterSubsystem()
       m_shooterMotor(kShooterMotorPort),
       m_feederMotor(kFeederMotorPort),
       m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]) {
-  m_controller.SetTolerance(kShooterToleranceRPS);
+  m_controller.SetTolerance(kShooterToleranceRPS.to<double>());
   m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
 }
 
 void ShooterSubsystem::UseOutput(double output) {
   // Use a feedforward of the form kS + kV * velocity
-  m_shooterMotor.Set(output + kSFractional + kVFractional * kShooterTargetRPS);
+  m_shooterMotor.SetVoltage(units::volt_t(output) + kS +
+                            kV * kShooterTargetRPS);
 }
 
 void ShooterSubsystem::Disable() {
@@ -38,7 +39,9 @@ bool ShooterSubsystem::AtSetpoint() { return m_controller.AtSetpoint(); }
 
 double ShooterSubsystem::GetMeasurement() { return m_shooterEncoder.GetRate(); }
 
-double ShooterSubsystem::GetSetpoint() { return kShooterTargetRPS; }
+double ShooterSubsystem::GetSetpoint() {
+  return kShooterTargetRPS.to<double>();
+}
 
 void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); }
 

wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h

@@ -47,9 +47,9 @@ constexpr double kEncoderDistancePerPulse =
 constexpr int kShooterMotorPort = 4;
 constexpr int kFeederMotorPort = 5;
 
-constexpr double kShooterFreeRPS = 5300;
-constexpr double kShooterTargetRPS = 4000;
-constexpr double kShooterToleranceRPS = 50;
+constexpr auto kShooterFreeRPS = 5300_tr / 1_s;
+constexpr auto kShooterTargetRPS = 4000_tr / 1_s;
+constexpr auto kShooterToleranceRPS = 50_tr / 1_s;
 
 constexpr double kP = 1;
 constexpr double kI = 0;
@@ -57,10 +57,10 @@ constexpr double kD = 0;
 
 // On a real robot the feedforward constants should be empirically determined;
 // these are reasonable guesses.
-constexpr double kSFractional = .05;
-constexpr double kVFractional =
-    // Should have value 1 at free speed...
-    1. / kShooterFreeRPS;
+constexpr auto kS = .05_V;
+constexpr auto kV =
+    // Should have value 12V at free speed...
+    12_V / kShooterFreeRPS;
 
 constexpr double kFeederSpeed = .5;
 }  // namespace ShooterConstants

wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp

@@ -75,13 +75,11 @@ frc2::Command* RobotContainer::GetAutonomousCommand() {
       },
       frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
       frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
-      [this](auto left, auto right) {
-        m_drive.TankDrive(left / 12_V, right / 12_V);
-      },
+      [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
       {&m_drive});
 
   // no auto
   return new frc2::SequentialCommandGroup(
       std::move(ramseteCommand),
-      frc2::InstantCommand([this] { m_drive.TankDrive(0, 0); }, {}));
+      frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
 }

wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp

@@ -7,8 +7,6 @@
 
 #include "subsystems/DriveSubsystem.h"
 
-#include <units/units.h>
-
 #include <frc/geometry/Rotation2d.h>
 #include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
 
@@ -39,8 +37,9 @@ void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
   m_drive.ArcadeDrive(fwd, rot);
 }
 
-void DriveSubsystem::TankDrive(double left, double right) {
-  m_drive.TankDrive(left, right, false);
+void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
+  m_leftMotors.SetVoltage(left);
+  m_rightMotors.SetVoltage(-right);
 }
 
 void DriveSubsystem::ResetEncoders() {

wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h

@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <units/units.h>
+
 #include <frc/ADXRS450_Gyro.h>
 #include <frc/Encoder.h>
 #include <frc/PWMVictorSPX.h>
@@ -38,13 +40,12 @@ class DriveSubsystem : public frc2::SubsystemBase {
   void ArcadeDrive(double fwd, double rot);
 
   /**
-   * Drives the robot using tank controls.  Does not square inputs to enable
-   * composition with external controllers.
+   * Controls each side of the drive directly with a voltage.
    *
    * @param left the commanded left output
    * @param right the commanded right output
    */
-  void TankDrive(double left, double right);
+  void TankDriveVolts(units::volt_t left, units::volt_t right);
 
   /**
    * Resets the drive encoders to currently read a position of 0.