diff --git a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java index 9ab907a7..66374153 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter/Shooter.java @@ -213,7 +213,8 @@ public static Pose2d getShooterPose(Pose2d robotPose) { rotatedShooter.getRotation()); } - ShooterSetpoint lastShooterSetpoint = new ShooterSetpoint(MetersPerSecond.zero(), Degrees.of(5)); + ShooterSetpoint lastShooterSetpoint = + new ShooterSetpoint(RotationsPerSecond.zero(), Degrees.of(5)); /** * @return The speed and position of the shooter to shoot into the hub. */ @@ -310,21 +311,8 @@ public Command runShooterControl() { // }) // .withName("Shooter Tuning"); // TODO: REPLACE HUB WITH VARIABLE TARGET - // return run(() -> { - // setFlywheelVelocityInternal(RotationsPerSecond.of(tunableShooterSpeed.get())); - // setHoodAngleInternal(Degrees.of(tunableHoodAngle.get())); - // }) - // .withName("Shooter Tuning"); - // TODO: REPLACE HUB WITH VARIABLE TARGET - // Joe: The LUT returns flywheelSurfaceSpeed as m/s, but setFlywheelVelocityInternal expects - // RPS — wrapping m/s in RotationsPerSecond.of() won't give the right value, right? Should we - // use convertLinearVelocityToAngular() here? return run(() -> { - setFlywheelVelocityInternal( - RotationsPerSecond.of( - getCurrentSetpoint(getHubLocation2d()) - .flywheelSurfaceSpeed() - .in(MetersPerSecond))); + setFlywheelVelocityInternal(getCurrentSetpoint(getHubLocation2d()).flywheelVelocity()); setHoodAngleInternal(calculateHoodAngle()); }) .withName("Shooter control"); @@ -343,16 +331,15 @@ public Command setFlywheelVelocity(AngularVelocity velocity) { * @return The target angular velocity for the flywheel */ public AngularVelocity targetVelocity() { - return convertLinearVelocityToAngular( - getCurrentSetpoint(getHubLocation2d()).flywheelSurfaceSpeed()); + return getCurrentSetpoint(getHubLocation2d()).flywheelVelocity(); } // TODO: REPLACE HUB WITH VARIABLE TARGET /** - * @return The target linear velocity for the flywheel + * @return The target angular velocity for the flywheel */ - public LinearVelocity targetLinearVelocity() { - return getCurrentSetpoint(getHubLocation2d()).flywheelSurfaceSpeed(); + public AngularVelocity targetFlywheelVelocity() { + return getCurrentSetpoint(getHubLocation2d()).flywheelVelocity(); } /** @@ -613,8 +600,8 @@ public void shootSimulatedProjectile() { chassisSpeeds.get(), drivetrainPose.get().getRotation()), kMapleSimSHooterRotationAlsoNotJank.plus(simPose.get().getRotation()), kShooterHeight, - getCurrentSetpoint(getHubLocation2d()) - .flywheelSurfaceSpeed() + convertAngularVelocityToLinear( + getCurrentSetpoint(getHubLocation2d()).flywheelVelocity()) .times(kEstimatedFlywheelSpeedToFuelSpeed), getCurrentSetpoint(getHubLocation2d()).rotation().plus(Degrees.of(90)))); } diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterConstants.java index ecbc086b..fdec74e5 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterConstants.java @@ -94,7 +94,7 @@ protected static TalonFXConfiguration generateHoodConfig( return config; } - protected static final double kEstimatedFlywheelSpeedToFuelSpeed = 0.3; + protected static final double kEstimatedFlywheelSpeedToFuelSpeed = 0.33; public static final TalonFXConfiguration kFlyWheelConfig = generateFlywheelConfig( diff --git a/src/main/java/frc/robot/subsystems/Shooter/ShooterLUT.java b/src/main/java/frc/robot/subsystems/Shooter/ShooterLUT.java index 56286d46..6e447f81 100644 --- a/src/main/java/frc/robot/subsystems/Shooter/ShooterLUT.java +++ b/src/main/java/frc/robot/subsystems/Shooter/ShooterLUT.java @@ -12,14 +12,14 @@ import java.util.Optional; public class ShooterLUT { - public record ShooterSetpoint(LinearVelocity flywheelSurfaceSpeed, Angle rotation) {} + public record ShooterSetpoint(AngularVelocity flywheelVelocity, Angle rotation) {} public record SOTMSetpoint( ShooterSetpoint shooterSetpoint, Rotation2d robotRotation, Pose2d iteratedPose) {} public static ShooterSetpoint getSpeedAndRotation(Distance distance) { return new ShooterSetpoint( - MetersPerSecond.of(kShooterSpeedMap.get(distance.in(Meters))), + RotationsPerSecond.of(kShooterSpeedMap.get(distance.in(Meters))), Degrees.of(kShooterAngleMap.get(distance.in(Meters)))); } @@ -64,8 +64,6 @@ public static Optional generateShootOnTheMoveSetpoint( private static InterpolatingDoubleTreeMap generateSpeedMap() { var map = new InterpolatingDoubleTreeMap(); if (RobotBase.isReal()) { - // Joe: These values are supposed to be flywheel surface speed in m/s, but 60-80 seems really - // high for m/s — are these actually RPS from the old shooter? Might need retuning. map.put(1.041, 60.0); map.put(1.92, 65.0); map.put(2.31, 65.0); @@ -74,18 +72,13 @@ private static InterpolatingDoubleTreeMap generateSpeedMap() { map.put(5.71, 92.0); map.put(7.37, 103.0); } else { - map.put(1.25, 25.2); - map.put(1.5, 26.5); - map.put(1.7, 26.8); - map.put(2.0, 27.0); - map.put(2.5, 27.5); - map.put(3.0, 28.1); - map.put(3.5, 28.5); - map.put(4.0, 29.2); - map.put(4.5, 29.9); - map.put(5.0, 30.4); - map.put(5.5, 30.8); - map.put(6.0, 31.5); + map.put(1.041, 60.0); + map.put(1.92, 65.0); + map.put(2.31, 65.0); + map.put(3.25, 72.0); + map.put(4.17, 78.0); + map.put(5.71, 92.0); + map.put(7.37, 103.0); } return map; } @@ -101,16 +94,13 @@ private static InterpolatingDoubleTreeMap generateAngleMap() { map.put(5.71, 32.0); map.put(7.37, 43.0); } else { - map.put(1.5, 8.56); - map.put(2.0, 11.32); - map.put(2.5, 13.63); - map.put(3.0, 15.76); - map.put(3.5, 18.04); - map.put(4.0, 20.18); - map.put(4.5, 22.24); - map.put(5.0, 24.27); - map.put(5.5, 26.23); - map.put(6.0, 28.14); + map.put(1.041, 18.0); + map.put(1.92, 23.0); + map.put(2.31, 24.0); + map.put(3.25, 27.5); + map.put(4.17, 29.0); + map.put(5.71, 32.0); + map.put(7.37, 43.0); } return map; } @@ -126,16 +116,13 @@ private static InterpolatingDoubleTreeMap generateTOFMap() { map.put(5.71, 1.64); map.put(7.37, 1.59); } else { - map.put(1.5, 1.1833); - map.put(2.0, 1.16); - map.put(2.5, 1.33); - map.put(3.0, 1.216); - map.put(3.5, 1.26); - map.put(4.0, 1.33); - map.put(4.5, 1.33); - map.put(5.0, 1.3); - map.put(5.5, 1.33); - map.put(6.0, 0.7); + map.put(1.041, 1.516); + map.put(1.92, 1.651); + map.put(2.31, 1.996); + map.put(3.25, 1.991); + map.put(4.17, 1.486); + map.put(5.71, 1.64); + map.put(7.37, 1.59); } return map; }