diff --git a/motor-ids.txt b/motor-ids.txt new file mode 100644 index 0000000..fc2ebeb --- /dev/null +++ b/motor-ids.txt @@ -0,0 +1,9 @@ +Intake + Roller 2 + Extension 1 +Indexer + Main: 4 + Accelerator: 3 +Shooter + Flywheel - x3: 5,6,7 + Hood: 8 \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 68f19c6..4ebb994 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.statemachines.LaunchState; import frc.robot.statemachines.ShiftState; /** @@ -106,6 +105,7 @@ public void disabledPeriodic() {} /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { + /* m_autonomousCommand = m_robotContainer.getAutonomousCommand(); LaunchState.getInstance().setTargetPose3d(Constants.FieldConstants.getHubTarget()); @@ -113,6 +113,7 @@ public void autonomousInit() { if (m_autonomousCommand != null) { CommandScheduler.getInstance().schedule(m_autonomousCommand); } + */ } /** This function is called periodically during autonomous. */ @@ -140,8 +141,8 @@ public void testInit() { CommandScheduler.getInstance().cancelAll(); SignalLogger.stop(); - m_robotContainer.removeSubsystemDefaultCommands(); - m_robotContainer.configureTestBindings(); + // m_robotContainer.removeSubsystemDefaultCommands(); + // 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 c342680..ad4807d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,6 +36,7 @@ public class RobotContainer { private final Telemetry logger = new Telemetry(DriveConstants.MAX_DRIVE_SPEED); private final CommandXboxController driverJoystick = new CommandXboxController(0); + private final CommandXboxController operatorJoystick = new CommandXboxController(1); @Logged(name = "Drivetrain") @@ -50,9 +51,6 @@ public class RobotContainer { @Logged(name = "Shooter") public final ShooterSubsystem shooter = new ShooterSubsystem(); - // @Logged(name = "Climber") - // public final ClimberSubsystem climber = new ClimberSubsystem(); - @Logged(name = "Vision") public final VisionSubsystem vision = new VisionSubsystem(); @@ -63,6 +61,7 @@ public class RobotContainer { private final DriveState driveState = DriveState.getInstance(); private final LaunchState launchState = LaunchState.getInstance(); + /* private final Command driveAndLaunchCommand = drivetrain .applyRequest(() -> getDriveAndLaunchRequest()) @@ -82,13 +81,14 @@ 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", @@ -103,7 +103,6 @@ public RobotContainer() { NamedCommands.registerCommand( "HP Reload", new WaitCommand(IntakePreferences.outpostReloadWait.getValue())); autoChooser = AutoBuilder.buildAutoChooser("Auto Chooser"); - autoChooser.addOption("Auton Shoot", autonShootCommand); SmartDashboard.putData("Auto Mode", autoChooser); // Idle while the robot is disabled. This ensures the configured @@ -166,6 +165,7 @@ 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. @@ -202,9 +202,9 @@ public void configureTestBindings() { driverJoystick.x().onTrue(intake.spinRollerCommand()).onFalse(intake.stopRollerCommand()); } + */ public void configureTeleopBindings() { - driverJoystick .rightBumper() // .whileTrue(intake.setExtendNoPID()) @@ -218,6 +218,10 @@ public void configureTeleopBindings() { .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() @@ -230,9 +234,6 @@ public void configureTeleopBindings() { .onFalse( intake.stopRollerNoPID().andThen(indexer.stopIndexerNoPID()).withName("Stop Roller")); - // stop the roller without retracting. - driverJoystick.x().onTrue(intake.stopRollerNoPID()); - operatorJoystick .rightTrigger() .whileTrue( @@ -256,6 +257,7 @@ public void configureTeleopBindings() { .rightBumper() .whileTrue(shooter.spinFlywheelPostCommand()) .onFalse(shooter.stopFlywheelCommand().andThen(shooter.stowHood())); + */ // Reset the field-centric heading on start button press. driverJoystick @@ -301,6 +303,11 @@ 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); } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java deleted file mode 100644 index eb32b6e..0000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.subsystems.climber; - -import static edu.wpi.first.units.Units.Amp; -import static edu.wpi.first.units.Units.Rotations; - -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; -import com.ctre.phoenix6.controls.DutyCycleOut; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Current; - -final class ClimberConstants { - - private ClimberConstants() {} - - // TODO move to real ID when implemented. - protected static final int CLIMB_MOTOR_ID = 19; - - // TODO: Tune motor - protected static final Angle ALLOWABLE_CLIMB_ERROR = Rotations.of(0.1); - protected static final Angle CLIMB_FORWARD_LIMIT = Rotations.of(100); - protected static final Angle CLIMB_REVERSE_LIMIT = Rotations.of(0); - protected static final double CLIMB_KS = 0; - protected static final double CLIMB_KP = 0; - protected static final double CLIMB_KD = 0; - - protected static Slot0Configs createClimbMotorSlot0Configs() { - Slot0Configs slot = new Slot0Configs(); - slot.kS = CLIMB_KS; - slot.kP = CLIMB_KP; - slot.kD = CLIMB_KD; - return slot; - } - - protected static SoftwareLimitSwitchConfigs createClimbSoftwareLimitSwitchConfigs() { - SoftwareLimitSwitchConfigs configs = new SoftwareLimitSwitchConfigs(); - configs.ForwardSoftLimitEnable = false; - configs.ReverseSoftLimitEnable = false; - configs.ForwardSoftLimitThreshold = CLIMB_FORWARD_LIMIT.in(Rotations); - configs.ReverseSoftLimitThreshold = CLIMB_REVERSE_LIMIT.in(Rotations); - return configs; - } - - protected static final DutyCycleOut SAFE_HOMING_EFFORT = new DutyCycleOut(-0.2); - protected static final Current SAFE_STATOR_LIMIT = Amp.of(0.8); -} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberPreferences.java b/src/main/java/frc/robot/subsystems/climber/ClimberPreferences.java deleted file mode 100644 index b40e1d4..0000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberPreferences.java +++ /dev/null @@ -1,9 +0,0 @@ -package frc.robot.subsystems.climber; - -import frc.robot.preferences.DoublePreference; - -final class ClimberPreferences { - - protected static DoublePreference pullPosition = - new DoublePreference("Climber/Pull Down (Rotations)", 3); // in rotations -} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java deleted file mode 100644 index 9f9ceee..0000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ /dev/null @@ -1,68 +0,0 @@ -package frc.robot.subsystems.climber; - -import static edu.wpi.first.units.Units.Amp; -import static edu.wpi.first.units.Units.Rotations; - -import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; -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.Angle; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -@Logged -public class ClimberSubsystem extends SubsystemBase { - private final TalonFX climbMotor; - - @Logged(name = "Climb Target", importance = Importance.CRITICAL) - private Angle climbTarget; // Rotations - - private PositionTorqueCurrentFOC climbControl; - - public ClimberSubsystem() { - climbMotor = new TalonFX(ClimberConstants.CLIMB_MOTOR_ID); - - climbMotor.getConfigurator().apply(ClimberConstants.createClimbMotorSlot0Configs()); - climbMotor.getConfigurator().apply(ClimberConstants.createClimbSoftwareLimitSwitchConfigs()); - climbMotor.setPosition(0); - climbTarget = Rotations.of(0); - climbControl = new PositionTorqueCurrentFOC(0); - } - - @Override - public void periodic() { - climbMotor.setControl(climbControl.withPosition(climbTarget)); - } - - @Logged(name = "Climb Setpoint", importance = Importance.CRITICAL) - public boolean atClimbSetpoint() { - return Math.abs(climbMotor.getPosition().getValueAsDouble() - climbTarget.in(Rotations)) - < ClimberConstants.ALLOWABLE_CLIMB_ERROR.in(Rotations); - } - - public Command climbToPosition(Angle position) { - return runOnce(() -> climbTarget = position) - .andThen(Commands.waitUntil(() -> atClimbSetpoint())); - } - - public Command resetClimb() { - return climbToPosition(Rotations.of(0)); - } - - public Command climbToPreference() { - return climbToPosition(Rotations.of(ClimberPreferences.pullPosition.getValue())); - } - - public Command homeClimber() { - return runEnd( - () -> climbMotor.set(ClimberConstants.SAFE_HOMING_EFFORT.Output), - () -> climbMotor.setPosition(0)) - .until( - () -> { - return climbMotor.getStatorCurrent().getValueAsDouble() - > ClimberConstants.SAFE_STATOR_LIMIT.in(Amp); - }); - } -} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index c6823a6..c78046a 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -10,12 +10,10 @@ final class IndexerConstants { private IndexerConstants() {} - // TODO Replace with real id - protected static final int INDEXER_MOTOR_LEADER_ID = 3; - protected static final int INDEXER_MOTOR_FOLLOWER_ID = 9; - protected static final int ACCELERATOR_MOTOR_ID = 8; + protected static final int INDEXER_MOTOR_LEADER_ID = 4; + protected static final int ACCELERATOR_MOTOR_ID = 3; - // TODO: Tune motor + // TODO: PID tune motor protected static final double INDEXER_KS = 0; protected static final double INDEXER_KV = 0; @@ -45,20 +43,13 @@ protected static Slot0Configs createAcceleratorMotorSlot0Configs() { return slot; } - public static MotorOutputConfigs createLeaderMotorOutputConfigs() { + public static MotorOutputConfigs createIndexerMotorOutputConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; newConfigs.NeutralMode = NeutralModeValue.Brake; return newConfigs; } - public static MotorOutputConfigs createFollowerMotorOutputConfigs() { - MotorOutputConfigs newConfigs = new MotorOutputConfigs(); - newConfigs.Inverted = InvertedValue.Clockwise_Positive; - newConfigs.NeutralMode = NeutralModeValue.Brake; - return newConfigs; - } - public static final double INDEXER_CURRENT_LIMIT = 40; public static CurrentLimitsConfigs createIndexerCurrentLimitsConfigs() { @@ -70,7 +61,7 @@ public static CurrentLimitsConfigs createIndexerCurrentLimitsConfigs() { public static MotorOutputConfigs createAcceleratorMotorOutputsConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); - newConfigs.Inverted = InvertedValue.Clockwise_Positive; + newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; newConfigs.NeutralMode = NeutralModeValue.Brake; return newConfigs; } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java b/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java index 90984ca..67920cd 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java @@ -6,21 +6,28 @@ final class IndexerPreferences { private IndexerPreferences() {} - protected static DoublePreference indexSpeed = - new DoublePreference("Indexer/Index Speed", 0.1); // in rotations per second - 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 acceleratorPercent = - new DoublePreference("Indexer/Accelerator 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 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 6e5bd60..b03b18f 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -1,25 +1,20 @@ package frc.robot.subsystems.indexer; -import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.SignalLogger; -import com.ctre.phoenix6.controls.Follower; 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.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; @Logged public class IndexerSubsystem extends SubsystemBase { - private final TalonFX indexerMotorLeader; - private final TalonFX indexerMotorFollower; + private final TalonFX indexerMotor; private final TalonFX acceleratorMotor; @Logged(name = "Indexer Velocity Target", importance = Importance.CRITICAL) @@ -53,99 +48,128 @@ public class IndexerSubsystem extends SubsystemBase { output -> setAcceleratorVoltage(output.magnitude()), null, this)); public IndexerSubsystem() { - indexerMotorLeader = new TalonFX(IndexerConstants.INDEXER_MOTOR_LEADER_ID); - indexerMotorFollower = new TalonFX(IndexerConstants.INDEXER_MOTOR_FOLLOWER_ID); + indexerMotor = new TalonFX(IndexerConstants.INDEXER_MOTOR_LEADER_ID); acceleratorMotor = new TalonFX(IndexerConstants.ACCELERATOR_MOTOR_ID); - indexerMotorLeader.getConfigurator().apply(IndexerConstants.createLeaderMotorOutputConfigs()); - indexerMotorFollower - .getConfigurator() - .apply(IndexerConstants.createFollowerMotorOutputConfigs()); - - indexerMotorLeader.getConfigurator().apply(IndexerConstants.createIndexerMotorSlot0Configs()); - indexerMotorFollower.getConfigurator().apply(IndexerConstants.createIndexerMotorSlot0Configs()); - - acceleratorMotor.getConfigurator().apply(IndexerConstants.createAcceleratorMotorSlot0Configs()); + indexerMotor.getConfigurator().apply(IndexerConstants.createIndexerMotorOutputConfigs()); acceleratorMotor .getConfigurator() .apply(IndexerConstants.createAcceleratorMotorOutputsConfigs()); - indexerMotorFollower.setControl( - new Follower(indexerMotorLeader.getDeviceID(), MotorAlignmentValue.Opposed)); + indexerMotor.getConfigurator().apply(IndexerConstants.createIndexerCurrentLimitsConfigs()); + acceleratorMotor + .getConfigurator() + .apply(IndexerConstants.createAcceleratorCurrentLimitsConfigs()); + /* indexerVelocityTarget = RotationsPerSecond.of(0); acceleratorVelocityTarget = RotationsPerSecond.of(0); indexerControl = new VelocityVoltage(0); acceleratorControl = new VelocityVoltage(0); + */ } @Override public void periodic() { - // TODO: removed for testing only. PUT IT BACK! + // TODO: removed for pre-pidtuning. PUT IT BACK! // indexerMotor.setControl( // indexerControl.withVelocity(indexerVelocityTarget.in(RotationsPerSecond))); } + // SysID Helpers private void setIndexerVoltage(double magnitude) { - indexerMotorLeader.setVoltage(magnitude); + indexerMotor.setVoltage(magnitude); } private void setAcceleratorVoltage(double magnitude) { acceleratorMotor.setVoltage(magnitude); } + // Indexer Commands public Command startIndexerNoPID() { - return run(() -> indexerMotorLeader.set(IndexerPreferences.indexerPercent.getValue())) + return runEnd( + () -> indexerMotor.set(IndexerPreferences.indexerPercent.getValue()), + () -> indexerMotor.set(0)) .withName("Set Indexer Percent"); } - public Command startIndexerReverseNoPID() { - return run(() -> indexerMotorLeader.set(IndexerPreferences.indexerReversePercent.getValue())) - .withName("Set Indexer Reverse Percent"); - } - + // Accelerator Commands public Command startAcceleratorNoPID() { - return run(() -> acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue())) + return runEnd( + () -> acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()), + () -> acceleratorMotor.set(0)) .withName("Set Acceleration Percent"); } - public Command stopIndexerNoPID() { - return runOnce(() -> indexerMotorLeader.set(0)).withName("Stop Indexer Percent"); - } - - public Command stopAcceleratorNoPID() { - return runOnce(() -> acceleratorMotor.set(0)).withName("Stop Accelerator Percent"); - } - public Command startFullIndexingNoPID() { - return run(() -> { - indexerMotorLeader.set(IndexerPreferences.indexerPercent.getValue()); + return runEnd( + () -> { + indexerMotor.set(IndexerPreferences.indexerPercent.getValue()); acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()); - }) - .withName("Start Full Indexing No PID"); + }, + () -> { + indexerMotor.set(0); + acceleratorMotor.set(0); + }); } public Command stopFullIndexingNoPID() { return runOnce( () -> { - indexerMotorLeader.set(0); + indexerMotor.set(0); acceleratorMotor.set(0); }) .withName("Stop Full Indexing No PID"); } + /* + public Command startIndexerReverseNoPID() { + return run(() -> indexerMotor.set(IndexerPreferences.indexerReversePercent.getValue())) + .withName("Set Indexer Reverse Percent"); + } + + public Command stopIndexerNoPID() { + return runOnce(() -> indexerMotor.set(0)).withName("Stop Indexer Percent"); + } + + public Command stopAcceleratorNoPID() { + return runOnce(() -> acceleratorMotor.set(0)).withName("Stop Accelerator Percent"); + } + + // Mainly Used Commands + public Command indexCommand() { return runOnce( - () -> - indexerVelocityTarget = - RotationsPerSecond.of(IndexerPreferences.indexSpeed.getValue())) + () -> { + indexerVelocityTarget = + RotationsPerSecond.of(IndexerPreferences.indexSpeed.getValue()); + acceleratorVelocityTarget = + RotationsPerSecond.of(IndexerPreferences.accelerateSpeed.getValue()); + } + .withName("Index Lemons"); + // TODO: Removed for pre-pidtuning + return run( + () -> { + indexerMotor.set(IndexerPreferences.indexerPercent.getValue()); + acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()); + }) .withName("Index Lemons"); } public Command stopCommand() { - return runOnce(() -> indexerVelocityTarget = RotationsPerSecond.of(0)) - .withName("Stop Indexing"); + return runOnce( + () -> { PUT IT BACK! + indexerVelocityTarget = RotationsPerSecond.of(0)); + acceleratorVelocityTarget = RotationsPerSecond.of(0); + }).withName("Stop Indexing"); + // TODO: Removed for pre-pidtuning + return runOnce( + () -> { + indexerMotor.set(0); + acceleratorMotor.set(0); + }) + .withName("Stop Indexing Lemons"); } public Command pulsingIndexCommand() { @@ -158,14 +182,15 @@ public Command pulsingIndexCommand() { double elapsed = timer.get() % cycleTime; boolean shouldRun = elapsed < IndexerPreferences.indexerRunTime.getValue(); - indexerMotorLeader.set(shouldRun ? IndexerPreferences.indexerPercent.getValue() : 0); + indexerMotor.set(shouldRun ? IndexerPreferences.indexerPercent.getValue() : 0); acceleratorMotor.set(IndexerPreferences.acceleratorPercent.getValue()); }, () -> { - indexerMotorLeader.set(0); + indexerMotor.set(0); acceleratorMotor.set(0); }) .beforeStarting(() -> timer.restart()) .withName("Pulsing Index"); } + */ } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 151b4b2..11b8682 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -6,50 +6,58 @@ 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 { private ShooterConstants() {} - public static final int FLYWHEEL_LEADER_MOTOR_ID = 5; - public static final int FLYWHEEL_FOLLOWER_MOTOR_ID = 6; - public static final int HOOD_MOTOR_ID = 4; + public static final int FLYWHEEL_LEFT_LEADER_MOTOR_ID = 5; + public static final int FLYWHEEL_RIGHT_MOTOR_ID = 6; + public static final int FLYWHEEL_LEFT_FOLLOWER_MOTOR_ID = 7; + public static final int HOOD_MOTOR_ID = 8; - public static MotorOutputConfigs createLeaderMotorOutputConfigs() { + public static MotorOutputConfigs createRightFlywheelMotorOutputConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); - newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + newConfigs.Inverted = InvertedValue.Clockwise_Positive; newConfigs.NeutralMode = NeutralModeValue.Coast; return newConfigs; } - public static MotorOutputConfigs createFollowerMotorOutputConfigs() { + public static MotorOutputConfigs createLeftFlywheelLeaderMotorOutputConfigs() { MotorOutputConfigs newConfigs = new MotorOutputConfigs(); newConfigs.Inverted = InvertedValue.Clockwise_Positive; newConfigs.NeutralMode = NeutralModeValue.Coast; return newConfigs; } - // TODO: Tune Flywheel and Hood Motor + public static MotorOutputConfigs createLeftFlywheelFollowerMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Coast; + return newConfigs; + } - // Flywheel motor - public static final double FLYWHEEL_KS = 0.11239; - public static final double FLYWHEEL_KV = 0.11589; - public static final double FLYWHEEL_KA = 0.0034356; - public static final double FLYWHEEL_KP = 0.066945 * 2; + public static MotorOutputConfigs createHoodMotorOutputConfigs() { + MotorOutputConfigs newConfigs = new MotorOutputConfigs(); + newConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + newConfigs.NeutralMode = NeutralModeValue.Brake; + return newConfigs; + } - public static Slot0Configs createFlywheelMotorSlot0Configs() { - Slot0Configs slot = new Slot0Configs(); - slot.kS = FLYWHEEL_KS; - slot.kV = FLYWHEEL_KV; - slot.kP = FLYWHEEL_KP; - slot.kA = FLYWHEEL_KA; - return slot; + public static final double HOOD_FORWARD_LIMIT = 4.9; + 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 final double FLYWHEEL_CURRENT_LIMIT = 40; @@ -61,11 +69,17 @@ public static CurrentLimitsConfigs createFlywheelCurrentLimitsConfigs() { return configs; } - 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 final double HOOD_KS = 0; - public static final double HOOD_KP = 2.0; + public static final double HOOD_CURRENT_LIMIT = 40; + + public static CurrentLimitsConfigs createHoodCurrentLimitsConfigs() { + CurrentLimitsConfigs configs = new CurrentLimitsConfigs(); + configs.StatorCurrentLimit = HOOD_CURRENT_LIMIT; + configs.StatorCurrentLimitEnable = true; + return configs; + } + + public static final double HOOD_KS = 4.8; + public static final double HOOD_KP = 9; public static final double HOOD_KD = 0; public static Slot0Configs createHoodMotorSlot0Configs() { @@ -76,6 +90,29 @@ public static Slot0Configs createHoodMotorSlot0Configs() { return slot; } + /* + // TODO: Tune Flywheel and Hood Motor + + // Flywheel motor + public static final double FLYWHEEL_KS = 0.11239; + public static final double FLYWHEEL_KV = 0.11589; + public static final double FLYWHEEL_KA = 0.0034356; + public static final double FLYWHEEL_KP = 0.066945 * 2; + + public static Slot0Configs createFlywheelMotorSlot0Configs() { + Slot0Configs slot = new Slot0Configs(); + slot.kS = FLYWHEEL_KS; + slot.kV = FLYWHEEL_KV; + slot.kP = FLYWHEEL_KP; + slot.kA = FLYWHEEL_KA; + 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; @@ -92,18 +129,10 @@ public static MotorOutputConfigs createHoodMotorOutputConfigs() { return newConfigs; } - public static final double HOOD_CURRENT_LIMIT = 40; - - public static CurrentLimitsConfigs createHoodCurrentLimitsConfigs() { - CurrentLimitsConfigs configs = new CurrentLimitsConfigs(); - configs.StatorCurrentLimit = HOOD_CURRENT_LIMIT; - configs.StatorCurrentLimitEnable = true; - return configs; - } - 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 1018c9b..9708e39 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPreferences.java @@ -5,30 +5,32 @@ public class ShooterPreferences { private ShooterPreferences() {} - 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 flywheelLaunchPercent = - new DoublePreference("Shooter/Launch Percent (for without PID)", 0.1); // in percent - - public static DoublePreference hoodkP = new DoublePreference("Shooter/Hood/kP", 1); - public static DoublePreference hoodkD = new DoublePreference("Shooter/Hood/kD", 0); - public static DoublePreference hoodTargetPreference = new DoublePreference("Shooter/Hood/Target Rotations", 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); + public static DoublePreference flywheelLaunchPercent = + new DoublePreference("Shooter/Launch Percent (for without PID)", 0.1); // in percent + /* + 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); + + // 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 bfc9238..fd0a0bb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -4,37 +4,35 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.controls.PositionVoltage; -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.Angle; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.preferences.DoublePreference; -import frc.robot.statemachines.LaunchState; @Logged public class ShooterSubsystem extends SubsystemBase { - private final TalonFX flywheelMotorLeader; - private final TalonFX flywheelMotorFollower; + private final TalonFX flywheelMotorLeftLeader; + private final TalonFX flywheelMotorRight; + 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. private PositionVoltage hoodControl; - private final LaunchState launchState = LaunchState.getInstance(); - final SysIdRoutine m_sysIdRoutineFlywheel = new SysIdRoutine( new SysIdRoutine.Config( @@ -46,47 +44,86 @@ public class ShooterSubsystem extends SubsystemBase { new SysIdRoutine.Mechanism(output -> setFlywheelVoltage(output.magnitude()), null, this)); public ShooterSubsystem() { - flywheelMotorLeader = new TalonFX(ShooterConstants.FLYWHEEL_LEADER_MOTOR_ID); - flywheelMotorFollower = new TalonFX(ShooterConstants.FLYWHEEL_FOLLOWER_MOTOR_ID); + flywheelMotorLeftLeader = new TalonFX(ShooterConstants.FLYWHEEL_LEFT_LEADER_MOTOR_ID); + flywheelMotorLeftFollower = new TalonFX(ShooterConstants.FLYWHEEL_LEFT_FOLLOWER_MOTOR_ID); + flywheelMotorRight = new TalonFX(ShooterConstants.FLYWHEEL_RIGHT_MOTOR_ID); hoodMotor = new TalonFX(ShooterConstants.HOOD_MOTOR_ID); - flywheelMotorLeader.getConfigurator().apply(ShooterConstants.createFlywheelMotorSlot0Configs()); - flywheelMotorLeader.getConfigurator().apply(ShooterConstants.createLeaderMotorOutputConfigs()); - flywheelMotorLeader + flywheelMotorLeftLeader .getConfigurator() - .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); + .apply(ShooterConstants.createLeftFlywheelLeaderMotorOutputConfigs()); + flywheelMotorLeftFollower + .getConfigurator() + .apply(ShooterConstants.createLeftFlywheelFollowerMotorOutputConfigs()); + flywheelMotorRight + .getConfigurator() + .apply(ShooterConstants.createRightFlywheelMotorOutputConfigs()); + hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorOutputConfigs()); - flywheelMotorFollower + flywheelMotorLeftLeader .getConfigurator() - .apply(ShooterConstants.createFlywheelMotorSlot0Configs()); - flywheelMotorFollower + .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); + flywheelMotorLeftFollower .getConfigurator() - .apply(ShooterConstants.createFollowerMotorOutputConfigs()); - flywheelMotorFollower + .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); + flywheelMotorRight .getConfigurator() .apply(ShooterConstants.createFlywheelCurrentLimitsConfigs()); - velocityTarget = RotationsPerSecond.of(0); - velocityControl = new VelocityVoltage(0); + hoodMotor.getConfigurator().apply(ShooterConstants.createHoodCurrentLimitsConfigs()); - hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorSlot0Configs()); hoodMotor.getConfigurator().apply(ShooterConstants.createHoodSoftwareLimitSwitchConfigs()); - hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorOutputConfigs()); - hoodMotor.getConfigurator().apply(ShooterConstants.createHoodCurrentLimitsConfigs()); + + hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorSlot0Configs()); + hoodTarget = Rotations.of(0); - hoodControl = new PositionVoltage(0); + hoodControl = new PositionVoltage(hoodTarget); + } + + private void setFlywheelVoltage(double magnitude) { + flywheelMotorLeftLeader.setVoltage(magnitude); + flywheelMotorLeftFollower.setVoltage(magnitude); + flywheelMotorRight.setVoltage(magnitude); } @Override public void periodic() { + /* launchState.refreshRequest(); flywheelMotorLeader.setControl( velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); - flywheelMotorFollower.setControl( + flywheelRightFollower.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) { + flywheelMotorLeftLeader.set(output); + flywheelMotorLeftFollower.set(output); + flywheelMotorRight.set(output); + } + + /* @Logged(name = "Velocity Target RPM", importance = Importance.CRITICAL) public double getFlywheelTargetRPM() { return velocityTarget.in(RotationsPerSecond) * 60; @@ -138,11 +175,6 @@ public Command setHoodToPreference() { () -> hoodTarget = Rotations.of(ShooterPreferences.hoodTargetPreference.getValue())); } - private void setFlywheelVoltage(double magnitude) { - flywheelMotorLeader.setVoltage(magnitude); - flywheelMotorFollower.setVoltage(magnitude); - } - @Logged(name = "At Hood Setpoint", importance = Importance.CRITICAL) public boolean atHoodSetpoint() { return Math.abs(hoodMotor.getPosition().getValueAsDouble() - hoodTarget.in(Rotations)) @@ -261,6 +293,7 @@ public Command stopShooterTuningCommand() { setHoodCommand(Rotations.of(ShooterPreferences.tuningDefaultHoodRotations.getValue()))) .withName("Stop Shooter Tuning"); } + */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return m_sysIdRoutineFlywheel.quasistatic(direction);