From c3d6df94d0ac7dbf56ea408aba819ff79b0fffb8 Mon Sep 17 00:00:00 2001
From: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>
Date: Tue, 31 Jan 2023 10:42:02 -0600
Subject: [PATCH] Added setVoltage function to swerve motors, made
 SwervePoseEstimator public, and changed setting targetVelocity to inside
 setState

---
 swervelib/SwerveDrive.java            | 46 ++++++++++++++++-----------
 swervelib/SwerveModule.java           |  8 ++---
 swervelib/SwerveMotor.java            | 13 ++++++--
 swervelib/motors/CTRESwerveMotor.java | 17 ++++++++--
 swervelib/motors/REVSwerveMotor.java  | 11 +++++++
 5 files changed, 66 insertions(+), 29 deletions(-)

diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index 90435de..e517c07 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -62,18 +62,18 @@ public class SwerveDrive extends RobotDriveBase implements Sendable, AutoCloseab
    * Maximum speed in meters per second.
    */
   public final double                m_driverMaxSpeedMPS, m_driverMaxAngularVelocity, m_physicalMaxSpeedMPS;
-  /**
-   * Swerve drive kinematics.
-   */
-  private final SwerveDriveKinematics2   m_swerveKinematics;
   /**
    * Swerve drive pose estimator for attempting to figure out our current position.
    */
-  private final SwerveDrivePoseEstimator m_swervePoseEstimator;
+  public final  SwerveDrivePoseEstimator m_swervePoseEstimator;
   /**
    * Pigeon 2.0 centered on the robot.
    */
-  private final WPI_Pigeon2              m_pigeonIMU;
+  public final  WPI_Pigeon2              m_pigeonIMU;
+  /**
+   * Swerve drive kinematics.
+   */
+  private final SwerveDriveKinematics2   m_swerveKinematics;
   /**
    * Field2d displayed on shuffleboard with current position.
    */
@@ -87,8 +87,17 @@ public class SwerveDrive extends RobotDriveBase implements Sendable, AutoCloseab
    * Invert the gyro reading.
    */
   private       boolean       m_gyroInverted;
+  /**
+   * Robot desired angle in degrees.
+   */
   private       double        m_angle;
+  /**
+   * Previous chassis speeds for state-space modeling.
+   */
   private       ChassisSpeeds m_prevChassisSpeed = new ChassisSpeeds(0, 0, 0);
+  /**
+   * Previous timer value for state-space modeling.
+   */
   private       double        m_timerPrev;
 
   /**
@@ -167,19 +176,6 @@ public class SwerveDrive extends RobotDriveBase implements Sendable, AutoCloseab
     zeroModules(); // Set all modules to 0.
   }
 
-  /**
-   * Enable voltage compensation to the current battery voltage on all modules.
-   */
-  public void setVoltageCompensation()
-  {
-    double currentVoltage = RobotController.getBatteryVoltage();
-    m_frontLeft.setVoltageCompensation(currentVoltage);
-    m_frontRight.setVoltageCompensation(currentVoltage);
-    m_backLeft.setVoltageCompensation(currentVoltage);
-    m_backRight.setVoltageCompensation(currentVoltage);
-  }
-
-
   /**
    * Create swerve drive modules
    *
@@ -233,6 +229,18 @@ public class SwerveDrive extends RobotDriveBase implements Sendable, AutoCloseab
     return modules;
   }
 
+  /**
+   * Enable voltage compensation to the current battery voltage on all modules.
+   */
+  public void setVoltageCompensation()
+  {
+    double currentVoltage = RobotController.getBatteryVoltage();
+    m_frontLeft.setVoltageCompensation(currentVoltage);
+    m_frontRight.setVoltageCompensation(currentVoltage);
+    m_backLeft.setVoltageCompensation(currentVoltage);
+    m_backRight.setVoltageCompensation(currentVoltage);
+  }
+
   /**
    * Sets the speed to 0 and angle to 0 for all the swerve drive modules.
    */
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index 994d19f..66ba7fe 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -2,7 +2,7 @@ package frc.robot.subsystems.swervedrive.swerve;
 
 import static java.util.Objects.requireNonNull;
 
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.MagnetFieldStrength;
 import com.revrobotics.AbsoluteEncoder;
@@ -166,8 +166,8 @@ public class SwerveModule<DriveMotorType extends MotorController, AngleMotorType
     this.wheelBase = wheelBaseMeters;
     this.driveTrainWidth = driveTrainWidthMeters;
 
-    assert mainMotor instanceof CANSparkMax || mainMotor instanceof TalonFX;
-    assert angleMotor instanceof CANSparkMax || angleMotor instanceof TalonFX;
+    assert mainMotor instanceof CANSparkMax || mainMotor instanceof WPI_TalonFX;
+    assert angleMotor instanceof CANSparkMax || angleMotor instanceof WPI_TalonFX;
     assert encoder instanceof DutyCycleEncoder || encoder instanceof AnalogEncoder || encoder instanceof CANCoder ||
            encoder instanceof AbsoluteEncoder;
 
