From 638ad804b32f552f538a9c6b7df34aa935967c44 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 24 Jan 2026 11:36:16 -0800 Subject: [PATCH 01/12] Added landmarks --- .../subsystems/pose/Landmarks.java | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 src/main/java/competition/subsystems/pose/Landmarks.java diff --git a/src/main/java/competition/subsystems/pose/Landmarks.java b/src/main/java/competition/subsystems/pose/Landmarks.java new file mode 100644 index 00000000..880bdd08 --- /dev/null +++ b/src/main/java/competition/subsystems/pose/Landmarks.java @@ -0,0 +1,40 @@ +package competition.subsystems.pose; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import xbot.common.subsystems.pose.BasePoseSubsystem; + +import javax.inject.Singleton; + +@Singleton +public class Landmarks { + // Starting on blue alliance towards outpost + public static Pose2d blueStartTrenchToOutpost = new Pose2d(3.57,.650, Rotation2d.fromDegrees(0)); + public static Pose2d blueStartBumpToOutpost = new Pose2d(3.57,2.3, Rotation2d.fromDegrees(0)); + + // Starting on blue alliance towards depot + public static Pose2d blueStartTrenchToDepot = new Pose2d(3.57,7.390, Rotation2d.fromDegrees(0)); + public static Pose2d blueStartBumpToDepot = new Pose2d(3.57,5.980, Rotation2d.fromDegrees(0)); + + // Starting on blue alliance towards neutral area + public static Pose2d blueStartBumpToNeutralAreaOutpostSide = new Pose2d(4.480,.65, Rotation2d.fromDegrees(180)); + public static Pose2d blueStartTrenchToNeutralAreaOutpostSide = new Pose2d(4.480,2.6, Rotation2d.fromDegrees(180)); + + public static Pose2d blueStartBumpToNeutralAreaDepotSide = new Pose2d(3.580,5.94, Rotation2d.fromDegrees(180)); + public static Pose2d blueStartTrenchToNeutralAreaDepotSide = new Pose2d(4.480,7.390, Rotation2d.fromDegrees(180)); + + // Blue Depot + public static Pose2d blueDepotCollectCenter = new Pose2d(1.180, 5.940, Rotation2d.fromDegrees(180)); + public static Pose2d blueDepotWallSide = new Pose2d(0.460, 6.980, Rotation2d.fromDegrees(270)); + public static Pose2d blueDepotTowerSide = new Pose2d(0.460, 4.950, Rotation2d.fromDegrees(90)); + + // Blue Outpost + public static Pose2d blueOutpost = new Pose2d(0.470, 0.650, Rotation2d.fromDegrees(180)); + + // Blue Tower + public static Pose2d blueClimbOutpostEdge = new Pose2d(1.510,2.780,Rotation2d.fromDegrees(180)); + public static Pose2d BlueClimbMiddleOutpostSide = new Pose2d(1.510,3.120,Rotation2d.fromDegrees(180)); + public static Pose2d BlueClimbCenter = new Pose2d(1.510,3.750,Rotation2d.fromDegrees(180)); + public static Pose2d BlueClimbMiddleDepotSide = new Pose2d(1.510,4.380,Rotation2d.fromDegrees(180)); + public static Pose2d BlueClimbDepotSideEdge = new Pose2d(1.510,4.650,Rotation2d.fromDegrees(180)); +} \ No newline at end of file From 82920f9d0ac387e9adce74b7951078ffceb79025 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Tue, 27 Jan 2026 20:10:33 -0800 Subject: [PATCH 02/12] Made depot collection auto command --- .../OperatorCommandMap.java | 13 ++++ .../simulation/MapleSimulator.java | 6 +- .../commands/ResetSimulatedPoseCommand.java | 3 +- .../commands/DepotCollectionAutoCommand.java | 62 +++++++++++++++++++ .../subsystems/pose/Landmarks.java | 4 +- 5 files changed, 83 insertions(+), 5 deletions(-) create mode 100644 src/main/java/competition/subsystems/drive/commands/DepotCollectionAutoCommand.java diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index f9385f0f..1308ca0d 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -3,7 +3,9 @@ import javax.inject.Inject; import javax.inject.Singleton; +import competition.simulation.commands.ResetSimulatedPoseCommand; import competition.subsystems.drive.commands.DebugSwerveModuleCommand; +import competition.subsystems.drive.commands.DepotCollectionAutoCommand; import competition.subsystems.drive.commands.SwerveDriveWithJoysticksCommand; import competition.subsystems.shooter.commands.ShooterOutputCommand; import competition.subsystems.shooter.commands.TrimShooterVelocityDown; @@ -46,5 +48,16 @@ public void setupDriveCommands(OperatorInterface operatorInterface, operatorInterface.driverGamepad.getPovIfAvailable(0).onTrue(debugModule); operatorInterface.driverGamepad.getPovIfAvailable(90).onTrue(changeActiveModule); operatorInterface.driverGamepad.getPovIfAvailable(180).onTrue(typicalSwerveDrive); + + + } + @Inject + public void setupSimulatorCommands( + ResetSimulatedPoseCommand resetSimulatorPositionCommand, + DepotCollectionAutoCommand depotCollectionAutoCommand + ) { + resetSimulatorPositionCommand.includeOnSmartDashboard("Reset Simulator Position"); + depotCollectionAutoCommand.includeOnSmartDashboard("Depot collection"); + } } diff --git a/src/main/java/competition/simulation/MapleSimulator.java b/src/main/java/competition/simulation/MapleSimulator.java index 20ca3736..ebe80fa3 100644 --- a/src/main/java/competition/simulation/MapleSimulator.java +++ b/src/main/java/competition/simulation/MapleSimulator.java @@ -16,6 +16,7 @@ import xbot.common.controls.sensors.mock_adapters.MockGyro; import xbot.common.logic.TimeStableValidator; +import static competition.subsystems.pose.Landmarks.blueStartTrenchToDepot; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Seconds; @@ -88,8 +89,9 @@ public MapleSimulator(PoseSubsystem pose, DriveSubsystem drive, ShooterSimulator drive.getRearRightSwerveModuleSubsystem().getModuleTranslation() }); - // starting middle ish of the field on blue - var startingPose = new Pose2d(7, 7 , new Rotation2d()); + // starting + // middle ish of the field on blue + var startingPose = blueStartTrenchToDepot; // Creating the SelfControlledSwerveDriveSimulation instance this.swerveDriveSimulation = new SelfControlledSwerveDriveSimulation( diff --git a/src/main/java/competition/simulation/commands/ResetSimulatedPoseCommand.java b/src/main/java/competition/simulation/commands/ResetSimulatedPoseCommand.java index 486b1f82..37153c6b 100644 --- a/src/main/java/competition/simulation/commands/ResetSimulatedPoseCommand.java +++ b/src/main/java/competition/simulation/commands/ResetSimulatedPoseCommand.java @@ -3,6 +3,7 @@ import javax.inject.Inject; import competition.simulation.BaseSimulator; +import competition.subsystems.pose.Landmarks; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import xbot.common.command.BaseCommand; @@ -17,7 +18,7 @@ public ResetSimulatedPoseCommand(BaseSimulator simulator) { @Override public void initialize() { - this.simulator.resetPosition(new Pose2d(6, 4, new Rotation2d())); + this.simulator.resetPosition(Landmarks.blueStartTrenchToDepot); } @Override diff --git a/src/main/java/competition/subsystems/drive/commands/DepotCollectionAutoCommand.java b/src/main/java/competition/subsystems/drive/commands/DepotCollectionAutoCommand.java new file mode 100644 index 00000000..e7bbe0ff --- /dev/null +++ b/src/main/java/competition/subsystems/drive/commands/DepotCollectionAutoCommand.java @@ -0,0 +1,62 @@ +package competition.subsystems.drive.commands; + +import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.pose.Landmarks; +import competition.subsystems.pose.PoseSubsystem; +import edu.wpi.first.math.geometry.Pose2d; +import xbot.common.logging.RobotAssertionManager; +import xbot.common.properties.PropertyFactory; +import xbot.common.subsystems.drive.BaseSwerveDriveSubsystem; +import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; +import xbot.common.subsystems.drive.control_logic.HeadingModule; +import xbot.common.trajectory.XbotSwervePoint; + +import javax.inject.Inject; +import java.util.ArrayList; + +public class DepotCollectionAutoCommand extends SwerveSimpleTrajectoryCommand { + final DriveSubsystem drive; + + public Pose2d outpostPose; + @Inject + public DepotCollectionAutoCommand(DriveSubsystem drive, BaseSwerveDriveSubsystem baseSwerveDriveSubsystem, + PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory, + PoseSubsystem pose, RobotAssertionManager robotAssertionManager) { + super(drive,pose,pf,headingModuleFactory,robotAssertionManager); + pf.setPrefix("DepotCollection"); + this.drive = drive; + this.addRequirements(drive); + } + + @Override + public void initialize() { + Pose2d depotWallSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotWallSide); + Pose2d depotTowerSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotTowerSide); + + ArrayList swervePoints = new ArrayList<>(); + swervePoints.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( + depotWallSidePose, 3)); + swervePoints.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( + depotTowerSidePose, 3)); + this.logic.setKeyPoints(swervePoints); + this.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); + super.initialize(); + log.info("DepotCollectionAutoCommand initialized"); + + } + + @Override + public void execute() { + super.execute(); + } + + @Override + public boolean isFinished() { + return super.isFinished(); + } + + @Override + public void end(boolean interrupted) { + log.info("end"); + } +} \ No newline at end of file diff --git a/src/main/java/competition/subsystems/pose/Landmarks.java b/src/main/java/competition/subsystems/pose/Landmarks.java index 5b720d9d..bc72f3b8 100644 --- a/src/main/java/competition/subsystems/pose/Landmarks.java +++ b/src/main/java/competition/subsystems/pose/Landmarks.java @@ -25,8 +25,8 @@ public class Landmarks { // Blue Depot public static Pose2d blueDepotCollectCenter = new Pose2d(1.180, 5.940, Rotation2d.fromDegrees(180)); - public static Pose2d blueDepotWallSide = new Pose2d(0.460, 6.980, Rotation2d.fromDegrees(270)); - public static Pose2d blueDepotTowerSide = new Pose2d(0.460, 4.950, Rotation2d.fromDegrees(90)); + public static Pose2d blueDepotWallSide = new Pose2d(0.44, 6.980, Rotation2d.fromDegrees(270)); + public static Pose2d blueDepotTowerSide = new Pose2d(0.44, 4.950, Rotation2d.fromDegrees(270)); // Blue Outpost public static Pose2d blueOutpost = new Pose2d(0.470, 0.650, Rotation2d.fromDegrees(180)); From ab8e56fbfade3103159bd48d3532742d3d71f3ec Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Wed, 18 Feb 2026 00:04:59 -0800 Subject: [PATCH 03/12] split the drive commands and made a collection command group --- .../DepotCollectionAutoCommandGroup.java | 21 ++++++++ .../OperatorCommandMap.java | 5 +- .../subsystems/climber/ClimberSubsystem.java | 2 +- .../commands/DepotCollectionAutoCommand.java | 20 ++------ .../commands/DriveToDepotAutoCommand.java | 49 +++++++++++++++++++ 5 files changed, 77 insertions(+), 20 deletions(-) create mode 100644 src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java create mode 100644 src/main/java/competition/subsystems/drive/commands/DriveToDepotAutoCommand.java diff --git a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java new file mode 100644 index 00000000..514b7064 --- /dev/null +++ b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java @@ -0,0 +1,21 @@ +package competition.command_groups; + +import competition.subsystems.drive.commands.DepotCollectionAutoCommand; +import competition.subsystems.drive.commands.DriveToDepotAutoCommand; +import competition.subsystems.pose.Landmarks; +import competition.subsystems.pose.PoseSubsystem; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class DepotCollectionAutoCommandGroup extends SequentialCommandGroup { + public DepotCollectionAutoCommandGroup(DriveToDepotAutoCommand driveToDepotAutoCommand, + DepotCollectionAutoCommand depotCollectionAutoCommand, + HopperAndIntakeCommandGroup hopperAndIntakeCommandGroup, + PoseSubsystem pose) { + addCommands( + pose.createSetPositionCommand(PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueStartTrenchToDepot)), + driveToDepotAutoCommand, + hopperAndIntakeCommandGroup, + depotCollectionAutoCommand + ); + } +} diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 50ce7e70..8b2da8da 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -3,11 +3,12 @@ import javax.inject.Inject; import javax.inject.Singleton; +import competition.simulation.commands.ResetSimulatedPoseCommand; import competition.subsystems.drive.commands.DriveToOutpostCommand; import competition.subsystems.climber.commands.ClimberExtendCommand; import competition.subsystems.climber.commands.ClimberRetractCommand; import competition.subsystems.drive.commands.DebugSwerveModuleCommand; -import competition.subsystems.drive.commands.DepotCollectionAutoCommand; +import competition.subsystems.drive.commands.DriveToDepotAutoCommand; import competition.subsystems.drive.commands.SwerveDriveWithJoysticksCommand; import competition.subsystems.fuel_intake.commands.FuelEjectCommand; import competition.subsystems.fuel_intake.commands.FuelIntakeCommand; @@ -64,7 +65,7 @@ public void setupDriveCommands(OperatorInterface operatorInterface, @Inject public void setupSimulatorCommands( ResetSimulatedPoseCommand resetSimulatorPositionCommand, - DepotCollectionAutoCommand depotCollectionAutoCommand + DriveToDepotAutoCommand depotCollectionAutoCommand ) { resetSimulatorPositionCommand.includeOnSmartDashboard("Reset Simulator Position"); depotCollectionAutoCommand.includeOnSmartDashboard("Depot collection"); diff --git a/src/main/java/competition/subsystems/climber/ClimberSubsystem.java b/src/main/java/competition/subsystems/climber/ClimberSubsystem.java index 19c9a85a..9b99821c 100644 --- a/src/main/java/competition/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/competition/subsystems/climber/ClimberSubsystem.java @@ -161,7 +161,7 @@ private Angle getAbsoluteAngle() { } private void forceCalibration() { - if (climberEncoder != null) { + if (climberEncoder != null && climberEncoder.getAbsolutePosition() != null) { encoderZeroOffset = climberEncoder.getAbsolutePosition().in(Rotations); isCalibrated = true; } diff --git a/src/main/java/competition/subsystems/drive/commands/DepotCollectionAutoCommand.java b/src/main/java/competition/subsystems/drive/commands/DepotCollectionAutoCommand.java index e7bbe0ff..b73e79ab 100644 --- a/src/main/java/competition/subsystems/drive/commands/DepotCollectionAutoCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DepotCollectionAutoCommand.java @@ -20,8 +20,8 @@ public class DepotCollectionAutoCommand extends SwerveSimpleTrajectoryCommand { public Pose2d outpostPose; @Inject public DepotCollectionAutoCommand(DriveSubsystem drive, BaseSwerveDriveSubsystem baseSwerveDriveSubsystem, - PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory, - PoseSubsystem pose, RobotAssertionManager robotAssertionManager) { + PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory, + PoseSubsystem pose, RobotAssertionManager robotAssertionManager) { super(drive,pose,pf,headingModuleFactory,robotAssertionManager); pf.setPrefix("DepotCollection"); this.drive = drive; @@ -30,29 +30,15 @@ public DepotCollectionAutoCommand(DriveSubsystem drive, BaseSwerveDriveSubsystem @Override public void initialize() { - Pose2d depotWallSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotWallSide); Pose2d depotTowerSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotTowerSide); ArrayList swervePoints = new ArrayList<>(); - swervePoints.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( - depotWallSidePose, 3)); + swervePoints.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( depotTowerSidePose, 3)); this.logic.setKeyPoints(swervePoints); this.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); super.initialize(); - log.info("DepotCollectionAutoCommand initialized"); - - } - - @Override - public void execute() { - super.execute(); - } - - @Override - public boolean isFinished() { - return super.isFinished(); } @Override diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToDepotAutoCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToDepotAutoCommand.java new file mode 100644 index 00000000..d5c1823a --- /dev/null +++ b/src/main/java/competition/subsystems/drive/commands/DriveToDepotAutoCommand.java @@ -0,0 +1,49 @@ +package competition.subsystems.drive.commands; + +import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.pose.Landmarks; +import competition.subsystems.pose.PoseSubsystem; +import edu.wpi.first.math.geometry.Pose2d; +import xbot.common.logging.RobotAssertionManager; +import xbot.common.properties.PropertyFactory; +import xbot.common.subsystems.drive.BaseSwerveDriveSubsystem; +import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; +import xbot.common.subsystems.drive.control_logic.HeadingModule; +import xbot.common.trajectory.XbotSwervePoint; + +import javax.inject.Inject; +import java.util.ArrayList; + +public class DriveToDepotAutoCommand extends SwerveSimpleTrajectoryCommand { + final DriveSubsystem drive; + + public Pose2d outpostPose; + @Inject + public DriveToDepotAutoCommand(DriveSubsystem drive, BaseSwerveDriveSubsystem baseSwerveDriveSubsystem, + PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory, + PoseSubsystem pose, RobotAssertionManager robotAssertionManager) { + super(drive,pose,pf,headingModuleFactory,robotAssertionManager); + pf.setPrefix("DepotCollection"); + this.drive = drive; + this.addRequirements(drive); + } + + @Override + public void initialize() { + Pose2d depotWallSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotWallSide); + + + ArrayList swervePoints = new ArrayList<>(); + swervePoints.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( + depotWallSidePose, 3)); + + this.logic.setKeyPoints(swervePoints); + this.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); + super.initialize(); + } + + @Override + public void end(boolean interrupted) { + log.info("end"); + } +} \ No newline at end of file From 35de339806f07245465cfee6d1c5cf1a19d4b03a Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sun, 22 Feb 2026 09:20:20 -0800 Subject: [PATCH 04/12] redsigned commnad group --- .../DepotCollectionAutoCommandGroup.java | 60 +++++++++++++++++-- 1 file changed, 55 insertions(+), 5 deletions(-) diff --git a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java index 514b7064..4f50861b 100644 --- a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java +++ b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java @@ -1,21 +1,71 @@ package competition.command_groups; +import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.drive.commands.DepotCollectionAutoCommand; import competition.subsystems.drive.commands.DriveToDepotAutoCommand; +import competition.subsystems.hopper_roller.HopperRollerSubsystem; +import competition.subsystems.intake_deploy.commands.IntakeDeployExtendCommand; +import competition.subsystems.intake_deploy.commands.IntakeDeployStopCommand; import competition.subsystems.pose.Landmarks; import competition.subsystems.pose.PoseSubsystem; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import xbot.common.command.BaseSequentialCommandGroup; +import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; +import xbot.common.trajectory.XbotSwervePoint; -public class DepotCollectionAutoCommandGroup extends SequentialCommandGroup { - public DepotCollectionAutoCommandGroup(DriveToDepotAutoCommand driveToDepotAutoCommand, +import javax.inject.Provider; +import java.util.ArrayList; + +public class DepotCollectionAutoCommandGroup extends BaseSequentialCommandGroup { + public DepotCollectionAutoCommandGroup( + Provider trajectoryProvider, + DriveToDepotAutoCommand driveToDepotAutoCommand, DepotCollectionAutoCommand depotCollectionAutoCommand, HopperAndIntakeCommandGroup hopperAndIntakeCommandGroup, + IntakeDeployExtendCommand intakeDeployExtendCommand, + IntakeDeployStopCommand intakeDeployStopCommand, + HopperRollerSubsystem hopper, + DriveSubsystem drive, PoseSubsystem pose) { + + var readyDepotCollect = trajectoryProvider.get(); + + Pose2d depotWallSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotWallSide); + + ArrayList readyPoint = new ArrayList<>(); + + readyPoint.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( + depotWallSidePose, 3)); + + readyDepotCollect.logic.setKeyPoints(readyPoint); + readyDepotCollect.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); + + var depotCollect = trajectoryProvider.get(); + + Pose2d depotTowerSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotTowerSide); + + ArrayList collectPoint = new ArrayList<>(); + + collectPoint.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( + depotTowerSidePose, 3)); + + depotCollect.logic.setKeyPoints(collectPoint); + depotCollect.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); + addCommands( pose.createSetPositionCommand(PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueStartTrenchToDepot)), - driveToDepotAutoCommand, - hopperAndIntakeCommandGroup, - depotCollectionAutoCommand + new ParallelCommandGroup( + intakeDeployExtendCommand, + hopperAndIntakeCommandGroup + ), + readyDepotCollect, + depotCollect, + new ParallelCommandGroup( + intakeDeployStopCommand, + hopper.getStopCommand() + ) ); } } From b722379807dd334445db88095e52542b6eb081bb Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Mon, 23 Feb 2026 03:41:49 -0800 Subject: [PATCH 05/12] Removed old commands and testing --- .../DepotCollectionAutoCommandGroup.java | 12 ++--- .../OperatorCommandMap.java | 4 +- .../subsystems/climber/ClimberSubsystem.java | 2 +- .../commands/DepotCollectionAutoCommand.java | 48 ------------------ .../commands/DriveToDepotAutoCommand.java | 49 ------------------- 5 files changed, 8 insertions(+), 107 deletions(-) delete mode 100644 src/main/java/competition/subsystems/drive/commands/DepotCollectionAutoCommand.java delete mode 100644 src/main/java/competition/subsystems/drive/commands/DriveToDepotAutoCommand.java diff --git a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java index 4f50861b..c5321dd7 100644 --- a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java +++ b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java @@ -1,8 +1,6 @@ package competition.command_groups; import competition.subsystems.drive.DriveSubsystem; -import competition.subsystems.drive.commands.DepotCollectionAutoCommand; -import competition.subsystems.drive.commands.DriveToDepotAutoCommand; import competition.subsystems.hopper_roller.HopperRollerSubsystem; import competition.subsystems.intake_deploy.commands.IntakeDeployExtendCommand; import competition.subsystems.intake_deploy.commands.IntakeDeployStopCommand; @@ -10,19 +8,19 @@ import competition.subsystems.pose.PoseSubsystem; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import xbot.common.command.BaseSequentialCommandGroup; import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; import xbot.common.trajectory.XbotSwervePoint; +import javax.inject.Inject; import javax.inject.Provider; import java.util.ArrayList; + public class DepotCollectionAutoCommandGroup extends BaseSequentialCommandGroup { - public DepotCollectionAutoCommandGroup( - Provider trajectoryProvider, - DriveToDepotAutoCommand driveToDepotAutoCommand, - DepotCollectionAutoCommand depotCollectionAutoCommand, + + @Inject + public DepotCollectionAutoCommandGroup(Provider trajectoryProvider, HopperAndIntakeCommandGroup hopperAndIntakeCommandGroup, IntakeDeployExtendCommand intakeDeployExtendCommand, IntakeDeployStopCommand intakeDeployStopCommand, diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index f0617d84..257f9984 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -3,6 +3,7 @@ import javax.inject.Inject; import javax.inject.Singleton; +import competition.command_groups.DepotCollectionAutoCommandGroup; import competition.simulation.commands.ResetSimulatedPoseCommand; import competition.command_groups.FireWhenShooterReadyCommandGroup; import competition.subsystems.climber.ClimberSubsystem; @@ -10,7 +11,6 @@ import competition.subsystems.climber.commands.ClimberExtendCommand; import competition.subsystems.climber.commands.ClimberRetractCommand; import competition.subsystems.drive.commands.DebugSwerveModuleCommand; -import competition.subsystems.drive.commands.DriveToDepotAutoCommand; import competition.subsystems.drive.commands.SwerveDriveWithJoysticksCommand; import competition.subsystems.fuel_intake.commands.FuelEjectCommand; import competition.subsystems.fuel_intake.commands.FuelIntakeCommand; @@ -67,7 +67,7 @@ public void setupDriveCommands(OperatorInterface operatorInterface, @Inject public void setupSimulatorCommands( ResetSimulatedPoseCommand resetSimulatorPositionCommand, - DriveToDepotAutoCommand depotCollectionAutoCommand + DepotCollectionAutoCommandGroup depotCollectionAutoCommand ) { resetSimulatorPositionCommand.includeOnSmartDashboard("Reset Simulator Position"); depotCollectionAutoCommand.includeOnSmartDashboard("Depot collection"); diff --git a/src/main/java/competition/subsystems/climber/ClimberSubsystem.java b/src/main/java/competition/subsystems/climber/ClimberSubsystem.java index 5a31f6c3..39a3d1e6 100644 --- a/src/main/java/competition/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/competition/subsystems/climber/ClimberSubsystem.java @@ -126,7 +126,7 @@ public void periodic() { @Override public Angle getCurrentValue() { double currentAngle = 0; - if (climberEncoder != null) { + if (climberEncoder != null && climberEncoder.getAbsolutePosition() != null ) { currentAngle = getCalibratedPosition().in(Rotations) * degreesPerRotation.get(); } return Degrees.of(currentAngle); diff --git a/src/main/java/competition/subsystems/drive/commands/DepotCollectionAutoCommand.java b/src/main/java/competition/subsystems/drive/commands/DepotCollectionAutoCommand.java deleted file mode 100644 index b73e79ab..00000000 --- a/src/main/java/competition/subsystems/drive/commands/DepotCollectionAutoCommand.java +++ /dev/null @@ -1,48 +0,0 @@ -package competition.subsystems.drive.commands; - -import competition.subsystems.drive.DriveSubsystem; -import competition.subsystems.pose.Landmarks; -import competition.subsystems.pose.PoseSubsystem; -import edu.wpi.first.math.geometry.Pose2d; -import xbot.common.logging.RobotAssertionManager; -import xbot.common.properties.PropertyFactory; -import xbot.common.subsystems.drive.BaseSwerveDriveSubsystem; -import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; -import xbot.common.subsystems.drive.control_logic.HeadingModule; -import xbot.common.trajectory.XbotSwervePoint; - -import javax.inject.Inject; -import java.util.ArrayList; - -public class DepotCollectionAutoCommand extends SwerveSimpleTrajectoryCommand { - final DriveSubsystem drive; - - public Pose2d outpostPose; - @Inject - public DepotCollectionAutoCommand(DriveSubsystem drive, BaseSwerveDriveSubsystem baseSwerveDriveSubsystem, - PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory, - PoseSubsystem pose, RobotAssertionManager robotAssertionManager) { - super(drive,pose,pf,headingModuleFactory,robotAssertionManager); - pf.setPrefix("DepotCollection"); - this.drive = drive; - this.addRequirements(drive); - } - - @Override - public void initialize() { - Pose2d depotTowerSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotTowerSide); - - ArrayList swervePoints = new ArrayList<>(); - - swervePoints.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( - depotTowerSidePose, 3)); - this.logic.setKeyPoints(swervePoints); - this.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); - super.initialize(); - } - - @Override - public void end(boolean interrupted) { - log.info("end"); - } -} \ No newline at end of file diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToDepotAutoCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToDepotAutoCommand.java deleted file mode 100644 index d5c1823a..00000000 --- a/src/main/java/competition/subsystems/drive/commands/DriveToDepotAutoCommand.java +++ /dev/null @@ -1,49 +0,0 @@ -package competition.subsystems.drive.commands; - -import competition.subsystems.drive.DriveSubsystem; -import competition.subsystems.pose.Landmarks; -import competition.subsystems.pose.PoseSubsystem; -import edu.wpi.first.math.geometry.Pose2d; -import xbot.common.logging.RobotAssertionManager; -import xbot.common.properties.PropertyFactory; -import xbot.common.subsystems.drive.BaseSwerveDriveSubsystem; -import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; -import xbot.common.subsystems.drive.control_logic.HeadingModule; -import xbot.common.trajectory.XbotSwervePoint; - -import javax.inject.Inject; -import java.util.ArrayList; - -public class DriveToDepotAutoCommand extends SwerveSimpleTrajectoryCommand { - final DriveSubsystem drive; - - public Pose2d outpostPose; - @Inject - public DriveToDepotAutoCommand(DriveSubsystem drive, BaseSwerveDriveSubsystem baseSwerveDriveSubsystem, - PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory, - PoseSubsystem pose, RobotAssertionManager robotAssertionManager) { - super(drive,pose,pf,headingModuleFactory,robotAssertionManager); - pf.setPrefix("DepotCollection"); - this.drive = drive; - this.addRequirements(drive); - } - - @Override - public void initialize() { - Pose2d depotWallSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotWallSide); - - - ArrayList swervePoints = new ArrayList<>(); - swervePoints.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( - depotWallSidePose, 3)); - - this.logic.setKeyPoints(swervePoints); - this.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); - super.initialize(); - } - - @Override - public void end(boolean interrupted) { - log.info("end"); - } -} \ No newline at end of file From 4161979d55a1f1ad1eb1814598506f265e8e60a8 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Mon, 23 Feb 2026 06:01:18 -0800 Subject: [PATCH 06/12] Adjustments to auto and landmarks --- .../command_groups/DepotCollectionAutoCommandGroup.java | 3 ++- src/main/java/competition/subsystems/pose/Landmarks.java | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java index c5321dd7..e484ec79 100644 --- a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java +++ b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java @@ -1,6 +1,7 @@ package competition.command_groups; import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.fuel_intake.commands.FuelIntakeCommand; import competition.subsystems.hopper_roller.HopperRollerSubsystem; import competition.subsystems.intake_deploy.commands.IntakeDeployExtendCommand; import competition.subsystems.intake_deploy.commands.IntakeDeployStopCommand; @@ -35,7 +36,7 @@ public DepotCollectionAutoCommandGroup(Provider t ArrayList readyPoint = new ArrayList<>(); readyPoint.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( - depotWallSidePose, 3)); + depotWallSidePose, 2.5)); readyDepotCollect.logic.setKeyPoints(readyPoint); readyDepotCollect.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); diff --git a/src/main/java/competition/subsystems/pose/Landmarks.java b/src/main/java/competition/subsystems/pose/Landmarks.java index 39933f2b..77b39546 100644 --- a/src/main/java/competition/subsystems/pose/Landmarks.java +++ b/src/main/java/competition/subsystems/pose/Landmarks.java @@ -36,8 +36,8 @@ public class Landmarks { // Blue Depot public static Pose2d blueDepotCollectCenter = new Pose2d(1.180, 5.940, Rotation2d.fromDegrees(180)); - public static Pose2d blueDepotWallSide = new Pose2d(0.44, 6.980, Rotation2d.fromDegrees(270)); - public static Pose2d blueDepotTowerSide = new Pose2d(0.44, 4.950, Rotation2d.fromDegrees(270)); + public static Pose2d blueDepotWallSide = new Pose2d(0.46, 7.390, Rotation2d.fromDegrees(90)); + public static Pose2d blueDepotTowerSide = new Pose2d(0.46, 4.950, Rotation2d.fromDegrees(90)); public static Pose2d blueDepotCenter = new Pose2d(0.270,5.940, Rotation2d.fromDegrees(0)); From ca26e280fadf4d37aac643a95a6af0b3f208c7c7 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Tue, 24 Feb 2026 20:02:13 -0800 Subject: [PATCH 07/12] WIP --- .../command_groups/DepotCollectionAutoCommandGroup.java | 3 ++- .../subsystems/fuel_intake/commands/FuelIntakeCommand.java | 6 ++++++ .../intake_deploy/commands/IntakeDeployExtendCommand.java | 2 +- .../commands/IntakeDeployMaintainerCommand.java | 2 +- 4 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java index e484ec79..3b0e3580 100644 --- a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java +++ b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java @@ -9,6 +9,7 @@ import competition.subsystems.pose.PoseSubsystem; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import xbot.common.command.BaseSequentialCommandGroup; import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; import xbot.common.trajectory.XbotSwervePoint; @@ -55,7 +56,7 @@ public DepotCollectionAutoCommandGroup(Provider t addCommands( pose.createSetPositionCommand(PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueStartTrenchToDepot)), - new ParallelCommandGroup( + new ParallelDeadlineGroup( intakeDeployExtendCommand, hopperAndIntakeCommandGroup ), diff --git a/src/main/java/competition/subsystems/fuel_intake/commands/FuelIntakeCommand.java b/src/main/java/competition/subsystems/fuel_intake/commands/FuelIntakeCommand.java index b3e62b55..3a84c734 100644 --- a/src/main/java/competition/subsystems/fuel_intake/commands/FuelIntakeCommand.java +++ b/src/main/java/competition/subsystems/fuel_intake/commands/FuelIntakeCommand.java @@ -19,4 +19,10 @@ public void initialize() { intakeSubsystem.intake(); log.info("Initialized FuelIntake"); } + + @Override + public boolean isFinished() { + return true; + } + } diff --git a/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployExtendCommand.java b/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployExtendCommand.java index ae8eabfd..23347f58 100644 --- a/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployExtendCommand.java +++ b/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployExtendCommand.java @@ -24,7 +24,7 @@ public void initialize() { @Override public boolean isFinished() { - return false; + return true; } } diff --git a/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployMaintainerCommand.java b/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployMaintainerCommand.java index c9c97032..6d07ea2d 100644 --- a/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployMaintainerCommand.java +++ b/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployMaintainerCommand.java @@ -39,7 +39,7 @@ protected void coastAction() { @Override protected void calibratedMachineControlAction() { - subsystem.setPositionGoal(subsystem.getTargetValue()); +// subsystem.setPositionGoal(subsystem.getTargetValue()); } @Override From e6a6ffb0ab01f1f0ee6b52e640e9b4cc55a98f15 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 7 Mar 2026 14:43:53 -0800 Subject: [PATCH 08/12] WIP --- .../command_groups/DepotCollectionAutoCommandGroup.java | 2 +- .../operator_interface/OperatorCommandMap.java | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java index 3b0e3580..b58d4afe 100644 --- a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java +++ b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java @@ -25,7 +25,7 @@ public class DepotCollectionAutoCommandGroup extends BaseSequentialCommandGroup public DepotCollectionAutoCommandGroup(Provider trajectoryProvider, HopperAndIntakeCommandGroup hopperAndIntakeCommandGroup, IntakeDeployExtendCommand intakeDeployExtendCommand, - IntakeDeployStopCommand intakeDeployStopCommand, + Intake intakeDeployStopCommand, HopperRollerSubsystem hopper, DriveSubsystem drive, PoseSubsystem pose) { diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index c3d23f4a..01cbd3c8 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -40,6 +40,7 @@ import xbot.common.subsystems.drive.swerve.commands.ChangeActiveSwerveModuleCommand; import xbot.common.subsystems.pose.commands.SetRobotHeadingCommand; + import static edu.wpi.first.units.Units.RPM; /** @@ -167,8 +168,11 @@ public void setupAutoCommands(Provider setAutonomousComman @Inject public void setupSimulatorCommands( - ResetSimulatedPoseCommand resetPose + ResetSimulatedPoseCommand resetSimulatorPositionCommand, + Depot depotCollectionAutoCommand ) { - resetPose.includeOnSmartDashboard(); + resetSimulatorPositionCommand.includeOnSmartDashboard("Reset Simulator Position"); + depotCollectionAutoCommand.includeOnSmartDashboard("Depot collection"); + } } \ No newline at end of file From c2a43efd79a8e8923a6695a08d4631089a3aa4ab Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 7 Mar 2026 16:18:46 -0800 Subject: [PATCH 09/12] wip --- .../DepotCollectionAutoCommandGroup.java | 12 +++---- .../DriveToDepotTowerSideCommand.java | 32 +++++++++++++++++++ .../OperatorCommandMap.java | 3 +- 3 files changed, 39 insertions(+), 8 deletions(-) create mode 100644 src/main/java/competition/command_groups/DriveToDepotTowerSideCommand.java diff --git a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java index b58d4afe..766d5682 100644 --- a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java +++ b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java @@ -1,10 +1,8 @@ package competition.command_groups; import competition.subsystems.drive.DriveSubsystem; -import competition.subsystems.fuel_intake.commands.FuelIntakeCommand; import competition.subsystems.hopper_roller.HopperRollerSubsystem; import competition.subsystems.intake_deploy.commands.IntakeDeployExtendCommand; -import competition.subsystems.intake_deploy.commands.IntakeDeployStopCommand; import competition.subsystems.pose.Landmarks; import competition.subsystems.pose.PoseSubsystem; import edu.wpi.first.math.geometry.Pose2d; @@ -12,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import xbot.common.command.BaseSequentialCommandGroup; import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; +import xbot.common.subsystems.pose.GameField; import xbot.common.trajectory.XbotSwervePoint; import javax.inject.Inject; @@ -21,14 +20,14 @@ public class DepotCollectionAutoCommandGroup extends BaseSequentialCommandGroup { + GameField gameField; + @Inject public DepotCollectionAutoCommandGroup(Provider trajectoryProvider, HopperAndIntakeCommandGroup hopperAndIntakeCommandGroup, IntakeDeployExtendCommand intakeDeployExtendCommand, - Intake intakeDeployStopCommand, - HopperRollerSubsystem hopper, - DriveSubsystem drive, - PoseSubsystem pose) { + HopperRollerSubsystem hopper, GameField gameField, + DriveSubsystem drive, PoseSubsystem pose) { var readyDepotCollect = trajectoryProvider.get(); @@ -63,7 +62,6 @@ public DepotCollectionAutoCommandGroup(Provider t readyDepotCollect, depotCollect, new ParallelCommandGroup( - intakeDeployStopCommand, hopper.getStopCommand() ) ); diff --git a/src/main/java/competition/command_groups/DriveToDepotTowerSideCommand.java b/src/main/java/competition/command_groups/DriveToDepotTowerSideCommand.java new file mode 100644 index 00000000..869bb83b --- /dev/null +++ b/src/main/java/competition/command_groups/DriveToDepotTowerSideCommand.java @@ -0,0 +1,32 @@ +package competition.command_groups; + +import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.pose.AutoLandmarks; +import competition.subsystems.pose.PoseSubsystem; +import xbot.common.injection.electrical_contract.XSwerveDriveElectricalContract; +import xbot.common.logging.RobotAssertionManager; +import xbot.common.properties.PropertyFactory; +import xbot.common.subsystems.drive.BaseSwerveDriveSubsystem; +import xbot.common.subsystems.drive.SwerveSimpleBezierCommand; +import xbot.common.subsystems.drive.control_logic.HeadingModule; +import xbot.common.subsystems.oracle.SwervePointPathPlanning; +import xbot.common.subsystems.pose.BasePoseSubsystem; +import xbot.common.subsystems.pose.GameField; + +public class DriveToDepotTowerSideCommand extends SwerveSimpleBezierCommand { + + public DriveToDepotTowerSideCommand(DriveSubsystem drive, PoseSubsystem pose, + PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory, + XSwerveDriveElectricalContract electricalContract, + RobotAssertionManager robotAssertionManager, SwervePointPathPlanning pathPlanning, GameField gamefield, + AutoLandmarks autoLandmarks) { + super(drive, pose, pf, headingModuleFactory, robotAssertionManager); + + this.drive = drive; + this.pose = pose; + this.pathPlanning = pathPlanning; + this.gamefield = gamefield; + this.robotRadius = electricalContract.getRadiusOfRobot(); + this.autoLandmarks = autoLandmarks; + } +} diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 01cbd3c8..78ddd176 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -5,6 +5,7 @@ import javax.inject.Singleton; import competition.auto_programs.vision.MoveAcrossFieldCommandGroup; +import competition.command_groups.DepotCollectionAutoCommandGroup; import competition.command_groups.FireWhenReadyShooterCommandGroup; import competition.command_groups.MinHoodShootingCommandGroup; import competition.command_groups.DriveToShootingPositionCommand; @@ -169,7 +170,7 @@ public void setupAutoCommands(Provider setAutonomousComman @Inject public void setupSimulatorCommands( ResetSimulatedPoseCommand resetSimulatorPositionCommand, - Depot depotCollectionAutoCommand + DepotCollectionAutoCommandGroup depotCollectionAutoCommand ) { resetSimulatorPositionCommand.includeOnSmartDashboard("Reset Simulator Position"); depotCollectionAutoCommand.includeOnSmartDashboard("Depot collection"); From 604c5c9b096caeddb92c1461d62356e79e25fd08 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 4 Apr 2026 11:22:00 -0700 Subject: [PATCH 10/12] Updated auto --- .../DepotCollectionAutoCommandGroup.java | 71 +++++++++++++++---- .../DriveToDepotTowerSideCommand.java | 32 --------- .../OperatorCommandMap.java | 5 +- .../hopper_roller/HopperRollerSubsystem.java | 3 +- .../subsystems/pose/Landmarks.java | 8 ++- 5 files changed, 68 insertions(+), 51 deletions(-) delete mode 100644 src/main/java/competition/command_groups/DriveToDepotTowerSideCommand.java diff --git a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java index 766d5682..fb46b80c 100644 --- a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java +++ b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java @@ -1,58 +1,85 @@ package competition.command_groups; +import competition.auto_programs.AimAndShootFromHereCommand; import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.hopper_roller.HopperRollerSubsystem; import competition.subsystems.intake_deploy.commands.IntakeDeployExtendCommand; +import competition.subsystems.pose.AutoLandmarks; import competition.subsystems.pose.Landmarks; import competition.subsystems.pose.PoseSubsystem; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import xbot.common.command.BaseSequentialCommandGroup; import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; -import xbot.common.subsystems.pose.GameField; import xbot.common.trajectory.XbotSwervePoint; import javax.inject.Inject; import javax.inject.Provider; import java.util.ArrayList; +import java.util.Set; public class DepotCollectionAutoCommandGroup extends BaseSequentialCommandGroup { - GameField gameField; - @Inject public DepotCollectionAutoCommandGroup(Provider trajectoryProvider, HopperAndIntakeCommandGroup hopperAndIntakeCommandGroup, IntakeDeployExtendCommand intakeDeployExtendCommand, - HopperRollerSubsystem hopper, GameField gameField, - DriveSubsystem drive, PoseSubsystem pose) { + HopperRollerSubsystem hopper, + AutoLandmarks autoLandmarks, + DriveSubsystem drive, PoseSubsystem pose, + DriveToShootingPositionCommand driveToPositionCommand, + AimAndShootFromHereCommand aimAndShootFromHereCommand + ) { var readyDepotCollect = trajectoryProvider.get(); - Pose2d depotWallSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotWallSide); + Pose2d depotCollectCenter = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotCollectCenter); ArrayList readyPoint = new ArrayList<>(); readyPoint.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( - depotWallSidePose, 2.5)); + depotCollectCenter, 2)); readyDepotCollect.logic.setKeyPoints(readyPoint); readyDepotCollect.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); var depotCollect = trajectoryProvider.get(); - Pose2d depotTowerSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotTowerSide); + Pose2d depotTowerSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotCollectPointShallow); ArrayList collectPoint = new ArrayList<>(); collectPoint.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( - depotTowerSidePose, 3)); + depotTowerSidePose, 2)); depotCollect.logic.setKeyPoints(collectPoint); depotCollect.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); + var readyDepotCollect2 = trajectoryProvider.get(); + + ArrayList readyPoint2 = new ArrayList<>(); + + readyPoint2.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( + depotCollectCenter,1)); + + readyDepotCollect2.logic.setKeyPoints(readyPoint2); + readyDepotCollect2.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); + + var depotCollectDeep = trajectoryProvider.get(); + + Pose2d DepotPointDeep = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotCollectPointDeep); + + ArrayList deepCollectPoint = new ArrayList<>(); + + deepCollectPoint.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( + DepotPointDeep, 1) + ); + + depotCollectDeep.logic.setKeyPoints(deepCollectPoint); + depotCollectDeep.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); + addCommands( pose.createSetPositionCommand(PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueStartTrenchToDepot)), new ParallelDeadlineGroup( @@ -61,9 +88,25 @@ public DepotCollectionAutoCommandGroup(Provider t ), readyDepotCollect, depotCollect, - new ParallelCommandGroup( - hopper.getStopCommand() - ) - ); + readyDepotCollect2, + depotCollectDeep, + hopper.getStopCommand(), + Commands.defer(() -> { + Pose2d currentPose = pose.getCurrentPose2d(); + Pose2d endPose = autoLandmarks.getClosestShootingPose(currentPose); + + var shootPose = trajectoryProvider.get(); + ArrayList shootPoint = new ArrayList<>(); + shootPoint.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(endPose, + 1)); + shootPose.logic.setKeyPoints(shootPoint); + shootPose.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); + + return shootPose; + }, + Set.of(drive) + + ), + aimAndShootFromHereCommand); } } diff --git a/src/main/java/competition/command_groups/DriveToDepotTowerSideCommand.java b/src/main/java/competition/command_groups/DriveToDepotTowerSideCommand.java deleted file mode 100644 index 869bb83b..00000000 --- a/src/main/java/competition/command_groups/DriveToDepotTowerSideCommand.java +++ /dev/null @@ -1,32 +0,0 @@ -package competition.command_groups; - -import competition.subsystems.drive.DriveSubsystem; -import competition.subsystems.pose.AutoLandmarks; -import competition.subsystems.pose.PoseSubsystem; -import xbot.common.injection.electrical_contract.XSwerveDriveElectricalContract; -import xbot.common.logging.RobotAssertionManager; -import xbot.common.properties.PropertyFactory; -import xbot.common.subsystems.drive.BaseSwerveDriveSubsystem; -import xbot.common.subsystems.drive.SwerveSimpleBezierCommand; -import xbot.common.subsystems.drive.control_logic.HeadingModule; -import xbot.common.subsystems.oracle.SwervePointPathPlanning; -import xbot.common.subsystems.pose.BasePoseSubsystem; -import xbot.common.subsystems.pose.GameField; - -public class DriveToDepotTowerSideCommand extends SwerveSimpleBezierCommand { - - public DriveToDepotTowerSideCommand(DriveSubsystem drive, PoseSubsystem pose, - PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory, - XSwerveDriveElectricalContract electricalContract, - RobotAssertionManager robotAssertionManager, SwervePointPathPlanning pathPlanning, GameField gamefield, - AutoLandmarks autoLandmarks) { - super(drive, pose, pf, headingModuleFactory, robotAssertionManager); - - this.drive = drive; - this.pose = pose; - this.pathPlanning = pathPlanning; - this.gamefield = gamefield; - this.robotRadius = electricalContract.getRadiusOfRobot(); - this.autoLandmarks = autoLandmarks; - } -} diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 19dde7d1..761a692c 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -10,6 +10,7 @@ import competition.auto_programs.vision.JustDriveNeutralMoveCommand; import competition.auto_programs.vision.MoveAcrossFieldCommandGroup; import competition.auto_programs.vision.ShootFromTrenchThenMoveToNeutralCommand; +import competition.command_groups.DepotCollectionAutoCommandGroup; import competition.command_groups.vision.DriveThroughAllianceTrenchCommand; import competition.command_groups.FireWhenReadyAndRetractIntakeDeployCommandGroup; import competition.command_groups.FireWhenShooterAndHoodReady; @@ -209,8 +210,10 @@ public void setupAutoCommands(Provider setAutonomousComman @Inject public void setupSimulatorCommands( - ResetSimulatedPoseCommand resetPose) { + ResetSimulatedPoseCommand resetPose, + DepotCollectionAutoCommandGroup depotCollectionAutoCommandGroup) { resetPose.includeOnSmartDashboard(); + depotCollectionAutoCommandGroup.includeOnSmartDashboard(); } @Inject diff --git a/src/main/java/competition/subsystems/hopper_roller/HopperRollerSubsystem.java b/src/main/java/competition/subsystems/hopper_roller/HopperRollerSubsystem.java index a991234a..46bead46 100644 --- a/src/main/java/competition/subsystems/hopper_roller/HopperRollerSubsystem.java +++ b/src/main/java/competition/subsystems/hopper_roller/HopperRollerSubsystem.java @@ -2,6 +2,7 @@ import competition.electrical_contract.ElectricalContract; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import xbot.common.command.BaseSubsystem; import xbot.common.command.NamedRunCommand; import xbot.common.controls.actuators.XCANMotorController; @@ -145,7 +146,7 @@ public Command getIntakeCommand() { } public Command getStopCommand() { - return new NamedRunCommand(getName() + "-stop", this::stop, this); + return new InstantCommand(this::stop, this); } public Command getCollectCommand() {return new NamedRunCommand(getName() + "-collect", this::setCollectPower, this);} diff --git a/src/main/java/competition/subsystems/pose/Landmarks.java b/src/main/java/competition/subsystems/pose/Landmarks.java index fbe9ff4c..02d5b08b 100644 --- a/src/main/java/competition/subsystems/pose/Landmarks.java +++ b/src/main/java/competition/subsystems/pose/Landmarks.java @@ -41,10 +41,12 @@ public class Landmarks { // Blue Depot public static Pose2d blueDepotCollectCenter = new Pose2d(1.180, 5.940, Rotation2d.fromDegrees(180)); - public static Pose2d blueDepotWallSide = new Pose2d(0.46, 7.390, Rotation2d.fromDegrees(90)); - public static Pose2d blueDepotTowerSide = new Pose2d(0.46, 4.950, Rotation2d.fromDegrees(90)); + public static Pose2d blueDepotCollectPointShallow = new Pose2d(.65, 5.940, Rotation2d.fromDegrees(180)); + public static Pose2d blueDepotCollectPointDeep = new Pose2d(.5, 5.940, Rotation2d.fromDegrees(180)); + public static Pose2d blueDepotWallSide = new Pose2d(0.46, 7.390, Rotation2d.fromDegrees(270)); + public static Pose2d blueDepotTowerSide = new Pose2d(0.46, 4.950, Rotation2d.fromDegrees(270)); - public static Pose2d blueDepotCenter = new Pose2d(0.270,5.940, Rotation2d.fromDegrees(0)); + public static Pose2d blueDepotCenter = new Pose2d(0.270,5.940, Rotation2d.fromDegrees(180)); // Blue Outpost public static Pose2d blueOutpost = new Pose2d(0, 0.650, Rotation2d.fromDegrees(0)); From 405f2cb66408242714e3bf8922da5cdd4316751d Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 4 Apr 2026 11:34:32 -0700 Subject: [PATCH 11/12] restart maplesim start --- src/main/java/competition/simulation/MapleSimulator.java | 2 +- src/main/java/competition/subsystems/pose/Landmarks.java | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/competition/simulation/MapleSimulator.java b/src/main/java/competition/simulation/MapleSimulator.java index b3eb6347..9a0c43c3 100644 --- a/src/main/java/competition/simulation/MapleSimulator.java +++ b/src/main/java/competition/simulation/MapleSimulator.java @@ -95,7 +95,7 @@ public MapleSimulator(PoseSubsystem pose, DriveSubsystem drive, ShooterSimulator // starting // middle ish of the field on blue - var startingPose = blueStartTrenchToDepot; + var startingPose = new Pose2d(7, 7 , new Rotation2d()); // Creating the SelfControlledSwerveDriveSimulation instance this.swerveDriveSimulation = new SelfControlledSwerveDriveSimulation( diff --git a/src/main/java/competition/subsystems/pose/Landmarks.java b/src/main/java/competition/subsystems/pose/Landmarks.java index 02d5b08b..c959a79f 100644 --- a/src/main/java/competition/subsystems/pose/Landmarks.java +++ b/src/main/java/competition/subsystems/pose/Landmarks.java @@ -32,6 +32,9 @@ public class Landmarks { public static Pose2d blueStartTrenchToDepot = new Pose2d(3.57,7.390, Rotation2d.fromDegrees(0)); public static Pose2d blueStartBumpToDepot = new Pose2d(3.57,5.980, Rotation2d.fromDegrees(0)); + // Starting from the hub center + public static Pose2d blueStartHub = new Pose2d(3.57,4, Rotation2d.fromDegrees(180)); + // Starting on blue alliance towards neutral area public static Pose2d blueStartBumpToNeutralAreaOutpostSide = new Pose2d(4.480,.65, Rotation2d.fromDegrees(180)); public static Pose2d blueStartTrenchToNeutralAreaOutpostSide = new Pose2d(4.480,2.6, Rotation2d.fromDegrees(180)); From 6fbe9ca4e6de2e47e97dce7abd115ad4ca5c9e81 Mon Sep 17 00:00:00 2001 From: David Nguyen Date: Sat, 4 Apr 2026 12:25:06 -0700 Subject: [PATCH 12/12] name change --- .../command_groups/DepotCollectionAutoCommandGroup.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java index fb46b80c..481d387b 100644 --- a/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java +++ b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java @@ -69,12 +69,12 @@ public DepotCollectionAutoCommandGroup(Provider t var depotCollectDeep = trajectoryProvider.get(); - Pose2d DepotPointDeep = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotCollectPointDeep); + Pose2d depotPointDeep = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotCollectPointDeep); ArrayList deepCollectPoint = new ArrayList<>(); deepCollectPoint.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( - DepotPointDeep, 1) + depotPointDeep, 1) ); depotCollectDeep.logic.setKeyPoints(deepCollectPoint);