From 9b5467b99e76e1a5db019444fd41f4bcda673965 Mon Sep 17 00:00:00 2001
From: BenG49 <ben@goldfisher.com>
Date: Sat, 24 Feb 2024 13:16:27 -0500
Subject: [PATCH] Switch flywheel to log rotations

---
 .../stuypulse/robot/constants/Settings.java   |  2 +-
 .../robot/subsystems/flywheel/Flywheel.java   |  5 ++-
 .../subsystems/flywheel/FlywheelSysID.java    | 32 +++++++++----------
 3 files changed, 19 insertions(+), 20 deletions(-)

diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
index 0499e4d..362038f 100644
--- a/src/main/java/com/stuypulse/robot/constants/Settings.java
+++ b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -27,7 +27,7 @@ public enum Mechanism {
             SWERVE_TURN,
             SWERVE_DRIVE,
             TANK_DRIVE,
-            FLYWHEEL,
+            FLYWHEEL,     // must use conversion of 60 in SysID for velocity
             ELEVATOR,
             SINGLE_JOINTED_ARM,
             DOUBLE_JOINTED_ARM_JOINT_ONE,
diff --git a/src/main/java/com/stuypulse/robot/subsystems/flywheel/Flywheel.java b/src/main/java/com/stuypulse/robot/subsystems/flywheel/Flywheel.java
index 2ab890c..0a965a6 100644
--- a/src/main/java/com/stuypulse/robot/subsystems/flywheel/Flywheel.java
+++ b/src/main/java/com/stuypulse/robot/subsystems/flywheel/Flywheel.java
@@ -10,7 +10,6 @@
 
 import com.stuypulse.robot.constants.Ports;
 
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import com.revrobotics.CANSparkLowLevel.MotorType;
@@ -38,11 +37,11 @@ public Flywheel() {
     }
 
     public double getVelocity() {
-        return Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity());
+        return encoder.getVelocity();
     }
 
     public double getPosition() {
-        return Units.rotationsToRadians(encoder.getPosition());
+        return encoder.getPosition();
     }
 
     public double getVoltage() {
diff --git a/src/main/java/com/stuypulse/robot/subsystems/flywheel/FlywheelSysID.java b/src/main/java/com/stuypulse/robot/subsystems/flywheel/FlywheelSysID.java
index 798e830..cebaef5 100644
--- a/src/main/java/com/stuypulse/robot/subsystems/flywheel/FlywheelSysID.java
+++ b/src/main/java/com/stuypulse/robot/subsystems/flywheel/FlywheelSysID.java
@@ -23,22 +23,22 @@ public class FlywheelSysID extends AbstractSysID {
     public FlywheelSysID() {
         this.flywheel = new Flywheel();
         this.flywheelRoutine =
-                new SysIdRoutine(
-                        new SysIdRoutine.Config(),
-                        new SysIdRoutine.Mechanism(
-                                (Measure<Voltage> voltage) -> {
-                                    flywheel.setVoltage(voltage.in(Units.Volts));
-                                },
-                                (log) -> {
-                                    log.motor(flywheel.getName())
-                                            .voltage(Units.Volts.of(flywheel.getVoltage()))
-                                            .angularPosition(
-                                                    Units.Radians.of(flywheel.getPosition()))
-                                            .angularVelocity(
-                                                    Units.RadiansPerSecond.of(
-                                                            flywheel.getVelocity()));
-                                },
-                                this));
+            new SysIdRoutine(
+                new SysIdRoutine.Config(),
+                new SysIdRoutine.Mechanism(
+                    (Measure<Voltage> voltage) -> {
+                        flywheel.setVoltage(voltage.in(Units.Volts));
+                    },
+                    (log) -> {
+                        log.motor(flywheel.getName())
+                            .voltage(Units.Volts.of(flywheel.getVoltage()))
+                            .angularPosition(
+                                Units.Rotations.of(flywheel.getPosition()))
+                            .angularVelocity(
+                                Units.RotationsPerSecond.of(
+                                    flywheel.getVelocity()));
+                    },
+                    this));
     }
 
     @Override