From ab9f726bc239b540a885ce42becb01a42b876d71 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sun, 22 Mar 2026 17:05:25 -0400 Subject: [PATCH] Systems check: All good --- src/main/java/frc/robot/Constants.java | 16 ++++++++-------- src/main/java/frc/robot/RobotContainer.java | 17 ++++------------- .../commands/AutoShootCommandChassisTurret.java | 4 +++- .../frc/robot/commands/ManualHopperCommand.java | 2 +- .../frc/robot/commands/ManualShootCommand.java | 6 +++--- .../frc/robot/subsystems/DriveSubsystem.java | 8 +++++--- .../frc/robot/subsystems/HopperSubsystem.java | 10 ++++++---- .../frc/robot/subsystems/IntakeSubsystem.java | 4 ++-- .../frc/robot/subsystems/TurretSubsystem.java | 16 +++++++++++++--- 9 files changed, 45 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dcdc0d1..a043bba 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -75,28 +75,28 @@ public final class Constants { public static class IntakeConstants { public static final int DEPLOY_MOTOR_ID = 17; - public static final boolean DEPLOY_MOTOR_INVERTED = false; // TODO: Test this + public static final boolean DEPLOY_MOTOR_INVERTED = true; // TODO: Test this public static final int INTAKE_MOTOR_ID = 2; public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this } public static class ShooterConstants { - public static final int LAUNCHER_MOTOR_ID = 2; - public static final boolean LAUNCHER_MOTOR_INVERTED = true; // TODO: Test this + public static final int LAUNCHER_MOTOR_ID = 3; + public static final boolean LAUNCHER_MOTOR_INVERTED = false; - public static final int TURRET_MOTOR_ID = 3; + public static final int TURRET_MOTOR_ID = 4; public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; public static final int TURRET_LIMIT_CHANNEL = 0; public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(265); public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5); public static final double HOMING_SPEED = .35; - public static final double TURRET_GEAR_RATIO = 0; + public static final double TURRET_GEAR_RATIO = 1; public static final int HOOD_SERVO_CHANNEL = 0; public static final boolean HOOD_SERVO_INVERTED = true; // TODO: Test this - public static final int HOOD_ENCODER_ID = 0; - public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(.5); + public static final int HOOD_ENCODER_ID = 4; + public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(1); public static final Angle LIMIT_ELEVATION_POS = edu.wpi.first.units.Units.Degrees.of(90); public static final Angle LIMIT_ELEVATION_NEG = edu.wpi.first.units.Units.Degrees.of(0); public static final Angle DEFLECTOR_STORED_ANGLE = edu.wpi.first.units.Units.Degrees.of(0); @@ -111,6 +111,6 @@ public final class Constants { public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; public static final int SPINDEXER_MOTOR_ID = 8; - public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; + public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a254508..f55b897 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,18 +4,11 @@ package frc.robot; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.AutoBuilderException; - -import edu.wpi.first.units.Unit; -import edu.wpi.first.units.Units; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; -import frc.robot.commands.Autos; +import frc.robot.commands.AutoShootCommandChassisTurret; import frc.robot.commands.ManualHopperCommand; import frc.robot.commands.ManualIntake; import frc.robot.commands.ManualShootCommand; @@ -39,8 +32,6 @@ public class RobotContainer { // private final CommandPS4Controller m_driverController = new CommandPS4Controller(OperatorConstants.kDriverControllerPort); - private final SendableChooser autoChooser; - public final DriveSubsystem driveSubsystem = new DriveSubsystem(m_driverController); public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); @@ -51,8 +42,6 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - autoChooser = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); // Configure the trigger bindings configureBindings(); @@ -80,6 +69,8 @@ public class RobotContainer { m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand()); + m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, intakeSubsystem, turretSubsystem)); + intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID())); } @@ -90,6 +81,6 @@ public class RobotContainer { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return autoChooser.getSelected(); + return driveSubsystem.getAutoCommand(); } } diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index e25df57..18a873a 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -31,7 +31,7 @@ public class AutoShootCommandChassisTurret extends Command{ this.intakeSubsystem = intakeSubsystem; this.turretSubsystem = turretSubsystem; - autoCenterController = new PIDController(0, 0, 0); + autoCenterController = new PIDController(0.1, 0, 0); autoCenterController.setTolerance(1); autoCenterController.setSetpoint(0); } @@ -52,6 +52,8 @@ public class AutoShootCommandChassisTurret extends Command{ LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); + // driveSubsystem. autoCenterController.calculate(0); + if(turretSubsystem.isLauncherUpToSpeed() && autoCenterController.atSetpoint()){ hopperSubsystem.feedKicker(); } diff --git a/src/main/java/frc/robot/commands/ManualHopperCommand.java b/src/main/java/frc/robot/commands/ManualHopperCommand.java index 0058003..21fadab 100644 --- a/src/main/java/frc/robot/commands/ManualHopperCommand.java +++ b/src/main/java/frc/robot/commands/ManualHopperCommand.java @@ -23,7 +23,7 @@ public class ManualHopperCommand extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - m_subsystem.spindexerOn();; + m_subsystem.spindexerOn(); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/ManualShootCommand.java b/src/main/java/frc/robot/commands/ManualShootCommand.java index aa82050..054940f 100644 --- a/src/main/java/frc/robot/commands/ManualShootCommand.java +++ b/src/main/java/frc/robot/commands/ManualShootCommand.java @@ -27,17 +27,17 @@ public class ManualShootCommand extends Command { public void execute() { if(m_controller.getRightTriggerAxis()>.8) { - m_subsystem.setLauncher(1); + m_subsystem.setLauncher(5000); m_controller.setRumble(RumbleType.kBothRumble, 1); } else if(m_controller.getRightTriggerAxis()>.5) { - m_subsystem.setLauncher(.7); + m_subsystem.setLauncher(3000); m_controller.setRumble(RumbleType.kBothRumble, .67); } else if(m_controller.getRightTriggerAxis()>.2) { - m_subsystem.setLauncher(.5); + m_subsystem.setLauncher(2000); m_controller.setRumble(RumbleType.kBothRumble, .33); } else diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 923bbcc..ed762c7 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -169,8 +169,9 @@ public class DriveSubsystem extends TestableSubsystem { }, this); - autoChooser = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); + // autoChooser = AutoBuilder.buildAutoChooser(); + // autoChooser.addOption("Do Nothing", Commands.none()); + // SmartDashboard.putData("Auto Chooser", autoChooser); } catch (Exception e) { e.printStackTrace(); @@ -191,7 +192,8 @@ public class DriveSubsystem extends TestableSubsystem { } public Command getAutoCommand(){ - return autoChooser.getSelected(); + return Commands.none(); + // return autoChooser.getSelected(); } @PostTest() diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 703336e..e741555 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -7,8 +7,10 @@ package frc.robot.subsystems; import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.test.PostTest; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; import edu.wpi.first.wpilibj2.command.Command; import static org.fairportrobotics.frc.posty.assertions.Assertions.*; @@ -22,10 +24,10 @@ public class HopperSubsystem extends TestableSubsystem { /** Creates a new HopperSubsystem. */ public HopperSubsystem() { spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID); - SetMotorDirection(spindexerMotor, Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION); + spindexerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION)); kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID); - SetMotorDirection(kickerMotor, Constants.HopperConstants.KICKER_MOTOR_DIRECTION); + kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)); } public void feedKicker() {kickerMotor.set(0.5);} @@ -40,8 +42,8 @@ public class HopperSubsystem extends TestableSubsystem { @PostTest(enabled = true) public void hopperSystem_CANDevicesConnected(){ - assertThat(kickerMotor.isConnected()).isTrue().as("Kicker motor not connected!"); - assertThat(spindexerMotor.isConnected()).isTrue().as("Spindexer motor not connected!"); + assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue(); + assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index cbd329f..3fe9223 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -64,7 +64,7 @@ public class IntakeSubsystem extends TestableSubsystem { @PostTest(enabled = true) public void IntakeSubsystem_CANDevicesConnected(){ - assertThat(intakeMotor.isConnected()).isTrue().as("Intake motor not connected!"); - assertThat(deployMotor.isAlive()).isTrue().as("Deploy controller not connected!"); + assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue(); + assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue(); } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index f11c164..185f357 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -6,6 +6,8 @@ package frc.robot.subsystems; import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.test.PostTest; +import org.littletonrobotics.junction.Logger; + import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import com.ctre.phoenix6.controls.PositionVoltage; @@ -38,6 +40,8 @@ public class TurretSubsystem extends TestableSubsystem { MANUAL; } + private final boolean LOGGING = true; + private TurretState currentState; private DigitalInput turretLimitSwitch; private TalonFX turretMotor; @@ -76,7 +80,7 @@ public class TurretSubsystem extends TestableSubsystem { SparkMaxConfig config = new SparkMaxConfig() .apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); - config.closedLoop.p(1).i(0).d(0).outputRange(0, 5000); + config.closedLoop.p(1).i(0).d(0.1).outputRange(0, 5000); config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); launcherControler = launcherMotor.getClosedLoopController(); @@ -158,6 +162,12 @@ public class TurretSubsystem extends TestableSubsystem { default: break; } + + if(LOGGING) + { + Logger.recordOutput("Launcher Speed (RPM)", launcherMotor.getEncoder().getVelocity()); + } + } private void periodicHoming() @@ -182,8 +192,8 @@ public class TurretSubsystem extends TestableSubsystem { @PostTest() public void TurretSubsystem_CANDevicesConnected(){ - assertThat(launcherMotor.getDeviceId()).isGreaterThan(0).as("Launcher motor not connected!"); - assertThat(turretMotor.isConnected()).isTrue().as("Turret motor not connected!"); + assertThat(launcherMotor.getDeviceId()).as("Launcher motor not connected!").isGreaterThan(0); + assertThat(turretMotor.isConnected()).as("Turret motor not connected!").isTrue(); } }