Switch flywheel to log rotations

StuyPulse · Feb 24, 2024 · 9b5467b · 9b5467b
1 parent 90afb7e
commit 9b5467b
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 20 deletions.
diff --git 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
@@ -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
@@ -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