diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a4df644..9c80061 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,6 +11,7 @@ import java.io.File; import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; @@ -19,30 +20,23 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // Commands import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.commands.MoveForTimeCommand; -import frc4388.robot.commands.MoveUntilSuply; import frc4388.robot.constants.Constants; -import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; import frc4388.robot.constants.FieldConstants; // Subsystems import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; import frc4388.utility.compute.TimesNegativeOne; -import frc4388.utility.configurable.ConfigurableString; import frc4388.utility.controller.DeadbandedXboxController; // Autos import frc4388.utility.controller.VirtualController; @@ -65,7 +59,7 @@ public class RobotContainer { public final LED m_robotLED = new LED(); //Testing of Colors - public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera); + public final Vision m_vision = new Vision(); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); private Boolean operatorManualMode = false; // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -116,19 +110,10 @@ public class RobotContainer { DriverStation.silenceJoystickConnectionWarning(true); m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - - // IF the driver is holding the aim button, aim the robot towards the hub - if(m_driverXbox.getRightTriggerAxis() > 0.5) { - // Aim - m_robotSwerveDrive.driveFacingPosition( - getDeadbandedDriverController().getLeft(), - FieldConstants.BLUE_HUB_POS); - } else { - // Drive normally - m_robotSwerveDrive.driveWithInput( - getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(),true); - } + // Drive normally + m_robotSwerveDrive.driveWithInput( + getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(),true); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); @@ -170,12 +155,46 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + // IF the driver is holding the aim button, aim the robot towards the hub + new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) + .whileTrue(new RunCommand( + () -> { + m_robotSwerveDrive.driveFacingPosition( + getDeadbandedDriverController().getLeft(), + FieldConstants.BLUE_HUB_POS); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + + + // IF the driver is holding the aim button, aim the robot towards the hub + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .whileTrue(new RunCommand( + () -> { + m_robotSwerveDrive.driveIntake( + getDeadbandedDriverController().getLeft() + ); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + + // D-PAD fine alignment + new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) + .whileTrue(new RunCommand( + () -> m_robotSwerveDrive.driveFine( + new Translation2d( + 1, + Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) + ), + getDeadbandedDriverController().getRight(), 0.15 + ), m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); } //.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT))); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index f0a4400..10bc3bc 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -46,8 +46,8 @@ public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public final VisionIO leftCamera; - public final VisionIO rightCamera; + // public final VisionIO leftCamera; + // public final VisionIO rightCamera; // public final LiDAR lidar = new @@ -62,21 +62,21 @@ public class RobotMap { public final SwerveIO swerveDrivetrain; // /* Elevator Subsystem */ - public final ShooterIO shooterIO; - public final IntakeIO intakeIO; + // public final ShooterIO shooterIO; + // public final IntakeIO intakeIO; public RobotMap(SimConstants.Mode mode) { switch (mode) { case REAL: // Configure cameras - PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); - PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); + // PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); + // PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); - leftCamera = new VisionReal(leftCameraReal, VisionConstants.RIGHT_CAMERA_POS); ; - rightCamera = new VisionReal(rightCameraReal, VisionConstants.LEFT_CAMERA_POS); + // leftCamera = new VisionReal(leftCameraReal, VisionConstants.RIGHT_CAMERA_POS); ; + // rightCamera = new VisionReal(rightCameraReal, VisionConstants.LEFT_CAMERA_POS); - FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); - FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); + // FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); + // FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); // // Configure LiDAR // reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); @@ -98,16 +98,16 @@ public class RobotMap { TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id); //Configure Intake - TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); - TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id); + // TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); + // TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - shooterIO = new ShooterReal(shooter1, shooter2, indexer); + // shooterIO = new ShooterReal(shooter1, shooter2, indexer); - intakeIO = new IntakeReal(arm, roller); + // intakeIO = new IntakeReal(arm, roller); // Fault FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); @@ -116,8 +116,8 @@ public class RobotMap { FaultTalonFX.addDevice(shooter1, "Shooter1"); FaultTalonFX.addDevice(shooter2, "Shooter2"); FaultTalonFX.addDevice(indexer, "Indexer"); - FaultTalonFX.addDevice(arm, "Arm"); - FaultTalonFX.addDevice(roller, "Roller"); + // FaultTalonFX.addDevice(arm, "Arm"); + // FaultTalonFX.addDevice(roller, "Roller"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); @@ -136,11 +136,11 @@ public class RobotMap { // case SIM: // break; default: - leftCamera = new VisionIO() {}; - rightCamera = new VisionIO() {}; + // leftCamera = new VisionIO() {}; + // rightCamera = new VisionIO() {}; swerveDrivetrain = new SwerveIO() {}; - intakeIO = new IntakeIO() {}; - shooterIO = new ShooterIO() {}; + // intakeIO = new IntakeIO() {}; + // shooterIO = new ShooterIO() {}; break; } } diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 2150560..fc4bd88 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 28; - public static final String GIT_SHA = "5f4d7887ffc93ef535cef23511da086cd7f6d8bd"; - public static final String GIT_DATE = "2026-01-31 15:33:14 MST"; + public static final int GIT_REVISION = 29; + public static final String GIT_SHA = "dc33af165d04720f576a5d490987d6ee079c4b8d"; + public static final String GIT_DATE = "2026-01-31 21:38:10 MST"; public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-01-31 21:37:04 MST"; - public static final long BUILD_UNIX_TIME = 1769920624950L; + public static final String BUILD_DATE = "2026-02-02 19:39:42 MST"; + public static final long BUILD_UNIX_TIME = 1770086382841L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index f3de1be..046ea38 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -287,6 +287,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } + public void driveIntake(Translation2d leftStick){ // if (invert){ // Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY()); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index 9318444..cee7d43 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -31,6 +31,7 @@ import frc4388.utility.structs.Gains; // No mans land // Beware, there be dragons. public final class SwerveDriveConstants { + public static final String CANBUS_NAME = "drivetrain"; public static final double MAX_ROT_SPEED = Math.PI * 2; public static final double AUTO_MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 1.0; @@ -79,7 +80,7 @@ public final class SwerveDriveConstants { private static final class ModuleSpecificConstants { //2025 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.35); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; @@ -87,7 +88,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; @@ -95,7 +96,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; @@ -103,7 +104,7 @@ public final class SwerveDriveConstants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; @@ -208,7 +209,7 @@ public final class SwerveDriveConstants { } public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withPigeon2Id(IDs.DRIVE_PIGEON.id); + .withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(CANBUS_NAME); private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() // holy verbosity batman.