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..481d387b --- /dev/null +++ b/src/main/java/competition/command_groups/DepotCollectionAutoCommandGroup.java @@ -0,0 +1,112 @@ +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.Commands; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +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; +import java.util.Set; + + +public class DepotCollectionAutoCommandGroup extends BaseSequentialCommandGroup { + + @Inject + public DepotCollectionAutoCommandGroup(Provider trajectoryProvider, + HopperAndIntakeCommandGroup hopperAndIntakeCommandGroup, + IntakeDeployExtendCommand intakeDeployExtendCommand, + HopperRollerSubsystem hopper, + AutoLandmarks autoLandmarks, + DriveSubsystem drive, PoseSubsystem pose, + DriveToShootingPositionCommand driveToPositionCommand, + AimAndShootFromHereCommand aimAndShootFromHereCommand + ) { + + var readyDepotCollect = trajectoryProvider.get(); + + Pose2d depotCollectCenter = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotCollectCenter); + + ArrayList readyPoint = new ArrayList<>(); + + readyPoint.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( + depotCollectCenter, 2)); + + readyDepotCollect.logic.setKeyPoints(readyPoint); + readyDepotCollect.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond()); + + var depotCollect = trajectoryProvider.get(); + + Pose2d depotTowerSidePose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.blueDepotCollectPointShallow); + + ArrayList collectPoint = new ArrayList<>(); + + collectPoint.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( + 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( + intakeDeployExtendCommand, + hopperAndIntakeCommandGroup + ), + readyDepotCollect, + depotCollect, + 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/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/simulation/MapleSimulator.java b/src/main/java/competition/simulation/MapleSimulator.java index d2cdd4b9..9a0c43c3 100644 --- a/src/main/java/competition/simulation/MapleSimulator.java +++ b/src/main/java/competition/simulation/MapleSimulator.java @@ -19,6 +19,8 @@ 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; @@ -91,7 +93,8 @@ public MapleSimulator(PoseSubsystem pose, DriveSubsystem drive, ShooterSimulator drive.getRearRightSwerveModuleSubsystem().getModuleTranslation() }); - // starting middle ish of the field on blue + // starting + // middle ish of the field on blue var startingPose = new Pose2d(7, 7 , new Rotation2d()); // Creating the SelfControlledSwerveDriveSimulation instance 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/intake_deploy/commands/IntakeDeployMaintainerCommand.java b/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployMaintainerCommand.java index 0b7f7e8c..9a12dbf3 100644 --- a/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployMaintainerCommand.java +++ b/src/main/java/competition/subsystems/intake_deploy/commands/IntakeDeployMaintainerCommand.java @@ -36,7 +36,7 @@ protected void coastAction() { @Override protected void calibratedMachineControlAction() { - subsystem.setPositionGoal(subsystem.getTargetValue()); +// subsystem.setPositionGoal(subsystem.getTargetValue()); } @Override diff --git a/src/main/java/competition/subsystems/pose/Landmarks.java b/src/main/java/competition/subsystems/pose/Landmarks.java index ee5c9b3c..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)); @@ -41,10 +44,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.460, 6.980, Rotation2d.fromDegrees(270)); - public static Pose2d blueDepotTowerSide = new Pose2d(0.460, 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));