gear ratio, linear threshold

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index 1a1729a294d556e4f64751c5e0efe2ece3160d3e..1724219180ccc0d1e8b4cdf98204118890c8720c 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -50,9 +50,13 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
 
         SmartDashboard.putNumber("Spindexer Power", power);
         SmartDashboard.putNumber("Spindexer Velocity", inputs.spindexerVelocity);
+        
+        // scale threshold based on power
+        double velocityThreshold = SpindexerConstants.spindexerVelocityWithBall * power;
+        SmartDashboard.putNumber("Spindexer Velocity Threshold", velocityThreshold);
         SmartDashboard.putNumber("Spindexer Ball Count", ballCount);
 
-        boolean isSpindexerSlow = inputs.spindexerVelocity < SpindexerConstants.spindexerVelocityWithBall;
+        boolean isSpindexerSlow = inputs.spindexerVelocity < velocityThreshold;
         if (wasSpindexerSlow && !isSpindexerSlow && power > 0.1) {
             ballCount++;
         }
@@ -73,7 +77,7 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
 
     @Override
     public void updateInputs() {
-        inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble();
+        inputs.spindexerVelocity = motor.getVelocity().getValueAsDouble() * SpindexerConstants.gearRatio;
         inputs.spindexerCurrent = motor.getStatorCurrent().getValueAsDouble();
         Logger.processInputs("Spindexer", inputs);
     }

diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
index 9b39829850971ef1a89c859c3ba0d020e71d5b17..5ff6be33d773cb3d3eb36276e3f3f7dc8d33319b 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java
@@ -1,7 +1,9 @@
 package frc.robot.subsystems.spindexer;
 
 public class SpindexerConstants {
-    public static final double spindexerVelocityWithBall = 6.0; // rps (for counting balls)
+    public static final double gearRatio = 1.0 / 27.0;
+    // TODO: measure actual velocity with/without ball to tune threshold
+    public static final double spindexerVelocityWithBall = 6.0 * gearRatio; // output rps at full power
     public static final double spindexerMaxPower = 1.0;
     public static final int currentLimit = 20; // amps
 }