diff --git a/src/main/java/competition/auto_programs/AutoCommandFactory.java b/src/main/java/competition/auto_programs/AutoCommandFactory.java index 529546ef..ccd83879 100644 --- a/src/main/java/competition/auto_programs/AutoCommandFactory.java +++ b/src/main/java/competition/auto_programs/AutoCommandFactory.java @@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import xbot.common.command.BaseRobot; import xbot.common.subsystems.autonomous.AutonomousCommandSelector; import javax.inject.Inject; @@ -198,8 +199,11 @@ public Command driveToAllianceAndShoot(Command shootingDeadline) { * @param shootingDeadline when this command ends we stop shooting */ public Command fireWhenReadyUntilDone(Command shootingDeadline) { - var fireAndCloseIntake = runFeederProvider.get().alongWith(intakeOscillatingProvider.get()); + if (BaseRobot.isSimulation()) { + return new WaitForDurationCommand(() -> 1.0); + } + var fireAndCloseIntake = runFeederProvider.get().alongWith(intakeOscillatingProvider.get()); return waitForRotationAndHoodAndShooterToBeAtGoalProvider.get() .andThen(shootingDeadline .deadlineFor(fireAndCloseIntake)); diff --git a/src/main/java/competition/command_groups/DriveToNeutralZoneForIntakeSecondTimeCommand.java b/src/main/java/competition/command_groups/DriveToNeutralZoneForIntakeSecondTimeCommand.java index 82cfceb1..1fbed1c8 100644 --- a/src/main/java/competition/command_groups/DriveToNeutralZoneForIntakeSecondTimeCommand.java +++ b/src/main/java/competition/command_groups/DriveToNeutralZoneForIntakeSecondTimeCommand.java @@ -36,14 +36,8 @@ public DriveToNeutralZoneForIntakeSecondTimeCommand(DriveSubsystem drive, PoseSu @Override public void initialize() { var currentPose = pose.getCurrentPose2d(); - var pathPoses = this.autoLandmarks.getStartCollectionPath(currentPose); + var pathPoses = this.autoLandmarks.getNearestAllianceToNeutralTrenchPath(currentPose); - var endPathPose = this.autoLandmarks.getStartCollectionPose(currentPose); - var fullEndCollectionLocation = this.autoLandmarks.getMidBallPitCollectionPose(currentPose); - var endPose = new Pose2d(endPathPose.getTranslation().interpolate(fullEndCollectionLocation.getTranslation(), 0.75), - fullEndCollectionLocation.getRotation()); - - pathPoses.add(endPose); super.setSegmentType(SegmentType.Start); super.logic.setKeyPoints(this.pathPlanning.generateSwervePoints(currentPose, pathPoses, false, true)); diff --git a/src/main/java/competition/command_groups/FirstDriveForCollectionCommand.java b/src/main/java/competition/command_groups/FirstDriveForCollectionCommand.java index 70159487..d83257df 100644 --- a/src/main/java/competition/command_groups/FirstDriveForCollectionCommand.java +++ b/src/main/java/competition/command_groups/FirstDriveForCollectionCommand.java @@ -9,23 +9,23 @@ import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.pose.AutoLandmarks; import competition.subsystems.pose.PoseSubsystem; +import competition.subsystems.pose.RefinedSwervePointPathPlanning; import edu.wpi.first.math.geometry.Pose2d; import xbot.common.logging.RobotAssertionManager; import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.drive.control_logic.HeadingModule; -import xbot.common.subsystems.oracle.SwervePointPathPlanning; import xbot.common.subsystems.pose.GameField; import xbot.common.trajectory.XbotSwervePoint; public class FirstDriveForCollectionCommand extends BaseDriveWithSimpleBezierCommand { private final AutoLandmarks autoLandmarks; private final PoseSubsystem pose; - private final SwervePointPathPlanning pathPlanning; + private final RefinedSwervePointPathPlanning pathPlanning; @Inject public FirstDriveForCollectionCommand(DriveSubsystem drive, PoseSubsystem pose, PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory, - RobotAssertionManager robotAssertionManager, SwervePointPathPlanning pathPlanning, GameField gamefield, + RobotAssertionManager robotAssertionManager, RefinedSwervePointPathPlanning pathPlanning, GameField gamefield, AutoLandmarks autoLandmarks) { super(drive, pose, pf, headingModuleFactory, robotAssertionManager); this.pose = pose; @@ -33,22 +33,13 @@ public FirstDriveForCollectionCommand(DriveSubsystem drive, PoseSubsystem pose, this.autoLandmarks = autoLandmarks; } - private List calcSwervePoints() { + @Override + public void initialize() { var currentPose = this.pose.getCurrentPose2d(); var startPose = this.autoLandmarks.getStartCollectionPose(currentPose); - var fullEndCollectionLocation = this.autoLandmarks.getMidBallPitCollectionPose(currentPose); - var endPose = new Pose2d(startPose.getTranslation().interpolate(fullEndCollectionLocation.getTranslation(), 1.25), - fullEndCollectionLocation.getRotation()); + var path = this.autoLandmarks.getMidBallPitCollectionPath(startPose); - List points = new ArrayList<>(); - points.addAll(this.pathPlanning.generateSwervePoints(startPose, endPose, false)); - - return points; - } - - @Override - public void initialize() { - super.logic.setKeyPoints(this.calcSwervePoints()); + super.logic.setKeyPoints(this.pathPlanning.generateSwervePoints(startPose, path, false, true)); super.setSegmentType(SegmentType.Mid); super.setMaxSpeed(MaxSpeed.Intake); diff --git a/src/main/java/competition/command_groups/SecondDriveForCollectionCommand.java b/src/main/java/competition/command_groups/SecondDriveForCollectionCommand.java index 51c880d8..872ab2a0 100644 --- a/src/main/java/competition/command_groups/SecondDriveForCollectionCommand.java +++ b/src/main/java/competition/command_groups/SecondDriveForCollectionCommand.java @@ -9,23 +9,23 @@ import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.pose.AutoLandmarks; import competition.subsystems.pose.PoseSubsystem; +import competition.subsystems.pose.RefinedSwervePointPathPlanning; import edu.wpi.first.math.geometry.Pose2d; import xbot.common.logging.RobotAssertionManager; import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.drive.control_logic.HeadingModule; -import xbot.common.subsystems.oracle.SwervePointPathPlanning; import xbot.common.subsystems.pose.GameField; import xbot.common.trajectory.XbotSwervePoint; public class SecondDriveForCollectionCommand extends BaseDriveWithSimpleBezierCommand { private final AutoLandmarks autoLandmarks; private final PoseSubsystem pose; - private final SwervePointPathPlanning pathPlanning; + private final RefinedSwervePointPathPlanning pathPlanning; @Inject public SecondDriveForCollectionCommand(DriveSubsystem drive, PoseSubsystem pose, PropertyFactory pf, HeadingModule.HeadingModuleFactory headingModuleFactory, - RobotAssertionManager robotAssertionManager, SwervePointPathPlanning pathPlanning, GameField gamefield, + RobotAssertionManager robotAssertionManager, RefinedSwervePointPathPlanning pathPlanning, GameField gamefield, AutoLandmarks autoLandmarks) { super(drive, pose, pf, headingModuleFactory, robotAssertionManager); this.pose = pose; @@ -33,24 +33,15 @@ public SecondDriveForCollectionCommand(DriveSubsystem drive, PoseSubsystem pose, this.autoLandmarks = autoLandmarks; } - private List calcSwervePoints() { - var currentPose = this.pose.getCurrentPose2d(); - var lastCollectFinish = this.autoLandmarks.getStartCollectionPose(currentPose); - var fullEndCollectionLocation = this.autoLandmarks.getMidBallPitCollectionPose(currentPose); - var startPose = new Pose2d(lastCollectFinish.getTranslation().interpolate(fullEndCollectionLocation.getTranslation(), 0.75), - fullEndCollectionLocation.getRotation()); - var endPose = new Pose2d(lastCollectFinish.getTranslation().interpolate(fullEndCollectionLocation.getTranslation(), 1.2), - fullEndCollectionLocation.getRotation()); - - List points = new ArrayList<>(); - points.addAll(this.pathPlanning.generateSwervePoints(startPose, endPose, false)); - - return points; - } - @Override public void initialize() { - super.logic.setKeyPoints(this.calcSwervePoints()); + var currentPose = this.pose.getCurrentPose2d(); + var lastDriveFinish = this.autoLandmarks.getNearestAllianceToNeutralTrenchPath(currentPose); + var startPose = lastDriveFinish.get(lastDriveFinish.size() - 1); + var pathPoses = this.autoLandmarks.getDriverCloserCollectionPath(currentPose); + + super.logic.setKeyPoints(this.pathPlanning.generateSwervePoints(startPose, pathPoses, + false, true)); super.setSegmentType(SegmentType.Mid); super.setMaxSpeed(MaxSpeed.Intake); diff --git a/src/main/java/competition/subsystems/pose/AutoLandmarks.java b/src/main/java/competition/subsystems/pose/AutoLandmarks.java index b8456a23..08860503 100644 --- a/src/main/java/competition/subsystems/pose/AutoLandmarks.java +++ b/src/main/java/competition/subsystems/pose/AutoLandmarks.java @@ -81,18 +81,50 @@ public Pose2d getStartCollectionPose(Pose2d pose) { return path.get(path.size() - 1); } - public Pose2d getMidBallPitCollectionPose(Pose2d pose) { + public List getMidBallPitCollectionPath(Pose2d pose) { + var results = new ArrayList(); var alliance = this.getAlliance(); var ballPitEdge = Landmarks.getClosestAutoBallPitEdge(this.gamefield, pose, alliance); var multiplierY = ballPitEdge.getY() > this.gamefield.getFieldCenter().getY() ? 1 : -1; var multiplierX = ballPitEdge.getX() > this.gamefield.getFieldCenter().getX() ? 1 : -1; + var rotationToDriver = ballPitEdge.getY() > this.gamefield.getFieldCenter().getY() ? Rotation2d.kCCW_Pi_2 : Rotation2d.kCW_Pi_2; var adjustedForRobot = new Translation2d(Units.Meters.of(0.5).times(multiplierX), this.robotRadius.plus(this.pathPlanning.getAdditionalClearance()).times(multiplierY)); var adjustedTranslation = new Translation2d(this.gamefield.getFieldCenter().getX(), this.gamefield.getFieldCenter().getY()) .plus(adjustedForRobot); - return new Pose2d(adjustedTranslation, ballPitEdge.getRotation()); + results.add(new Pose2d(adjustedTranslation, ballPitEdge.getRotation())); + + var rotationTowardsDriverStation = alliance == Alliance.Blue + ? Rotation2d.kZero + : Rotation2d.kPi; + var nextCollectionLocation = new Pose2d( + adjustedTranslation.getX() + (0.5 * multiplierX), + adjustedTranslation.getY() + (0.75 * multiplierY), + ballPitEdge.getRotation().plus(rotationToDriver)); + + results.add(nextCollectionLocation); + + nextCollectionLocation = new Pose2d( + nextCollectionLocation.getX(), + adjustedTranslation.getY() + (0.75 * multiplierY), + rotationTowardsDriverStation); + + results.add(nextCollectionLocation); + + nextCollectionLocation = new Pose2d( + nextCollectionLocation.getX() + (0.5 * multiplierX), + adjustedTranslation.getY() + (0.75 * multiplierY), + ballPitEdge.getRotation().plus(Rotation2d.kPi)); + + results.add(nextCollectionLocation); + return results; + } + + public Pose2d getMidBallPitCollectionPose(Pose2d pose) { + var path = this.getMidBallPitCollectionPath(pose); + return path.get(path.size() - 1); } public Pose2d getFinishBallPitCollectionPose(Pose2d pose) { @@ -109,6 +141,27 @@ public Pose2d getFinishBallPitCollectionPose(Pose2d pose) { returnPathPose.getRotation()); } + public List getDriverCloserCollectionPath(Pose2d pose) { + var results = new ArrayList(); + var lastDriveFinish = this.getNearestAllianceToNeutralTrenchPath(pose); + var originalCollectionPose = this.getStartCollectionPose(pose); + var startPoseCollection = lastDriveFinish.get(lastDriveFinish.size() - 1); + + var multiplierX = this.gamefield.getFieldCenter().getX() > startPoseCollection.getX() ? 1 : -1; + var multiplierY = this.gamefield.getFieldCenter().getY() > startPoseCollection.getY() ? 1 : -1; + var firstPoseAdjustment = new Translation2d(Units.Meters.of(0.05).times(multiplierX), Units.Meters.of(0.3).times(multiplierY)); + var firstPathPose = new Pose2d(startPoseCollection.getTranslation().plus(firstPoseAdjustment), originalCollectionPose.getRotation()); + + results.add(firstPathPose); + + var adjustedForRobotY = this.robotRadius.plus(this.pathPlanning.getAdditionalClearance()).times(multiplierY * -1).in(Units.Meters); + var midFieldAdjustedRobotTranslation = new Translation2d(firstPathPose.getX(), this.gamefield.getFieldCenter().getY() + adjustedForRobotY); + var endCollectionPose = new Pose2d(midFieldAdjustedRobotTranslation, firstPathPose.getRotation()); + results.add(endCollectionPose); + + return results; + } + public List getAllianceShootingStartingPath(Pose2d pose) { var alliance = this.getAlliance(); var results = new ArrayList();