@@ -413,7 +413,6 @@ public class SwerveModule<DriveMotorType extends MotorController, AngleMotorType
    */
   public void setVelocity(double velocity)
   {
-    targetVelocity = velocity;
     driveMotor.setTarget(velocity, driveFeedforward.calculate(velocity));
   }
 
@@ -463,6 +462,7 @@ public class SwerveModule<DriveMotorType extends MotorController, AngleMotorType
     setAngle(angle, optimizedState.angularVelocityRadPerSecond * steeringKV);
     setVelocity(velocity);
     targetAngle = angle;
+    targetVelocity = velocity;
     targetAngularVelocityRPS = optimizedState.angularVelocityRadPerSecond;
   }
 
diff --git a/swervelib/SwerveMotor.java b/swervelib/SwerveMotor.java
index 4d68eab..f462e3a 100644
--- a/swervelib/SwerveMotor.java
+++ b/swervelib/SwerveMotor.java
@@ -1,6 +1,6 @@
 package frc.robot.subsystems.swervedrive.swerve;
 
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.revrobotics.CANSparkMax;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import frc.robot.subsystems.swervedrive.swerve.motors.CTRESwerveMotor;
@@ -54,9 +54,9 @@ public abstract class SwerveMotor
                                 freeSpeedRPM,
                                 powerLimit);
     }
-    if (motor instanceof TalonFX)
+    if (motor instanceof WPI_TalonFX)
     {
-      return new CTRESwerveMotor((TalonFX) motor,
+      return new CTRESwerveMotor((WPI_TalonFX) motor,
                                  absoluteEncoder,
                                  type,
                                  gearRatio,
@@ -120,6 +120,13 @@ public abstract class SwerveMotor
    */
   public abstract void set(double speed);
 
+  /**
+   * Set the voltage of the motor.
+   *
+   * @param voltage Voltage to output.
+   */
+  public abstract void setVoltage(double voltage);
+
   /**
    * Get the current value of the encoder corresponding to the PID.
    *
diff --git a/swervelib/motors/CTRESwerveMotor.java b/swervelib/motors/CTRESwerveMotor.java
index f847243..eff6322 100644
--- a/swervelib/motors/CTRESwerveMotor.java
+++ b/swervelib/motors/CTRESwerveMotor.java
@@ -5,8 +5,8 @@ import com.ctre.phoenix.motorcontrol.DemandType;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
 import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.AbsoluteSensorRange;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.SensorInitializationStrategy;
@@ -26,7 +26,7 @@ public class CTRESwerveMotor extends SwerveMotor
   private final int              m_mainPIDSlotId;
   private final int              m_mainPidId;
   private final Supplier<Double> m_encoderRet;
-  private final TalonFX          m_motor;
+  private final WPI_TalonFX      m_motor;
 
   /**
    * kV feed forward for PID
@@ -34,7 +34,7 @@ public class CTRESwerveMotor extends SwerveMotor
   private final double m_moduleRadkV;
 
   // TODO: Finish this based off of BaseFalconSwerve
-  public CTRESwerveMotor(TalonFX motor, SwerveEncoder<?> encoder, ModuleMotorType type, double gearRatio,
+  public CTRESwerveMotor(WPI_TalonFX motor, SwerveEncoder<?> encoder, ModuleMotorType type, double gearRatio,
                          double wheelDiameter,
                          double freeSpeedRPM, double powerLimit)
   {
@@ -197,6 +197,17 @@ public class CTRESwerveMotor extends SwerveMotor
     m_motor.set(ControlMode.PercentOutput, speed);
   }
 
+  /**
+   * Set the voltage of the motor.
+   *
+   * @param voltage Voltage to output.
+   */
+  @Override
+  public void setVoltage(double voltage)
+  {
+    m_motor.setVoltage(voltage);
+  }
+
   /**
    * Get the current value of the encoder corresponding to the PID.
    *
diff --git a/swervelib/motors/REVSwerveMotor.java b/swervelib/motors/REVSwerveMotor.java
index 9eeddae..a088a15 100644
--- a/swervelib/motors/REVSwerveMotor.java
+++ b/swervelib/motors/REVSwerveMotor.java
@@ -236,6 +236,17 @@ public class REVSwerveMotor extends SwerveMotor
     m_motor.set(speed);
   }
 
+  /**
+   * Set the voltage of the motor.
+   *
+   * @param voltage Voltage to output.
+   */
+  @Override
+  public void setVoltage(double voltage)
+  {
+    m_motor.setVoltage(voltage);
+  }
+
   /**
    * Get the current value of the encoder corresponding to the PID.
    *