double yawDegrees = swerve.getYaw().getDegrees();
double snappedDeg;
- if (swerve.getPose().getX() > FieldConstants.FIELD_LENGTH / 2) {
+ if (swerve.getPose().getX() < FieldConstants.FIELD_LENGTH / 2) {
snappedDeg = (swerve.getYaw().getDegrees() > 135 || swerve.getYaw().getDegrees() < 225) ? 180 : 0;
} else {
- snappedDeg = (swerve.getYaw().getDegrees() < 45 || swerve.getYaw().getDegrees() > 315) ? 180 : 0;
+ snappedDeg = (swerve.getYaw().getDegrees() < 45 || swerve.getYaw().getDegrees() > 315) ? 0 : 180;
}
swerve.setAlignAngle(Units.degreesToRadians(snappedDeg));