Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-08 16:28:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Remove errrors from merge

Browse Source
This commit is contained in:
Michael Mikovsky
2026-02-11 16:36:51 -07:00
parent 1eb38c1e49
commit b81a6b5be7
2 changed files with 142 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+122 -69
View File
@@ -23,14 +23,11 @@ import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
// Autos
import frc4388.utility.controller.VirtualController;
import frc4388.utility.controller.XboxController;
import frc4388.utility.structs.LEDPatterns;
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.constants.Constants;
import frc4388.robot.constants.Constants.AutoConstants;
import frc4388.robot.constants.Constants.LEDConstants;
import frc4388.robot.constants.Constants.OIConstants;
import frc4388.robot.constants.Constants.SimConstants.Mode;
import frc4388.robot.constants.FieldConstants;
@@ -64,10 +61,13 @@ public class RobotContainer {
public final RobotMap m_robotMap = new RobotMap(Mode.REAL);
/* Subsystems */
public final LED m_robotLED = new LED(LEDConstants.LED_SPARK_ID);
public final LED m_robotLED = new LED();
//Testing of Colors
public final Vision m_vision = new Vision();
// public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED);
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO);
public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED);
private Boolean operatorManualMode = false;
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
// public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef");
@@ -82,11 +82,11 @@ public class RobotContainer {
// public List<Subsystem> subsystems = new ArrayList<>();
// ! Teleop Commands
// public void stop() {
// new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
// m_robotSwerveDrive.stopModules();
// Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
// }
public void stop() {
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
m_robotSwerveDrive.stopModules();
Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
}
// ! /* Autos */
private SendableChooser<String> autoChooser;
@@ -113,10 +113,13 @@ public class RobotContainer {
configureButtonBindings();
// DeferredBlock.addBlock(() -> { // Called on first robot enable
// m_robotSwerveDrive.resetGyro();
// }, false);
DeferredBlock.addBlock(() -> { // Called on every robot enable
// Called on first robot enable
DeferredBlock.addBlock(() -> {
m_robotSwerveDrive.resetGyro();
}, false);
// Called on every robot enable
DeferredBlock.addBlock(() -> {
TimesNegativeOne.update();
FieldPositions.update();
}, true);
@@ -124,27 +127,19 @@ public class RobotContainer {
DriverStation.silenceJoystickConnectionWarning(true);
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
// Drive normally
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
m_robotSwerveDrive.driveWithInput(
getDeadbandedDriverController().getLeft(),
getDeadbandedDriverController().getRight(),true);
// // IF the driver is holding the aim button, aim the robot towards the hub
// if(m_driverXbox.getRightTriggerAxis() > 0.5) {
// // Aim
// Translation2d shootTarget = new Translation2d();
// // new Rotation2()
// Rotation2d ang = m_robotSwerveDrive.getPose2d().getTranslation().minus(shootTarget).getAngle();
// m_robotSwerveDrive.driveFieldAngle(
// getDeadbandedDriverController().getLeft(),
// ang);
// } else {
// // Drive normally
// m_robotSwerveDrive.driveWithInput(
// getDeadbandedDriverController().getLeft(),
// getDeadbandedDriverController().getRight(),true);
// }
// }, m_robotSwerveDrive)
// .withName("SwerveDrive DefaultCommand"));
// m_robotSwerveDrive.setToSlow();
}, m_robotSwerveDrive)
.withName("SwerveDrive DefaultCommand"));
m_robotSwerveDrive.setToSlow();
makeAutoChooser();
SmartDashboard.putData("Auto Chooser", autoChooser);
}
@@ -163,18 +158,9 @@ public class RobotContainer {
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.C1C2_GRADIENT)));
new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.OCEAN_RAINBOW)));
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.RAINBOW_RAINBOW)));
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotLED.setTo5V()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
// .onTrue(new RotTo45(m_robotSwerveDrive));
// new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
// .onTrue(RobotShoot)
@@ -187,19 +173,86 @@ public class RobotContainer {
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive));
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
// .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter));
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter));
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
// .onTrue(new InstantCommand(() -> {;}, m_robotIntake));
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake));
// new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode()));
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> {
m_robotIntake.io.updateGains();
m_robotShooter.io.updateGains();
}));
// new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode()));
}
// IF the driver is holding the left trigger, intake driving
new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
.whileTrue(new RunCommand(
() -> {
m_robotSwerveDrive.driveIntake(
getDeadbandedDriverController().getLeft(),
false
);
}, m_robotSwerveDrive))
.onTrue(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Extended);
}))
.onFalse(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Retracted);
m_robotSwerveDrive.softStop();
}, m_robotSwerveDrive));
// IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
.whileTrue(new RunCommand(
() -> {
m_robotSwerveDrive.driveFacingPosition(
getDeadbandedDriverController().getLeft(),
FieldPositions.HUB_POSITION);
}, m_robotSwerveDrive)
)
.onTrue(new InstantCommand(() -> {
// When Right trigger is pressed,
m_robotIntake.setMode(IntakeMode.Extended);
}))
.onFalse(new InstantCommand(() -> {
m_robotIntake.setMode(IntakeMode.Retracted);
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)));
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
@@ -249,17 +302,17 @@ public class RobotContainer {
}
autoChooser.onChange((filename) -> {
// autoChooserUpdated = true;
// if (filename.equals("Taxi")) {
// autoCommand = new SequentialCommandGroup(
// new MoveForTimeCommand(m_robotSwerveDrive,
// new Translation2d(0, -1),
// new Translation2d(), 1000, true
// ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
// } else {
// autoCommand = new PathPlannerAuto(filename);
// }
// System.out.println("Robot Auto Changed " + filename);
autoChooserUpdated = true;
if (filename.equals("Taxi")) {
autoCommand = new SequentialCommandGroup(
new MoveForTimeCommand(m_robotSwerveDrive,
new Translation2d(0, -1),
new Translation2d(), 1000, true
), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
} else {
autoCommand = new PathPlannerAuto(filename);
}
System.out.println("Robot Auto Changed " + filename);
});
SmartDashboard.putData(autoChooser);
@@ -286,4 +339,4 @@ public class RobotContainer {
// public ButtonBox getButtonBox() {
// return this.m_buttonBox;
// }
}
}
src/main/java/frc4388/robot/RobotMap.java
+20 -20
View File
@@ -51,7 +51,7 @@ public class RobotMap {
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
/* Swreve Drive Subsystem */
// public final SwerveIO swerveDrivetrain;
public final SwerveIO swerveDrivetrain;
// /* Shooter and Intake Subsystem */
public final ShooterIO shooterIO;
@@ -75,13 +75,13 @@ public class RobotMap {
// reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
// Configure swerve drive train
// SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new,
// SwerveDriveConstants.DrivetrainConstants,
// SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT,
// SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT
// );
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new,
SwerveDriveConstants.DrivetrainConstants,
SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT,
SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT
);
// swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
// Configure Shooter 22,23,24
TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.CANIVORE_CANBUS);
@@ -103,7 +103,7 @@ public class RobotMap {
intakeIO = new IntakeReal(arm, roller);
// Fault
// FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
FaultTalonFX.addDevice(shooter1, "Shooter1");
@@ -112,18 +112,18 @@ public class RobotMap {
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");
// FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder");
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive");
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer");
// FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder");
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive");
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer");
// FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder");
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive");
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer");
// FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer");
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer");
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive");
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer");
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
break;
// case SIM: