From 8144a55ce59a386fe7ca8359e90bfa1adf53d075 Mon Sep 17 00:00:00 2001 From: gavinskycastle <62412298+gavinskycastle@users.noreply.github.com> Date: Sat, 18 Jan 2025 05:43:08 +0000 Subject: [PATCH] Automatically applied spotless --- src/main/java/frc/robot/RobotContainer.java | 63 ++++++++++--------- .../java/frc/robot/commands/DriveForward.java | 44 ++++++------- .../subsystems/CommandSwerveDrivetrain.java | 15 ++--- 3 files changed, 63 insertions(+), 59 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 25888dc..4d49545 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,15 +8,10 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathPlannerPath; - -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Joystick; 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.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -29,8 +24,8 @@ import frc.robot.commands.SwerveCharacterization; import frc.robot.constants.ROBOT; import frc.robot.constants.SWERVE; -import frc.robot.constants.USB; import frc.robot.constants.SWERVE.ROUTINE_TYPE; +import frc.robot.constants.USB; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.AlgaeIntake; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -102,9 +97,9 @@ private void initializeSubSystems() { private void initAutoChooser() { SmartDashboard.putData("Auto Chooser", m_chooser); - + m_chooser.addOption("Do Nothing", new WaitCommand(0)); - + m_chooser.addOption("DriveForward", new DriveForward(m_swerveDrive)); } @@ -113,48 +108,56 @@ private void initSmartDashboard() { else initAutoChooser(); SmartDashboard.putData("ResetGyro", new ResetGyro(m_swerveDrive)); } - - private void initSysidChooser(){ + + private void initSysidChooser() { SignalLogger.setPath("/media/sda1/"); SysIdUtils.createSwerveDriveRoutines(m_swerveDrive); SysIdUtils.createSwerveTurnRoutines(m_swerveDrive); SmartDashboard.putData( - "Start Logging", new InstantCommand(SignalLogger::start).ignoringDisable(true)); + "Start Logging", new InstantCommand(SignalLogger::start).ignoringDisable(true)); SmartDashboard.putData( - "Stop Logging", new InstantCommand(SignalLogger::stop).ignoringDisable(true)); + "Stop Logging", new InstantCommand(SignalLogger::stop).ignoringDisable(true)); SmartDashboard.putData( - "initDriveSettings", + "initDriveSettings", new InstantCommand(m_swerveDrive::initDriveSysid).ignoringDisable(true)); m_sysidChooser.addOption( - "driveQuasistaticForward", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.DRIVE_QUASISTATIC)); + "driveQuasistaticForward", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.DRIVE_QUASISTATIC)); m_sysidChooser.addOption( - "driveQuasistaticBackwards", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.DRIVE_QUASISTATIC)); + "driveQuasistaticBackwards", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.DRIVE_QUASISTATIC)); m_sysidChooser.addOption( - "driveDynamicForward", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.DRIVE_DYNAMIC)); + "driveDynamicForward", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.DRIVE_DYNAMIC)); m_sysidChooser.addOption( - "driveDynamicBackward", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.DRIVE_DYNAMIC)); + "driveDynamicBackward", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.DRIVE_DYNAMIC)); m_sysidChooser.addOption( - "turnQuasistaticForward", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.TURN_QUASISTATIC)); + "turnQuasistaticForward", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.TURN_QUASISTATIC)); m_sysidChooser.addOption( - "turnQuasistaticBackwards", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.TURN_QUASISTATIC)); + "turnQuasistaticBackwards", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.TURN_QUASISTATIC)); m_sysidChooser.addOption( - "turnDynamicForward", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.TURN_DYNAMIC)); + "turnDynamicForward", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kForward, ROUTINE_TYPE.TURN_DYNAMIC)); m_sysidChooser.addOption( - "turnDynamicBackward", - new SwerveCharacterization(m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.TURN_DYNAMIC)); + "turnDynamicBackward", + new SwerveCharacterization( + m_swerveDrive, SysIdRoutine.Direction.kReverse, ROUTINE_TYPE.TURN_DYNAMIC)); + } - } /** * Use this method to define your trigger->command mappings. Triggers can be created via the * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary diff --git a/src/main/java/frc/robot/commands/DriveForward.java b/src/main/java/frc/robot/commands/DriveForward.java index 9ee7930..103ca7d 100644 --- a/src/main/java/frc/robot/commands/DriveForward.java +++ b/src/main/java/frc/robot/commands/DriveForward.java @@ -21,29 +21,29 @@ public class DriveForward extends SequentialCommandGroup { /** Creates a new DriveForward. */ public DriveForward(CommandSwerveDrivetrain swerveDrive) { try { - PathPlannerPath path = PathPlannerPath.fromPathFile("DriveForward"); + PathPlannerPath path = PathPlannerPath.fromPathFile("DriveForward"); - var m_ppCommand = AutoBuilder.followPath(path); - - var point = new SwerveRequest.PointWheelsAt(); - var stopRequest = new SwerveRequest.ApplyRobotSpeeds(); - - // Will throw an exception if the starting pose is not present - var starting_pose = path.getStartingHolonomicPose().orElseThrow(); - - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - addCommands( - new InstantCommand( // Reset the pose of the robot to the starting pose of the path - () -> swerveDrive.resetPose(starting_pose)), - new InstantCommand( - () -> swerveDrive.applyRequest(() -> point.withModuleDirection(new Rotation2d())), - swerveDrive) - .alongWith(new WaitCommand(1)), - m_ppCommand.andThen(() -> swerveDrive.setControl(stopRequest))); + var m_ppCommand = AutoBuilder.followPath(path); + + var point = new SwerveRequest.PointWheelsAt(); + var stopRequest = new SwerveRequest.ApplyRobotSpeeds(); + + // Will throw an exception if the starting pose is not present + var starting_pose = path.getStartingHolonomicPose().orElseThrow(); + + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new InstantCommand( // Reset the pose of the robot to the starting pose of the path + () -> swerveDrive.resetPose(starting_pose)), + new InstantCommand( + () -> swerveDrive.applyRequest(() -> point.withModuleDirection(new Rotation2d())), + swerveDrive) + .alongWith(new WaitCommand(1)), + m_ppCommand.andThen(() -> swerveDrive.setControl(stopRequest))); } catch (Exception e) { - DriverStation.reportError("Failed to load path for DriveForward", e.getStackTrace()); - addCommands(new WaitCommand(0)); + DriverStation.reportError("Failed to load path for DriveForward", e.getStackTrace()); + addCommands(new WaitCommand(0)); } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index b0bebfc..9fccf41 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -28,11 +28,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.generated.TunerConstants; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; import frc.robot.utils.CtreUtils; import frc.robot.utils.ModuleMap; - import java.util.function.Supplier; /** @@ -143,14 +141,17 @@ public void initDriveSysid() { var driveMotor = getModule(i.ordinal()).getDriveMotor(); var turnMotor = getModule(i.ordinal()).getSteerMotor(); CtreUtils.configureTalonFx(driveMotor, new TalonFXConfiguration()); - CtreUtils.configureTalonFx(turnMotor, new TalonFXConfiguration() /* was previously CtreUtils.generateTurnMotorConfig() TODO: get that method working again*/); + CtreUtils.configureTalonFx( + turnMotor, + new TalonFXConfiguration() /* was previously CtreUtils.generateTurnMotorConfig() TODO: get that method working again*/); driveMotor.setNeutralMode(NeutralModeValue.Brake); BaseStatusSignal.setUpdateFrequencyForAll( 250, driveMotor.getPosition(), driveMotor.getVelocity(), driveMotor.getMotorVoltage()); driveMotor.optimizeBusUtilization(); } - } + } + /** * Constructs a CTRE SwerveDrivetrain using the specified constants. * @@ -207,9 +208,9 @@ public CommandSwerveDrivetrain( } // TODO: Re-implement -// public Command applyChassisSpeeds(Supplier chassisSpeeds) { -// return applyChassisSpeeds(chassisSpeeds, 0.02, 1.0, false); -// } + // public Command applyChassisSpeeds(Supplier chassisSpeeds) { + // return applyChassisSpeeds(chassisSpeeds, 0.02, 1.0, false); + // } /** * Second-Order Kinematics