diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 75fcda7..7590bc8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,83 +15,70 @@ import frc.robot.subsystems.led.LEDSubsystem; public class RobotContainer { - private double MaxSpeed = 6; // 6 meters per second desired top speed - private double MaxAngularRate = Math.PI; // Half a rotation per second max angular velocity + private class Controller { + public static final CommandController driver = new CommandController(0); + public static final CommandController operator = new CommandController(1); + } - /* Setting up bindings for necessary control of the swerve drive platform */ - private final CommandController driver = new CommandController(0); // Driver Controller - private final CommandController operator = new CommandController(1); // Operator Controller - final DriveSubsystem drivetrain = DriveConstants.DriveTrain; // My drivetrain - private final LEDSubsystem ledSubsystem; - private final SwerveRequest.FieldCentric drive = - new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1) - .withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // I want field-centric - // driving in open loop + private final LEDSubsystem ledSubsystem; - private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - private final SwerveRequest.RobotCentric forwardStraight = - new SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + // Set up the base for the drive and drivetrain + final DriveSubsystem drivetrain = DriveConstants.DriveTrain; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(DriveConstants.kMaxAngularRate * 0.1).withRotationalDeadband(DriveConstants.kMaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - /* Path follower */ - private Command runAuto = drivetrain.getAutoPath("Tests"); + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric() + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final Telemetry logger = new Telemetry(MaxSpeed); + /* Path follower */ + private Command runAuto = drivetrain.getAutoPath("Tests"); - private void configureBindings() { - ledSubsystem.setDefaultCommand(new InstantCommand(() -> ledSubsystem.periodic(), ledSubsystem)); + private final Telemetry logger = new Telemetry(DriveConstants.kMaxSpeed); - drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically - drivetrain - .applyRequest( - () -> - drive - .withVelocityX(-driver.getLeftY() * MaxSpeed) // Drive forward with - // negative Y (forward) - .withVelocityY( - -driver.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate( - -driver.getRightX() - * MaxAngularRate) // Drive counterclockwise with negative X - // (left) - ) - .ignoringDisable(true)); + private void configureBindings() { + ledSubsystem.setDefaultCommand(new InstantCommand(() -> ledSubsystem.periodic(), ledSubsystem)); - driver.greenButton().whileTrue(drivetrain.applyRequest(() -> brake)); - driver - .yellowButton() - .whileTrue( - drivetrain.applyRequest( - () -> - point.withModuleDirection( - new Rotation2d(-driver.getLeftY(), -driver.getLeftX())))); + drivetrain.setDefaultCommand( + drivetrain + .applyRequest( + () -> drive + .withVelocityX(-Controller.driver.getLeftY() * DriveConstants.kMaxSpeed) // Drive forward with + // negative Y (forward) + .withVelocityY( + -Controller.driver.getLeftX() * DriveConstants.kMaxSpeed) // Drive left with negative X (left) + .withRotationalRate( + -Controller.driver.getRightX() + * DriveConstants.kMaxAngularRate) // Drive counterclockwise with negative X + // (left) + ) + .ignoringDisable(true)); - // if (Utils.isSimulation()) { - // drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), - // Rotation2d.fromDegrees(90))); - // } - drivetrain.registerTelemetry(logger::telemeterize); - driver.POVUp() - .whileTrue( - drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0))); - driver.POVDown() - .whileTrue( - drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0))); - } + Controller.driver.greenButton().whileTrue(drivetrain.applyRequest(() -> brake)); + Controller.driver + .yellowButton() + .whileTrue( + drivetrain.applyRequest( + () -> point.withModuleDirection( + new Rotation2d(-Controller.driver.getLeftY(), -Controller.driver.getLeftX())))); - public RobotContainer() { - configureBindings(); - ledSubsystem = new LEDSubsystem(); - } + drivetrain.registerTelemetry(logger::telemeterize); + Controller.driver.POVUp() + .whileTrue( + drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0))); + Controller.driver.POVDown() + .whileTrue( + drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0))); + } - public Command getAutonomousCommand() { - /* First put the drivetrain into auto run mode, then run the auto */ - return runAuto; - } + public RobotContainer() { + configureBindings(); + ledSubsystem = new LEDSubsystem(); + } - public boolean seedPoseButtonDown() { - return driver.leftBumper().getAsBoolean(); - } + public Command getAutonomousCommand() { + return runAuto; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 45d8e5c..3d03bc8 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -9,9 +9,11 @@ public class DriveConstants { /** Only touch these if you know what you're doing */ - private static final SlotGains steerGains = new SlotGains(100, 0, 0.05, 0, 0); + private static final SlotGains steerGains = new SlotGains(100.0, 0, 0.05, 0, 0); + private static final SlotGains driveGains = new SlotGains(3.0, 0, 0, 0, 0); - private static final SlotGains driveGains = new SlotGains(3, 0, 0, 0, 0); + public static final double kMaxSpeed = 6.0; // 6 meters per second desired top speed + public static final double kMaxAngularRate = Math.PI; // Half a rotation per second max angular velocity private static final double kCoupleRatio = 3.5; @@ -23,6 +25,7 @@ public class DriveConstants { private static final int kPigeonId = 1; public static final double kSpeedAt12VoltsMps = 5.0; + public static final double kSimLoopPeriod = 0.005; // 5 ms private static final boolean kSteerMotorReversed = true; public static final boolean kInvertLeftSide = false; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 81f1aa5..2d9fe7a 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -23,7 +23,6 @@ import edu.wpi.first.wpilibj2.command.Subsystem; public class DriveSubsystem extends SwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms private Notifier m_simNotifier = null; private double m_lastSimTime; @@ -104,7 +103,7 @@ private void startSimThread() { /* use the measured time delta, get battery voltage from WPILib */ updateSimState(deltaTime, RobotController.getBatteryVoltage()); }); - m_simNotifier.startPeriodic(kSimLoopPeriod); + m_simNotifier.startPeriodic(DriveConstants.kSimLoopPeriod); } public void initialize() {