diff --git a/src/main/java/competition/command_groups/DriveToNeutralZoneForIntakeCommand.java b/src/main/java/competition/command_groups/DriveToNeutralZoneForIntakeCommand.java index 8e4e194c..2d7a7106 100644 --- a/src/main/java/competition/command_groups/DriveToNeutralZoneForIntakeCommand.java +++ b/src/main/java/competition/command_groups/DriveToNeutralZoneForIntakeCommand.java @@ -38,7 +38,7 @@ public void initialize() { var pathPoses = this.autoLandmarks.getStartCollectionPath(currentPose); super.setSegmentType(SegmentType.Start); super.logic.setKeyPoints(this.pathPlanning.generateSwervePoints(currentPose, pathPoses, - false, true)); + false, false)); super.initialize(); } diff --git a/src/main/java/competition/subsystems/pose/AutoLandmarks.java b/src/main/java/competition/subsystems/pose/AutoLandmarks.java index 8c2adfcf..1ef09669 100644 --- a/src/main/java/competition/subsystems/pose/AutoLandmarks.java +++ b/src/main/java/competition/subsystems/pose/AutoLandmarks.java @@ -152,6 +152,7 @@ public List getNearestAllianceToNeutralTrenchPath(Pose2d startingPose) { var changeInX = alliance == Alliance.Blue ? -1 : 1; var driverSideTranslation = nearestAllianceTrenchPose.getTranslation() .plus(new Translation2d(this.trenchPlanningOffsetMeters.get() * changeInX, 0)); + var midpointTranslation = nearestAllianceTrenchPose.getTranslation(); var neutralSideTranslation = nearestAllianceTrenchPose.getTranslation() .plus(new Translation2d(this.trenchPlanningOffsetMeters.get() * -1 * changeInX, 0)); var rotationThroughTrench = alliance == Alliance.Blue @@ -159,6 +160,7 @@ public List getNearestAllianceToNeutralTrenchPath(Pose2d startingPose) { : Rotation2d.kPi; results.add(new Pose2d(driverSideTranslation, rotationThroughTrench)); + results.add(new Pose2d(midpointTranslation, rotationThroughTrench)); results.add(new Pose2d(neutralSideTranslation, rotationThroughTrench)); return results;