same speed for both flywheels

grt192 · Apr 5, 2024 · 85c02fb · 85c02fb
1 parent 2161910
commit 85c02fb
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 22 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -550,8 +550,7 @@ private void configureBindings() {
             ) {
                 System.out.println("Dist: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getShootingDistance())
                                 + " Angle: " + GRTUtil.twoDecimals(shooterPivotSubsystem.getPosition())
-                                + " Top: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getTopMotorSplineSpeed())
-                                + " Bot: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getBottomMotorSplineSpeed()));
+                                + " Speed: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getSplineSpeed()));
             }
             noteInBack = intakeRollerSubsystem.getRockwellSensorValue();
 

diff --git a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java
@@ -22,8 +22,8 @@ public ShooterFlywheelReadyCommand(ShooterFlywheelSubsystem shooterSubsystem, Li
         this.lightBarSubsystem = lightBarSubsystem;
         addRequirements(shooterSubsystem);
 
-        topSpeed = shooterSubsystem.getTopMotorSplineSpeed();
-        bottomSpeed = shooterSubsystem.getBottomMotorSplineSpeed();
+        topSpeed = shooterSubsystem.getSplineSpeed();
+        bottomSpeed = shooterSubsystem.getSplineSpeed();
         shooterSubsystem.setAutoAimShooter(true);
     }
 

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java
@@ -50,7 +50,7 @@ public class ShooterFlywheelSubsystem extends SubsystemBase {
     private BooleanSupplier redSupplier;
 
     private AkimaSplineInterpolator akima;
-    private PolynomialSplineFunction topFlywheelSpline;
+    private PolynomialSplineFunction flywheelSpline;
     private PolynomialSplineFunction bottomFlywheelSpline;
     private boolean atSpeed = false;
     private boolean autoAim = false;
@@ -89,15 +89,13 @@ public ShooterFlywheelSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier
                               5.6, 
                               ShooterConstants.MAX_SHOOTER_DISTANCE};
 
-        double[] topSpeeds = {.4, .5, .7, .75, .75, .75, .75};
-        double[] bottomSpeeds = {.5, .5, .35, .4, .4, .75, .75};
+        double[] speeds = {.4, .5, .7, .75, .75, .75, .75};
 
         targetTopRPS = 0.0;
         targetBottomRPS = 0.0; 
 
         akima = new AkimaSplineInterpolator();
-        topFlywheelSpline = akima.interpolate(distances, topSpeeds);
-        bottomFlywheelSpline = akima.interpolate(distances, bottomSpeeds);
+        flywheelSpline = akima.interpolate(distances, speeds);
 
         configs.kP = .5;
         configs.kI = 0.05;
@@ -146,7 +144,7 @@ public void setShooterMotorSpeed(double speed) {
 
     /** Sets the shooter to the splined speeds. */
     public void setShooterMotorSpeed() {
-        setShooterMotorSpeed(getTopMotorSplineSpeed(), getBottomMotorSplineSpeed());
+        setShooterMotorSpeed(getSplineSpeed(), getSplineSpeed());
     }
 
     /** Changes auto aim angle. */
@@ -176,17 +174,10 @@ public boolean atSpeed() {
      *
      * @return The splined top motor speed.
      */
-    public double getTopMotorSplineSpeed() {
-        return topFlywheelSpline.value(MathUtil.clamp(getShootingDistance(), ShooterConstants.MIN_SHOOTER_DISTANCE, ShooterConstants.MAX_SHOOTER_DISTANCE));
-    }
-
-    /**
-     * Returns the current spline bottom motor speed.
-     *
-     * @return The spline bottom motor speed.
-     */
-    public double getBottomMotorSplineSpeed() {
-        return bottomFlywheelSpline.value(MathUtil.clamp(getShootingDistance(), ShooterConstants.MIN_SHOOTER_DISTANCE, ShooterConstants.MAX_SHOOTER_DISTANCE));
+    public double getSplineSpeed() {
+        return flywheelSpline.value(MathUtil.clamp(
+            getShootingDistance(), ShooterConstants.MIN_SHOOTER_DISTANCE, ShooterConstants.MAX_SHOOTER_DISTANCE
+        ));
     }
 
     /**
@@ -233,7 +224,7 @@ public double getShootingDistance() {
     public void periodic() {
         //only grabs spline speeds if shooter motor is running (ie not stopped)
         if (autoAim) {
-            setShooterMotorSpeed(getTopMotorSplineSpeed(), getBottomMotorSplineSpeed());
+            setShooterMotorSpeed(getSplineSpeed(), getSplineSpeed());
         }
 
         shooter13CurrentEntry.setDouble(shooterMotorTop.getSupplyCurrent().getValueAsDouble());