Gear Ratio Calculator (#32)

* added `GearRatioCalculationCommand.java`

* Updated

* Finished

* Updated TRIGONLib

* Removed useless method

Author: Strflightmight09

build.gradle

@@ -88,7 +88,7 @@ dependencies {
     simulationRelease wpi.sim.enableRelease()
 
     implementation 'com.google.code.gson:gson:2.10.1'
-    implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.9'
+    implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.10'
 
     def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
     annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"

src/main/java/frc/trigon/robot/subsystems/pitcher/Pitcher.java

@@ -15,6 +15,8 @@
 import frc.trigon.robot.subsystems.MotorSubsystem;
 import frc.trigon.robot.subsystems.ampaligner.AmpAlignerConstants;
 import org.littletonrobotics.junction.Logger;
+import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder;
+import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal;
 import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor;
 import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal;
 
@@ -23,6 +25,7 @@ public class Pitcher extends MotorSubsystem {
     private final TalonFXMotor
             masterMotor = PitcherConstants.MASTER_MOTOR,
             followerMotor = PitcherConstants.FOLLOWER_MOTOR;
+    private final CANcoderEncoder encoder = PitcherConstants.ENCODER;
     private final MotionMagicExpoVoltage positionRequest = new MotionMagicExpoVoltage(0).withEnableFOC(PitcherConstants.FOC_ENABLED);
     private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(PitcherConstants.FOC_ENABLED);
     private Rotation2d targetPitch = PitcherConstants.DEFAULT_PITCH;
@@ -53,6 +56,7 @@ public void stop() {
     @Override
     public void updatePeriodically() {
         masterMotor.update();
+        encoder.update();
     }
 
     @Override
@@ -88,6 +92,14 @@ public boolean atTargetPitch() {
         return Math.abs(masterMotor.getSignal(TalonFXSignal.POSITION) - targetPitch.getRotations()) < PitcherConstants.PITCH_TOLERANCE.getRotations();
     }
 
+    Rotation2d getRotorPosition() {
+        return Rotation2d.fromRotations(masterMotor.getSignal(TalonFXSignal.ROTOR_POSITION));
+    }
+
+    Rotation2d getEncoderPosition() {
+        return Rotation2d.fromRotations(encoder.getSignal(CANcoderSignal.POSITION));
+    }
+
     void reachTargetPitchFromShootingCalculations() {
         targetPitch = shootingCalculations.getTargetShootingState().targetPitch();
         setTargetPitch(targetPitch);

src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherCommands.java

@@ -1,10 +1,12 @@
 package frc.trigon.robot.subsystems.pitcher;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import frc.trigon.robot.RobotContainer;
 import org.trigon.commands.ExecuteEndCommand;
+import org.trigon.commands.GearRatioCalculationCommand;
 import org.trigon.commands.NetworkTablesCommand;
 
 public class PitcherCommands {
@@ -16,6 +18,15 @@ public static Command getDebuggingCommand() {
         );
     }
 
+    public static Command getCalculateGearRatioCommand() {
+        return new GearRatioCalculationCommand(
+                () -> RobotContainer.PITCHER.getRotorPosition().getDegrees(),
+                () -> RobotContainer.PITCHER.getEncoderPosition().getDegrees(),
+                (voltage) -> RobotContainer.PITCHER.drive(Units.Volts.of(voltage)),
+                RobotContainer.PITCHER
+        );
+    }
+
     public static Command getReachTargetPitchFromShootingCalculationsCommand() {
         return new ExecuteEndCommand(
                 RobotContainer.PITCHER::reachTargetPitchFromShootingCalculations,

src/main/java/frc/trigon/robot/subsystems/pitcher/PitcherConstants.java

@@ -16,6 +16,7 @@
 import frc.trigon.robot.subsystems.ampaligner.AmpAlignerConstants;
 import org.trigon.hardware.RobotHardwareStats;
 import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder;
+import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal;
 import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor;
 import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal;
 import org.trigon.hardware.simulation.SingleJointedArmSimulation;
@@ -146,6 +147,7 @@ private static void configureMasterMotor() {
         MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100);
         MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100);
         MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100);
+        MASTER_MOTOR.registerSignal(TalonFXSignal.ROTOR_POSITION, 100);
     }
 
     private static void configureFollowerMotor() {
@@ -172,5 +174,8 @@ public static void configureEncoder() {
 
         ENCODER.applyConfiguration(config);
         ENCODER.setSimulationInputsFromTalonFX(MASTER_MOTOR);
+
+        ENCODER.registerSignal(CANcoderSignal.POSITION, 100);
+        ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100);
     }
 }