From 10d8c19125688b320c70cc94ad0155c624932f91 Mon Sep 17 00:00:00 2001 From: Dave Korhumel Date: Sun, 3 Mar 2024 16:25:22 -0800 Subject: [PATCH] Fix formatting and remove unused imports --- src/main/java/frc/robot/Robot.java | 67 +++-- src/main/java/frc/robot/RobotContainer.java | 249 +++++++++--------- .../commands/ShooterSetSpeedCommand.java | 7 +- .../robot/subsystems/ClimberSubsystem.java | 16 +- .../robot/subsystems/ShooterSubsystem.java | 10 +- 5 files changed, 179 insertions(+), 170 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d715be3..bf49561 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,18 +4,15 @@ package frc.robot; - -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.subsystems.ClimberSubsystem; /** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the + * The VM is configured to automatically run this class, and to call the + * functions corresponding to each mode, as described in the TimedRobot + * documentation. If you change the name of this class or the package after + * creating this project, you must also update the build.gradle file in the * project. */ public class Robot extends TimedRobot { @@ -25,47 +22,54 @@ public class Robot extends TimedRobot { // public ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); - private Timer m_buttonTimer = new Timer(); /** - * This function is run when the robot is first started up and should be used for any - * initialization code. + * This function is run when the robot is first started up and should be used + * for any initialization code. */ @Override public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); } /** - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics - * that you want ran during disabled, autonomous, teleoperated and test. + * This function is called every 20 ms, no matter the mode. Use this for items + * like diagnostics that you want ran during disabled, autonomous, teleoperated + * and test. * - *

This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. + *

+ * This runs after the mode specific periodic functions, but before LiveWindow + * and SmartDashboard integrated updating. + *

*/ @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled commands, running already-scheduled commands, removing + // finished or interrupted commands, and running subsystem periodic() methods. + // This must be called from the robot's periodic block in order for anything in + // the Command-based framework to work. CommandScheduler.getInstance().run(); } /** This function is called once each time the robot enters Disabled mode. */ @Override - public void disabledInit() {} + public void disabledInit() { + } @Override public void disabledPeriodic() { // if (RobotController.getUserButton() && m_buttonTimer.get() > 1) { - // m_climberSubsystem.toggleCompressor(); - // m_buttonTimer.reset(); + // m_climberSubsystem.toggleCompressor(); + // m_buttonTimer.reset(); // } } - /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + /** + * This autonomous runs the autonomous command selected by your + * {@link RobotContainer} class. + */ @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); @@ -78,7 +82,8 @@ public void autonomousInit() { /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override public void teleopInit() { @@ -93,7 +98,8 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } @Override public void testInit() { @@ -103,13 +109,16 @@ public void testInit() { /** This function is called periodically during test mode. */ @Override - public void testPeriodic() {} + public void testPeriodic() { + } /** This function is called once when the robot is first started up. */ @Override - public void simulationInit() {} + public void simulationInit() { + } /** This function is called periodically whilst in simulation. */ @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5845dbe..945ee10 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,21 +5,17 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -30,12 +26,11 @@ import frc.robot.commands.NoteIntakeCommand; import frc.robot.commands.NoteOuttakeCommand; import frc.robot.commands.ShooterSetSpeedCommand; -import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.ArmPosition; -import frc.robot.subsystems.ShooterSubsystem.ShootSpeed; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.ShooterSubsystem.ShootSpeed; import frc.robot.subsystems.VisionSubsystem; /* @@ -45,125 +40,129 @@ * (including subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here - private final DriveSubsystem m_robotDrive = new DriveSubsystem(); - private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); - private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); - // private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); - private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); - - private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); - private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); - - /** - * The container for the robot. Contains subsystems, IO devices, and commands. - */ - public RobotContainer() { - - AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry, - m_robotDrive::getChassisSpeeds, - m_robotDrive::autonDrive, - new HolonomicPathFollowerConfig( - new PIDConstants(5, 0.0, 0.0), // Translation PID constants - new PIDConstants(5, 0.0, 0.0), // Rotation PID constants - DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s - Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in meters. Distance - // from robot center to furthest module. - new ReplanningConfig(true, true)), - () -> false, m_robotDrive); - - m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement); - - // Configure the trigger bindings - configureBindings(); - - m_robotDrive.setDefaultCommand( - new RunCommand( - () -> m_robotDrive.drive( - MathUtil.applyDeadband( - -m_driverController.getLeftY(), - IOConstants.kControllerDeadband) - * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar), - // * 0.8, - MathUtil.applyDeadband( - -m_driverController.getLeftX(), - IOConstants.kControllerDeadband) - * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar), - // * 0.8, - MathUtil.applyDeadband( - -m_driverController.getRightX(), - IOConstants.kControllerDeadband) - * DriveConstants.kMaxAngularSpeedRadiansPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar) - / 2, - !m_driverController.getRightBumper()), - m_robotDrive)); - } - - /** - * Use this method to define your button->command mappings. - */ - private void configureBindings() { - new JoystickButton(m_driverController, Button.kStart.value) - .onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive)); - - // new JoystickButton(m_driverController, Button.kA.value).whileTrue( - // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new - // PathConstraints( - // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, - // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); - - new JoystickButton(m_driverController, Button.kX.value) - .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting), new NoteOuttakeCommand(m_intakeSubsystem))) - .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off)); - - // new JoystickButton(m_operatorController, Button.kA.value) - // .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_climberSubsystem)); - // new JoystickButton(m_operatorController, Button.kB.value) - // .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_climberSubsystem)); - - new Trigger(() -> { - return m_driverController.getLeftTriggerAxis() > 0.5; - }).whileTrue( - new SequentialCommandGroup( - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), - new NoteIntakeCommand(m_intakeSubsystem), - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))) - .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); - - new Trigger(() -> { - return m_driverController.getRightTriggerAxis() > 0.5; - }).whileTrue( - new SequentialCommandGroup( - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp), - new NoteOuttakeCommand(m_intakeSubsystem))) - .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); - } - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - PathPlannerPath path = PathPlannerPath.fromPathFile("New New Path"); - - var alliance = DriverStation.getAlliance(); - PathPlannerPath autonPath = path; - if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { - autonPath = autonPath.flipPath(); + // The robot's subsystems and commands are defined here + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); + private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); + // private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); + private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); + + private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); + + /** + * The container for the robot. Contains subsystems, IO devices, and commands. + */ + public RobotContainer() { + + AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry, + m_robotDrive::getChassisSpeeds, + m_robotDrive::autonDrive, + new HolonomicPathFollowerConfig( + new PIDConstants(5, 0.0, 0.0), // Translation PID constants + new PIDConstants(5, 0.0, 0.0), // Rotation PID constants + DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s + Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in + // meters. Distance + // from robot center to + // furthest module. + new ReplanningConfig(true, true)), + () -> false, m_robotDrive); + + m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement); + + // Configure the trigger bindings + configureBindings(); + + m_robotDrive.setDefaultCommand( + new RunCommand( + () -> m_robotDrive.drive( + MathUtil.applyDeadband( + -m_driverController.getLeftY(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxSpeedMetersPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar), + // * 0.8, + MathUtil.applyDeadband( + -m_driverController.getLeftX(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxSpeedMetersPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar), + // * 0.8, + MathUtil.applyDeadband( + -m_driverController.getRightX(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxAngularSpeedRadiansPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar) + / 2, + !m_driverController.getRightBumper()), + m_robotDrive)); } - m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose()); - // return AutoBuilder.followPath(autonPath); - return null; - } + /** + * Use this method to define your button->command mappings. + */ + private void configureBindings() { + new JoystickButton(m_driverController, Button.kStart.value) + .onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive)); + + // new JoystickButton(m_driverController, Button.kA.value).whileTrue( + // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new + // PathConstraints( + // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, + // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); + + new JoystickButton(m_driverController, Button.kX.value) + .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting), + new NoteOuttakeCommand(m_intakeSubsystem))) + .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off)); + + // new JoystickButton(m_operatorController, Button.kA.value) + // .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), + // m_climberSubsystem)); + // new JoystickButton(m_operatorController, Button.kB.value) + // .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), + // m_climberSubsystem)); + + new Trigger(() -> { + return m_driverController.getLeftTriggerAxis() > 0.5; + }).whileTrue( + new SequentialCommandGroup( + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), + new NoteIntakeCommand(m_intakeSubsystem), + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))) + .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); + + new Trigger(() -> { + return m_driverController.getRightTriggerAxis() > 0.5; + }).whileTrue( + new SequentialCommandGroup( + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp), + new NoteOuttakeCommand(m_intakeSubsystem))) + .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + PathPlannerPath path = PathPlannerPath.fromPathFile("New New Path"); + + var alliance = DriverStation.getAlliance(); + PathPlannerPath autonPath = path; + if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { + autonPath = autonPath.flipPath(); + } + m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose()); + + // return AutoBuilder.followPath(autonPath); + return null; + } } diff --git a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java index 4eea10c..b369a8e 100644 --- a/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java +++ b/src/main/java/frc/robot/commands/ShooterSetSpeedCommand.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.ShooterConstants; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ShooterSubsystem.ShootSpeed; @@ -37,11 +36,13 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 1072b4b..d8c42b3 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -1,17 +1,17 @@ package frc.robot.subsystems; +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kOff; +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; + import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.ClimberConstants; -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kOff; -import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; - -public class ClimberSubsystem extends SubsystemBase{ +public class ClimberSubsystem extends SubsystemBase { private final DoubleSolenoid m_leftSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, ClimberConstants.leftForwardChannel, ClimberConstants.leftReverseChannel); @@ -28,7 +28,7 @@ public ClimberSubsystem() { m_compressorEnabled = false; solenoidOff(); toggleCompressor(); - } + } // Runs once every tick (~20ms) public void periodic() { @@ -59,7 +59,7 @@ public void reverse() { /** * Toggles the state of the compressor (on/off) - */ + */ public void toggleCompressor() { m_compressorEnabled = !m_compressorEnabled; if (m_compressorEnabled) { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index c7ab4f5..788d265 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -4,8 +4,8 @@ package frc.robot.subsystems; -import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -29,7 +29,7 @@ public ShooterSubsystem() { } public void setShootingSpeed(ShootSpeed speed) { - switch (speed){ + switch (speed) { case Shooting: m_topSpeed = ShooterConstants.kShooterSpeed; m_bottomSpeed = ShooterConstants.kShooterSpeed; @@ -43,7 +43,7 @@ public void setShootingSpeed(ShootSpeed speed) { } } - public double returnCurrentSpeed(){ + public double returnCurrentSpeed() { return m_bottom.getEncoder().getVelocity(); } @@ -57,8 +57,8 @@ public void periodic() { m_top.set(m_topSpeed); } - public static enum ShootSpeed{ + public static enum ShootSpeed { Shooting, Off - } + } }