Depot collection and shoot auto

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<SwerveSimpleTrajectoryCommand> 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<XbotSwervePoint> 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<XbotSwervePoint> collectPoint = new ArrayList<>();
+
+        collectPoint.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(
+                depotTowerSidePose, 2));
+
+        depotCollect.logic.setKeyPoints(collectPoint);
+        depotCollect.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond());
+
+        var readyDepotCollect2 = trajectoryProvider.get();
+
+        ArrayList<XbotSwervePoint> 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<XbotSwervePoint> 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<XbotSwervePoint> shootPoint = new ArrayList<>();
+                            shootPoint.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(endPose,
+                                    1));
+                            shootPose.logic.setKeyPoints(shootPoint);
+                            shootPose.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond());
+
+                            return shootPose;
+                        },
+                        Set.of(drive)
+
+                ),
+                aimAndShootFromHereCommand);
+    }
+}

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<SetAutonomousCommand> setAutonomousComman
 
     @Inject
     public void setupSimulatorCommands(
-            ResetSimulatedPoseCommand resetPose) {
+            ResetSimulatedPoseCommand resetPose,
+            DepotCollectionAutoCommandGroup depotCollectionAutoCommandGroup) {
         resetPose.includeOnSmartDashboard();
+        depotCollectionAutoCommandGroup.includeOnSmartDashboard();
     }
 
     @Inject

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

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);}

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

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));