Skip to content

Commit 70e8287

Browse files
committed
unfinished opponent reef close enough distance
1 parent d5cc8a7 commit 70e8287

File tree

1 file changed

+4
-1
lines changed

1 file changed

+4
-1
lines changed

src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -220,6 +220,8 @@ public boolean isAligned() {
220220
}
221221

222222
public void setup(Alliance alliance, ScoringLevel level, boolean scoreLeft, boolean algae) {
223+
boolean blueSide = driveSubsystem.getPoseMeters().getX() < (DriveConstants.kFieldMaxX / 2);
224+
223225
this.alliance = alliance;
224226
this.scoreLeft = scoreLeft;
225227
this.finalDrive = false;
@@ -239,8 +241,9 @@ public void setup(Alliance alliance, ScoringLevel level, boolean scoreLeft, bool
239241
this.targetPose = getTargetDrivePose(scoreLeft);
240242

241243
this.fieldRelHexant = computeFieldRelHexant();
244+
242245
this.coralOffset =
243-
driveSubsystem.getPoseMeters().getX() < (DriveConstants.kFieldMaxX / 2)
246+
blueSide
244247
? TagServoingConstants.kBlueCoralOffset[fieldRelHexant][level.ordinal()][
245248
scoreLeft ? 0 : 1]
246249
: TagServoingConstants.kRedCoralOffset[(fieldRelHexant + 3) % 6][level.ordinal()][

0 commit comments

Comments
 (0)