From 08e61dc68c74c1397fcc31c7559415691172d99a Mon Sep 17 00:00:00 2001 From: Joshua Reddick Date: Sat, 4 Apr 2026 11:58:17 -0400 Subject: [PATCH 1/7] MM on HOOD --- .../subsystems/shooter/ShooterConstants.java | 15 +++++++++++++++ .../subsystems/shooter/ShooterSubsystem.java | 7 +++++-- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 5881ae3..4f3ab56 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; @@ -60,6 +61,20 @@ public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() return configs; } + public static final double HOOD_MAX_VELOCITY_RPS = 5; + public static final double HOOD_MAX_ACCELERATION_RPS2 = 10; + public static final double HOOD_MAX_JERK_RPS2 = 10; + + + public static MotionMagicConfigs createHoodMotionMagicConfigs() { + MotionMagicConfigs configs = new MotionMagicConfigs(); + configs.MotionMagicCruiseVelocity = HOOD_MAX_VELOCITY_RPS; + configs.MotionMagicAcceleration = HOOD_MAX_ACCELERATION_RPS2; + configs.MotionMagicJerk = HOOD_MAX_JERK_RPS2; + + return configs; + } + public static final double FLYWHEEL_CURRENT_LIMIT = 40; public static CurrentLimitsConfigs createFlywheelCurrentLimitsConfigs() { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index fd0a0bb..01e7eb4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.epilogue.Logged; @@ -31,7 +32,7 @@ public class ShooterSubsystem extends SubsystemBase { @Logged(name = "Hood Target (radians)", importance = Importance.CRITICAL) private Angle hoodTarget; // radians is the base unit. - private PositionVoltage hoodControl; + private MotionMagicVoltage hoodControl; final SysIdRoutine m_sysIdRoutineFlywheel = new SysIdRoutine( @@ -74,10 +75,12 @@ public ShooterSubsystem() { hoodMotor.getConfigurator().apply(ShooterConstants.createHoodSoftwareLimitSwitchConfigs()); + hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotionMagicConfigs()); + hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorSlot0Configs()); hoodTarget = Rotations.of(0); - hoodControl = new PositionVoltage(hoodTarget); + hoodControl = new MotionMagicVoltage(hoodTarget); } private void setFlywheelVoltage(double magnitude) { From bbdeeb3856de65c18343e0b838eb54037b03929f Mon Sep 17 00:00:00 2001 From: William Crouch Date: Sat, 4 Apr 2026 14:50:29 -0400 Subject: [PATCH 2/7] Testing changes (basic issues) --- src/main/java/frc/robot/RobotContainer.java | 15 +- .../indexer/IndexerPreferences.java | 14 +- .../subsystems/indexer/IndexerSubsystem.java | 33 +--- .../subsystems/intake/IntakeSubsystem.java | 8 +- .../subsystems/shooter/ShooterConstants.java | 32 +-- .../shooter/ShooterPreferences.java | 37 ++-- .../subsystems/shooter/ShooterSubsystem.java | 186 +++--------------- .../subsystems/vision/VisionConstants.java | 11 +- 8 files changed, 75 insertions(+), 261 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index deccbe3..b848431 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -63,7 +63,6 @@ public class RobotContainer { private final DriveState driveState = DriveState.getInstance(); private final LaunchState launchState = LaunchState.getInstance(); - /* private final Command driveAndLaunchCommand = drivetrain .applyRequest(() -> getDriveAndLaunchRequest()) @@ -83,14 +82,13 @@ public class RobotContainer { .stopFullIndexingNoPID() .andThen(shooter.stopFlywheelCommand()) .andThen(shooter.stowHood()); - */ private final SendableChooser autoChooser; public RobotContainer() { NamedCommands.registerCommand("Seed", drivetrain.runOnce(drivetrain::seedFieldCentric)); - // NamedCommands.registerCommand("AutonShoot", autonShootCommand); - // NamedCommands.registerCommand("StopShot", stopShotCommand); + NamedCommands.registerCommand("AutonShoot", autonShootCommand); + NamedCommands.registerCommand("StopShot", stopShotCommand); NamedCommands.registerCommand("StopRoller", intake.stopRollerNoPID()); NamedCommands.registerCommand( "Collect Intake", @@ -213,14 +211,13 @@ public void configureTeleopBindings() { .leftBumper() .onTrue( intake - .stopRollerNoPID() + .stopRollerCommand() .andThen(intake.setIntakeExtensionCommand(IntakeConstants.INTAKE_REVERSE_LIMIT)) .withName("Stow Intake")); // stop the roller without retracting. driverJoystick.x().onTrue(intake.stopRollerNoPID()); - /* // outtake fuel. don't retract when done. driverJoystick .b() @@ -256,7 +253,6 @@ public void configureTeleopBindings() { .rightBumper() .whileTrue(shooter.spinFlywheelPostCommand()) .onFalse(shooter.stopFlywheelCommand().andThen(shooter.stowHood())); - */ // Reset the field-centric heading on start button press. driverJoystick @@ -302,11 +298,6 @@ public void configureTeleopBindings() { .manualRumbleCommand(driverJoystick.getHID()) .withName("Rumble Driver Controller")); - operatorJoystick.rightTrigger().whileTrue(indexer.startFullIndexingNoPID()); - operatorJoystick.a().onTrue(shooter.setHoodTargetCommand()); - operatorJoystick.leftBumper().onTrue(shooter.setFlywheelOutputCommand()); - operatorJoystick.rightBumper().onTrue(shooter.stopFlywheelOutputCommand()); - drivetrain.registerTelemetry(logger::telemeterize); SmartDashboard.putData( diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java b/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java index 67920cd..2a9c1e8 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java @@ -9,25 +9,15 @@ private IndexerPreferences() {} protected static DoublePreference indexerPercent = new DoublePreference("Indexer/Index Percent (for without PID)", 0.1); // in percent - protected static DoublePreference acceleratorPercent = - new DoublePreference("Indexer/Accelerator Percent (for without PID)", 0.1); // in percent - - /* - protected static DoublePreference indexSpeed = - new DoublePreference("Indexer/Index Speed (RotationsPS)", 0.1); // in rotations per second - protected static DoublePreference indexerReversePercent = new DoublePreference("Indexer/Reverse Index Percent (for without PID)", -0.1); // in percent - protected static DoublePreference accelerateSpeed = - new DoublePreference( - "Indexer/Accelerate Speed (RotationsPS)", 0.1); // in rotations per second - + protected static DoublePreference acceleratorPercent = + new DoublePreference("Indexer/Accelerator Percent (for without PID)", 0.1); // in percent protected static DoublePreference indexerRunTime = new DoublePreference("Indexer/Pulsing Run Time", 2.0); // in seconds protected static DoublePreference indexerPauseTime = new DoublePreference("Indexer/Pulsing Pause Time", 0.1); // in seconds - */ } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index b03b18f..2f90d58 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -6,8 +6,7 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.epilogue.Logged.Importance; -import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -17,12 +16,6 @@ public class IndexerSubsystem extends SubsystemBase { private final TalonFX indexerMotor; private final TalonFX acceleratorMotor; - @Logged(name = "Indexer Velocity Target", importance = Importance.CRITICAL) - private AngularVelocity indexerVelocityTarget; // RotationsPerSecond - - @Logged(name = "Accelerator Velocity Target", importance = Importance.CRITICAL) - private AngularVelocity acceleratorVelocityTarget; - private VelocityVoltage indexerControl; private VelocityVoltage acceleratorControl; @@ -86,22 +79,6 @@ private void setAcceleratorVoltage(double magnitude) { acceleratorMotor.setVoltage(magnitude); } - // Indexer Commands - public Command startIndexerNoPID() { - return runEnd( - () -> indexerMotor.set(IndexerPreferences.indexerPercent.getValue()), - () -> indexerMotor.set(0)) - .withName("Set Indexer Percent"); - } - - // Accelerator Commands - public Command startAcceleratorNoPID() { - return runEnd( - () -> acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()), - () -> acceleratorMotor.set(0)) - .withName("Set Acceleration Percent"); - } - public Command startFullIndexingNoPID() { return runEnd( () -> { @@ -123,9 +100,8 @@ public Command stopFullIndexingNoPID() { .withName("Stop Full Indexing No PID"); } - /* public Command startIndexerReverseNoPID() { - return run(() -> indexerMotor.set(IndexerPreferences.indexerReversePercent.getValue())) + return runOnce(() -> indexerMotor.set(IndexerPreferences.indexerReversePercent.getValue())) .withName("Set Indexer Reverse Percent"); } @@ -133,6 +109,8 @@ public Command stopIndexerNoPID() { return runOnce(() -> indexerMotor.set(0)).withName("Stop Indexer Percent"); } + /* + public Command stopAcceleratorNoPID() { return runOnce(() -> acceleratorMotor.set(0)).withName("Stop Accelerator Percent"); } @@ -172,6 +150,8 @@ public Command stopCommand() { .withName("Stop Indexing Lemons"); } + */ + public Command pulsingIndexCommand() { Timer timer = new Timer(); return runEnd( @@ -192,5 +172,4 @@ public Command pulsingIndexCommand() { .beforeStarting(() -> timer.restart()) .withName("Pulsing Index"); } - */ } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 677d837..baf3906 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -16,12 +16,10 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @Logged @@ -99,12 +97,14 @@ public IntakeSubsystem() { public void periodic() { // rollerMotor.setControl(rollerControl.withVelocity(rollerVelocityTarget.in(RotationsPerSecond))); + /* if (extensionMotor.getStatorCurrent().getValueAsDouble() > IntakePreferences.resistanceCurrentLimit.getValue() && isCompliantMode && RobotModeTriggers.teleop().getAsBoolean()) CommandScheduler.getInstance() .schedule(stopRollerNoPID().andThen(setIntakeExtensionCommand(0))); + */ extensionMotor.setControl( extensionControl @@ -161,10 +161,14 @@ public boolean atExtensionSetpoint() { } public Command setIntakeExtensionCommand(double rotations) { + /* return runOnce(() -> isCompliantMode = false) .andThen(runOnce(() -> extensionTarget = Rotations.of(rotations))) .andThen(Commands.waitUntil(() -> atExtensionSetpoint())) .finallyDo(() -> isCompliantMode = true); + */ + return runOnce(() -> extensionTarget = Rotations.of(rotations)) + .andThen(Commands.waitUntil(() -> atExtensionSetpoint())); } public Command setExtendNoPID() { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 4f3ab56..1ae593b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -7,9 +7,11 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Distance; public class ShooterConstants { @@ -65,10 +67,9 @@ public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() public static final double HOOD_MAX_ACCELERATION_RPS2 = 10; public static final double HOOD_MAX_JERK_RPS2 = 10; - public static MotionMagicConfigs createHoodMotionMagicConfigs() { MotionMagicConfigs configs = new MotionMagicConfigs(); - configs.MotionMagicCruiseVelocity = HOOD_MAX_VELOCITY_RPS; + configs.MotionMagicCruiseVelocity = HOOD_MAX_VELOCITY_RPS; configs.MotionMagicAcceleration = HOOD_MAX_ACCELERATION_RPS2; configs.MotionMagicJerk = HOOD_MAX_JERK_RPS2; @@ -95,17 +96,18 @@ public static CurrentLimitsConfigs createHoodCurrentLimitsConfigs() { public static final double HOOD_KS = 4.8; public static final double HOOD_KP = 9; - public static final double HOOD_KD = 0; + public static final double HOOD_KD = 0.1; + public static final double HOOD_KI = 1; public static Slot0Configs createHoodMotorSlot0Configs() { Slot0Configs slot = new Slot0Configs(); slot.kS = HOOD_KS; slot.kP = HOOD_KP; + slot.kI = HOOD_KI; slot.kD = HOOD_KD; return slot; } - // TODO: Tune Flywheel and Hood Motor // Flywheel motor @@ -121,33 +123,11 @@ public static Slot0Configs createFlywheelMotorSlot0Configs() { return slot; } - - /* public static final double ALLOWABLE_HOOD_ERROR = 0.1; - public static final double HOOD_FORWARD_LIMIT = 6.2; - public static final double HOOD_REVERSE_LIMIT = 0; - - public static SoftwareLimitSwitchConfigs createHoodSoftwareLimitSwitchConfigs() { - SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs(); - configs.ForwardSoftLimitEnable = true; - configs.ReverseSoftLimitEnable = true; - configs.ForwardSoftLimitThreshold = HOOD_FORWARD_LIMIT; - configs.ReverseSoftLimitThreshold = HOOD_REVERSE_LIMIT; - return configs; - } - - public static MotorOutputConfigs createHoodMotorOutputConfigs() { - MotorOutputConfigs newConfigs = new MotorOutputConfigs(); - newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; - newConfigs.NeutralMode = NeutralModeValue.Brake; - return newConfigs; - } - public static final DutyCycleOut SAFE_HOMING_EFFORT = new DutyCycleOut(-0.2); public static final Current SAFE_STATOR_LIMIT = Amp.of(0.8); - */ // Conversion Constants public static final Angle ROTATIONS_PER_LAUNCH_DEGREE = Rotations.of(0.2); public static final Distance FLYWHEEL_RADIUS = Inch.of(2); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java index 9708e39..c4ec371 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java @@ -5,32 +5,25 @@ public class ShooterPreferences { private ShooterPreferences() {} - public static DoublePreference hoodTargetPreference = - new DoublePreference("Shooter/Hood/Target Rotations", 0); + public static DoublePreference flywheelLaunchSpeed = + new DoublePreference("Shooter/Launch Speed", 0.1); // in rotations per second - public static DoublePreference flywheelLaunchPercent = - new DoublePreference("Shooter/Launch Percent (for without PID)", 0.1); // in percent + public static DoublePreference hoodLaunchAngle = + new DoublePreference("Shooter/Hood Launch Position", 3); // in rotations - /* - public static DoublePreference flywheelLaunchSpeed = - new DoublePreference("Shooter/Launch Speed", 0.1); // in rotations per second - public static DoublePreference hoodLaunchAngle = - new DoublePreference("Shooter/Hood Launch Position", 3); // in rotations + public static DoublePreference hoodkP = new DoublePreference("Shooter/Hood/kP", 1); + public static DoublePreference hoodkD = new DoublePreference("Shooter/Hood/kD", 0); - public static DoublePreference hoodkP = new DoublePreference("Shooter/Hood/kP", 1); - public static DoublePreference hoodkD = new DoublePreference("Shooter/Hood/kD", 0); + // unit is *radians per second** for velocity. + public static DoublePreference tuningDefaultFlywheelRPS = + new DoublePreference("Shooter/Tuning/FlywheelRPS", 1000.0 / 60.0 / 2 * Math.PI); + public static DoublePreference tuningDefaultFlywheelStepRPS = + new DoublePreference("Shooter/Tuning/FlywheelStepRPS", 100.0 / 60.0 / 2 * Math.PI); + public static DoublePreference tuningDefaultHoodRotations = + new DoublePreference("Shooter/Tuning/HoodRotations", 0.0); + public static DoublePreference tuningDefaultHoodStepRotations = + new DoublePreference("Shooter/Tuning/HoodStepRotations", 0.2); - // unit is *radians per second** for velocity. - public static DoublePreference tuningDefaultFlywheelRPS = - new DoublePreference("Shooter/Tuning/FlywheelRPS", 1000.0 / 60.0 / 2 * Math.PI); - public static DoublePreference tuningDefaultFlywheelStepRPS = - new DoublePreference("Shooter/Tuning/FlywheelStepRPS", 100.0 / 60.0 / 2 * Math.PI); - public static DoublePreference tuningDefaultHoodRotations = - new DoublePreference("Shooter/Tuning/HoodRotations", 0.0); - public static DoublePreference tuningDefaultHoodStepRotations = - new DoublePreference("Shooter/Tuning/HoodStepRotations", 0.2); - - */ public static DoublePreference passingFlywheelSpeed = new DoublePreference("Shooter/Tuning/Flywheel Pass Speed", 62); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 01e7eb4..4ae08f6 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -3,15 +3,19 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.Logged.Importance; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.statemachines.LaunchState; @Logged public class ShooterSubsystem extends SubsystemBase { @@ -20,14 +24,12 @@ public class ShooterSubsystem extends SubsystemBase { private final TalonFX flywheelMotorLeftFollower; private final TalonFX hoodMotor; - /* @Logged(name = "Velocity Target rads/s", importance = Importance.CRITICAL) private AngularVelocity velocityTarget; // *rads* Per Second is the base unit. private VelocityVoltage velocityControl; private final LaunchState launchState = LaunchState.getInstance(); - */ @Logged(name = "Hood Target (radians)", importance = Importance.CRITICAL) private Angle hoodTarget; // radians is the base unit. @@ -37,7 +39,7 @@ public class ShooterSubsystem extends SubsystemBase { final SysIdRoutine m_sysIdRoutineFlywheel = new SysIdRoutine( new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) + Volts.per(Second).of(0.2), // Use default ramp rate (1 V/s) Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout null, // Use default timeout (10 s) // Log state with SignalLogger class @@ -79,8 +81,16 @@ public ShooterSubsystem() { hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorSlot0Configs()); + flywheelMotorLeftFollower.setControl(new Follower(5, MotorAlignmentValue.Aligned)); + flywheelMotorRight.setControl(new Follower(5, MotorAlignmentValue.Opposed)); + hoodTarget = Rotations.of(0); hoodControl = new MotionMagicVoltage(hoodTarget); + + velocityTarget = RotationsPerSecond.of(0); + velocityControl = new VelocityVoltage(velocityTarget); + + hoodMotor.setPosition(0); } private void setFlywheelVoltage(double magnitude) { @@ -91,33 +101,10 @@ private void setFlywheelVoltage(double magnitude) { @Override public void periodic() { - /* launchState.refreshRequest(); - flywheelMotorLeader.setControl( - velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); - flywheelRightFollower.setControl( + flywheelMotorLeftLeader.setControl( velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); hoodMotor.setControl(hoodControl.withPosition(hoodTarget)); - flywheelLeftFollower.setControl( - velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); - */ - // hoodMotor.setControl(hoodControl.withPosition(hoodTarget)); - } - - public Command setHoodTargetCommand() { - return runOnce( - () -> hoodTarget = Rotations.of(ShooterPreferences.hoodTargetPreference.getValue())) - .withName("Set Hood Target"); - } - - public Command setFlywheelOutputCommand() { - return runOnce( - () -> setFlywheelMotorOutput(ShooterPreferences.flywheelLaunchPercent.getValue())) - .withName("Set Flywheel Output"); - } - - public Command stopFlywheelOutputCommand() { - return runOnce(() -> setFlywheelMotorOutput(0)).withName("Stop Flywheel Output"); } public void setFlywheelMotorOutput(double output) { @@ -126,17 +113,22 @@ public void setFlywheelMotorOutput(double output) { flywheelMotorRight.set(output); } - /* - @Logged(name = "Velocity Target RPM", importance = Importance.CRITICAL) - public double getFlywheelTargetRPM() { - return velocityTarget.in(RotationsPerSecond) * 60; - } - @Logged(name = "Hood Angle Rotations", importance = Importance.CRITICAL) public double getHoodTargetRotations() { return hoodTarget.in(Rotations); } + @Logged(name = "At Hood Setpoint", importance = Importance.CRITICAL) + public boolean atHoodSetpoint() { + return Math.abs(hoodMotor.getPosition().getValueAsDouble() - hoodTarget.in(Rotations)) + < ShooterConstants.ALLOWABLE_HOOD_ERROR; + } + + @Logged(name = "Velocity Target RPM", importance = Importance.CRITICAL) + public double getFlywheelTargetRPM() { + return velocityTarget.in(RotationsPerSecond) * 60; + } + public Command spinFlywheelCommand() { return runOnce( () -> { @@ -147,23 +139,6 @@ public Command spinFlywheelCommand() { .withName("Start Spinning Flywheel"); } - public Command spinFlywheelPostCommand() { - return runOnce( - () -> { - velocityTarget = RotationsPerSecond.of(74.5); - hoodTarget = Rotations.of(4); - }) - .withName("Start Spinning Flywheel"); - } - - public Command spinFlywheelCommand(AngularVelocity v) { - return runOnce(() -> velocityTarget = v).withName("Spinning To AV"); - } - - public Command spinFlyWheelCommand(DoublePreference d) { - return runOnce(() -> velocityTarget = RotationsPerSecond.of(d.getValue())); - } - public Command stopFlywheelCommand() { return runOnce(() -> velocityTarget = RotationsPerSecond.of(0)) .withName("Stop Spinning Flywheel"); @@ -173,21 +148,13 @@ public Command stowHood() { return runOnce(() -> hoodTarget = Rotations.of(0)); } - public Command setHoodToPreference() { + public Command spinFlywheelPostCommand() { return runOnce( - () -> hoodTarget = Rotations.of(ShooterPreferences.hoodTargetPreference.getValue())); - } - - @Logged(name = "At Hood Setpoint", importance = Importance.CRITICAL) - public boolean atHoodSetpoint() { - return Math.abs(hoodMotor.getPosition().getValueAsDouble() - hoodTarget.in(Rotations)) - < ShooterConstants.ALLOWABLE_HOOD_ERROR; - } - - public Command setHoodCommand(Angle position) { - return runOnce(() -> hoodTarget = position) - .andThen(Commands.waitUntil(() -> atHoodSetpoint())) - .withName("Set Hood Angle"); + () -> { + velocityTarget = RotationsPerSecond.of(74.5); + hoodTarget = Rotations.of(4); + }) + .withName("Start Spinning Flywheel"); } public Command spinFlywheelRanged() { @@ -198,40 +165,6 @@ public Command spinFlywheelRanged() { }); } - public Command spinFlywheelHardCoded() { - return run( - () -> { - velocityTarget = RotationsPerSecond.of(66.5); - hoodTarget = Rotations.of(2.38); - }); - } - - public Command launchLemonsCommandNoPID() { - return setHoodCommand(Rotations.of(ShooterPreferences.hoodLaunchAngle.getValue())) - .andThen( - runOnce( - () -> { - flywheelMotorLeader.set(ShooterPreferences.flywheelLaunchPercent.getValue()); - flywheelMotorFollower.set(ShooterPreferences.flywheelLaunchPercent.getValue()); - })) - .withName("Start Launching Lemons (No PID)"); - } - - public Command stopLaunchLemonsNoPIDCommand() { - return setHoodCommand(Rotations.of(0)) - .andThen( - runOnce( - () -> { - flywheelMotorLeader.set(0); - flywheelMotorFollower.set(0); - })) - .withName("Stop Launching Lemons (No PID)"); - } - - public Command stowCommand() { - return stopFlywheelCommand().andThen(setHoodCommand(Rotations.of(0))).withName("Stow Shooter"); - } - public Command homeShooterCommand() { return runEnd( () -> hoodMotor.set(ShooterConstants.SAFE_HOMING_EFFORT.Output), @@ -243,61 +176,6 @@ public Command homeShooterCommand() { }); } - public Command increaseFlywheelCommand() { - return runOnce( - () -> - velocityTarget = - RotationsPerSecond.of( - velocityTarget.in(RotationsPerSecond) - + ShooterPreferences.tuningDefaultFlywheelStepRPS.getValue())) - .withName("Increase FlyWheel Speed"); - } - - public Command decreaseFlywheelCommand() { - return runOnce( - () -> - velocityTarget = - RotationsPerSecond.of( - velocityTarget.in(RotationsPerSecond) - - ShooterPreferences.tuningDefaultFlywheelStepRPS.getValue())) - .withName("Decrease FlyWheel Speed"); - } - - public Command increaseHoodCommand() { - return runOnce( - () -> - hoodTarget = - Rotations.of( - hoodTarget.in(Rotations) - + ShooterPreferences.tuningDefaultHoodStepRotations.getValue())) - .withName("Increase Hood Angle"); - } - - public Command decreaseHoodCommand() { - return runOnce( - () -> - hoodTarget = - Rotations.of( - hoodTarget.in(Rotations) - - ShooterPreferences.tuningDefaultHoodStepRotations.getValue())) - .withName("Decrease Hood Angle"); - } - - public Command startShooterTuningCommand() { - return spinFlyWheelCommand(ShooterPreferences.tuningDefaultFlywheelRPS) - .andThen( - setHoodCommand(Rotations.of(ShooterPreferences.tuningDefaultHoodRotations.getValue()))) - .withName("Start Shooter Tuning"); - } - - public Command stopShooterTuningCommand() { - return stopFlywheelCommand() - .andThen( - setHoodCommand(Rotations.of(ShooterPreferences.tuningDefaultHoodRotations.getValue()))) - .withName("Stop Shooter Tuning"); - } - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return m_sysIdRoutineFlywheel.quasistatic(direction); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 6eb0399..992c0d4 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -36,18 +36,17 @@ public class VisionConstants { put( photonCameraName_Front, new Transform3d( - new Translation3d(-0.1638, 0, 0.7192), - new Rotation3d(0, Math.toRadians(-20), 0))); + new Translation3d(0.13, 0, 0.696), new Rotation3d(0, Math.toRadians(-22), 0))); put( photonCameraName_Left, new Transform3d( - new Translation3d(-0.2680, 0.3164, 0.451), - new Rotation3d(0, Math.toRadians(-20), Math.toRadians(120)))); + new Translation3d(-0.252, 0.328, 0.715), + new Rotation3d(0, 0, Math.toRadians(160)))); put( photonCameraName_Right, new Transform3d( - new Translation3d(-0.2680, -0.3164, 0.451), - new Rotation3d(0, Math.toRadians(-20), Math.toRadians(-120)))); + new Translation3d(-0.252, -0.328, 0.715), + new Rotation3d(0, 0, Math.toRadians(-160)))); } }; From 9f1098514ab863d7bd9bc2923339b982f0268653 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Sat, 4 Apr 2026 15:04:25 -0400 Subject: [PATCH 3/7] Switch of push --- src/main/java/frc/robot/RobotContainer.java | 5 ++--- .../java/frc/robot/subsystems/indexer/IndexerSubsystem.java | 4 ---- 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b848431..5c98158 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -235,9 +235,8 @@ public void configureTeleopBindings() { .whileTrue( drivetrain .setXCommand() - .alongWith(indexer.pulsingIndexCommand()) - .withName("Lock Wheels and Index")) - .onFalse(indexer.stopFullIndexingNoPID()); + .alongWith(indexer.startFullIndexingNoPID()) + .withName("Lock Wheels and Index")); operatorJoystick .leftTrigger() diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 2f90d58..0ef0b0e 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -3,7 +3,6 @@ import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj.Timer; @@ -16,9 +15,6 @@ public class IndexerSubsystem extends SubsystemBase { private final TalonFX indexerMotor; private final TalonFX acceleratorMotor; - private VelocityVoltage indexerControl; - private VelocityVoltage acceleratorControl; - final SysIdRoutine m_sysIdRoutineIndexer = new SysIdRoutine( new SysIdRoutine.Config( From b3560f03e4bc5a0bfbab29079e99790345bdbfc5 Mon Sep 17 00:00:00 2001 From: TheGreatPintoJ Date: Sat, 4 Apr 2026 15:14:26 -0400 Subject: [PATCH 4/7] timer --- src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 0ef0b0e..1cc0bc6 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -150,6 +150,7 @@ public Command stopCommand() { public Command pulsingIndexCommand() { Timer timer = new Timer(); + timer.start(); return runEnd( () -> { double cycleTime = From 007595a287392f66072d13f989968d8646911ba1 Mon Sep 17 00:00:00 2001 From: William Crouch Date: Sat, 4 Apr 2026 16:38:24 -0400 Subject: [PATCH 5/7] Probably last changes --- src/main/java/frc/robot/Robot.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 13 ++-- .../shooter/MappedLaunchRequestBuilder.java | 29 ++++++--- .../subsystems/shooter/ShooterSubsystem.java | 60 ++++++++++++++++++- 4 files changed, 84 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4ebb994..6c2448d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -129,6 +129,7 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_robotContainer.configureTeleopBindings(); } /** This function is called periodically during operator control. */ @@ -141,8 +142,7 @@ public void testInit() { CommandScheduler.getInstance().cancelAll(); SignalLogger.stop(); - // m_robotContainer.removeSubsystemDefaultCommands(); - // m_robotContainer.configureTestBindings(); + m_robotContainer.configureTestBindings(); } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5c98158..c805de9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -122,9 +122,8 @@ public RobotContainer() { LaunchState.getInstance() .setTargetPose3d(Constants.FieldConstants.getHubTarget()))) .withName("Rumble & Set Pose")); - + configureSubsystemDefaultCommands(); - configureTeleopBindings(); } public void configureSubsystemDefaultCommands() { @@ -161,11 +160,6 @@ public void configureSubsystemDefaultCommands() { .withName("Teleop Drive")); } - public void removeSubsystemDefaultCommands() { - drivetrain.removeDefaultCommand(); - } - - /* public void configureTestBindings() { // Idle while the robot is disabled. This ensures the configured // neutral mode is applied to the drive motors while disabled. @@ -192,14 +186,13 @@ public void configureTestBindings() { driverJoystick .a() - .whileTrue(driveAndLaunchCommand) + .whileTrue(driveAndLaunchCommand.alongWith(indexer.startFullIndexingNoPID())) .onFalse(shooter.stopFlywheelCommand().andThen(shooter.stowHood())); driverJoystick.b().onTrue(intake.testRollerNoPID()).onFalse(intake.stopRollerNoPID()); driverJoystick.x().onTrue(intake.spinRollerCommand()).onFalse(intake.stopRollerCommand()); } - */ public void configureTeleopBindings() { driverJoystick @@ -230,6 +223,8 @@ public void configureTeleopBindings() { .onFalse( intake.stopRollerNoPID().andThen(indexer.stopIndexerNoPID()).withName("Stop Roller")); + driverJoystick.rightTrigger().onTrue(shooter.spinFlywheelCommand()); + operatorJoystick .rightTrigger() .whileTrue( diff --git a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java index b68db1b..f26fb34 100644 --- a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java @@ -16,9 +16,6 @@ /** Add your docs here. */ public class MappedLaunchRequestBuilder implements LaunchRequestBuilder { - private static final double minDistance; - private static final double maxDistance; - // Launching Maps // private static final InterpolatingTreeMap hoodAngleMap = // new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate); @@ -26,25 +23,41 @@ public class MappedLaunchRequestBuilder implements LaunchRequestBuilder { private static final InterpolatingDoubleTreeMap flywheelSpeedMap = new InterpolatingDoubleTreeMap(); - // TODO: All of this is made up. Need real numbers. static { - minDistance = 0.9; - maxDistance = 4.0; + hoodAngleMap.put(1.3, 0.0); + hoodAngleMap.put(2.0, 0.6); + hoodAngleMap.put(2.6, 1.4); + hoodAngleMap.put(3.1, 1.8); + hoodAngleMap.put(3.21, 2.2); + hoodAngleMap.put(3.7, 2.6); + hoodAngleMap.put(4.2, 2.6); + hoodAngleMap.put(5.4, 3.0); + + flywheelSpeedMap.put(1.3, 326.0); + flywheelSpeedMap.put(2.0, 342.6); + flywheelSpeedMap.put(2.6, 359.0); + flywheelSpeedMap.put(3.1, 391.9); + flywheelSpeedMap.put(3.21, 391.0); + flywheelSpeedMap.put(3.7, 408.4); + flywheelSpeedMap.put(4.2, 424.8); + flywheelSpeedMap.put(5.4, 490.6); + /* Old shooter settings hoodAngleMap.put(0.99, 0.0); hoodAngleMap.put(1.62, 1.0); hoodAngleMap.put(1.94, 2.1); hoodAngleMap.put(2.53, 3.3); hoodAngleMap.put(3.00, 4.0); hoodAngleMap.put(3.51, 4.0); - hoodAngleMap.put(6.00, 6.1); // put in a value to max out the hood + hoodAngleMap.put(6.00, 6.1); flywheelSpeedMap.put(0.99, 57.7); flywheelSpeedMap.put(1.62, 64.3); flywheelSpeedMap.put(1.94, 64.7); flywheelSpeedMap.put(3.00, 74.5); flywheelSpeedMap.put(3.51, 80.0); - flywheelSpeedMap.put(6.00, 108.0); // put in a value to max out the hood + flywheelSpeedMap.put(6.00, 108.0); + */ } public LaunchRequest createLaunchRequest( diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 4ae08f6..144ed5e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -4,7 +4,7 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; @@ -34,7 +34,7 @@ public class ShooterSubsystem extends SubsystemBase { @Logged(name = "Hood Target (radians)", importance = Importance.CRITICAL) private Angle hoodTarget; // radians is the base unit. - private MotionMagicVoltage hoodControl; + private PositionVoltage hoodControl; final SysIdRoutine m_sysIdRoutineFlywheel = new SysIdRoutine( @@ -85,7 +85,7 @@ public ShooterSubsystem() { flywheelMotorRight.setControl(new Follower(5, MotorAlignmentValue.Opposed)); hoodTarget = Rotations.of(0); - hoodControl = new MotionMagicVoltage(hoodTarget); + hoodControl = new PositionVoltage(hoodTarget); velocityTarget = RotationsPerSecond.of(0); velocityControl = new VelocityVoltage(velocityTarget); @@ -148,6 +148,60 @@ public Command stowHood() { return runOnce(() -> hoodTarget = Rotations.of(0)); } + public Command increaseFlywheelCommand() { + return runOnce( + () -> + velocityTarget = + RotationsPerSecond.of( + velocityTarget.in(RotationsPerSecond) + + ShooterPreferences.tuningDefaultFlywheelStepRPS.getValue())) + .withName("Increase FlyWheel Speed"); + } + + public Command decreaseFlywheelCommand() { + return runOnce( + () -> + velocityTarget = + RotationsPerSecond.of( + velocityTarget.in(RotationsPerSecond) + - ShooterPreferences.tuningDefaultFlywheelStepRPS.getValue())) + .withName("Decrease FlyWheel Speed"); + } + + public Command increaseHoodCommand() { + return runOnce( + () -> + hoodTarget = + Rotations.of( + hoodTarget.in(Rotations) + + ShooterPreferences.tuningDefaultHoodStepRotations.getValue())) + .withName("Increase Hood Angle"); + } + + public Command decreaseHoodCommand() { + return runOnce( + () -> + hoodTarget = + Rotations.of( + hoodTarget.in(Rotations) + - ShooterPreferences.tuningDefaultHoodStepRotations.getValue())) + .withName("Decrease Hood Angle"); + } + + public Command startShooterTuningCommand() { + return runOnce( + () -> { + velocityTarget = + RotationsPerSecond.of(ShooterPreferences.tuningDefaultFlywheelRPS.getValue()); + hoodTarget = Rotations.of(ShooterPreferences.tuningDefaultHoodRotations.getValue()); + }) + .withName("Start Shooter Tuning"); + } + + public Command stopShooterTuningCommand() { + return stopFlywheelCommand().andThen(stowHood()).withName("Stop Shooter Tuning"); + } + public Command spinFlywheelPostCommand() { return runOnce( () -> { From d8039e29fefe4f206935eb1eeca69dce2acaef2a Mon Sep 17 00:00:00 2001 From: William Crouch Date: Sat, 4 Apr 2026 17:02:25 -0400 Subject: [PATCH 6/7] Final changes --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/indexer/IndexerSubsystem.java | 1 - .../robot/subsystems/shooter/MappedLaunchRequestBuilder.java | 4 ++-- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c805de9..853e2f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -122,7 +122,7 @@ public RobotContainer() { LaunchState.getInstance() .setTargetPose3d(Constants.FieldConstants.getHubTarget()))) .withName("Rumble & Set Pose")); - + configureSubsystemDefaultCommands(); } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 1cc0bc6..0ef0b0e 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -150,7 +150,6 @@ public Command stopCommand() { public Command pulsingIndexCommand() { Timer timer = new Timer(); - timer.start(); return runEnd( () -> { double cycleTime = diff --git a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java index f26fb34..80f3a7e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java @@ -49,14 +49,14 @@ public class MappedLaunchRequestBuilder implements LaunchRequestBuilder { hoodAngleMap.put(2.53, 3.3); hoodAngleMap.put(3.00, 4.0); hoodAngleMap.put(3.51, 4.0); - hoodAngleMap.put(6.00, 6.1); + hoodAngleMap.put(6.00, 6.1); flywheelSpeedMap.put(0.99, 57.7); flywheelSpeedMap.put(1.62, 64.3); flywheelSpeedMap.put(1.94, 64.7); flywheelSpeedMap.put(3.00, 74.5); flywheelSpeedMap.put(3.51, 80.0); - flywheelSpeedMap.put(6.00, 108.0); + flywheelSpeedMap.put(6.00, 108.0); */ } From 367082eeafe30a83c08c922c87223fcb912bb97e Mon Sep 17 00:00:00 2001 From: William Crouch Date: Sun, 5 Apr 2026 16:43:45 -0400 Subject: [PATCH 7/7] Units fix --- .../shooter/MappedLaunchRequestBuilder.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java index 80f3a7e..91cf73f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java @@ -33,14 +33,14 @@ public class MappedLaunchRequestBuilder implements LaunchRequestBuilder { hoodAngleMap.put(4.2, 2.6); hoodAngleMap.put(5.4, 3.0); - flywheelSpeedMap.put(1.3, 326.0); - flywheelSpeedMap.put(2.0, 342.6); - flywheelSpeedMap.put(2.6, 359.0); - flywheelSpeedMap.put(3.1, 391.9); - flywheelSpeedMap.put(3.21, 391.0); - flywheelSpeedMap.put(3.7, 408.4); - flywheelSpeedMap.put(4.2, 424.8); - flywheelSpeedMap.put(5.4, 490.6); + flywheelSpeedMap.put(1.3, 51.9); + flywheelSpeedMap.put(2.0, 54.5); + flywheelSpeedMap.put(2.6, 57.1); + flywheelSpeedMap.put(3.1, 62.4); + flywheelSpeedMap.put(3.21, 62.2); + flywheelSpeedMap.put(3.7, 65.0); + flywheelSpeedMap.put(4.2, 67.6); + flywheelSpeedMap.put(5.4, 78.1); /* Old shooter settings hoodAngleMap.put(0.99, 0.0);