diff --git a/documentation/README.md b/documentation/README.md index 5b22285..5651cad 100644 --- a/documentation/README.md +++ b/documentation/README.md @@ -5,6 +5,7 @@ Guides for using the library from a robot project, with the example code they re | Guide | What it covers | | ----- | -------------- | | [swerve](swerve/README.md) | Building a drivetrain on `SwerveChassis` and following paths with it, using PathPlanner or BLine | +| [alliance-flip](alliance-flip/README.md) | Writing field positions once, in blue coordinates, and getting the red ones from them | ## About the examples diff --git a/documentation/alliance-flip/README.md b/documentation/alliance-flip/README.md new file mode 100644 index 0000000..35996a1 --- /dev/null +++ b/documentation/alliance-flip/README.md @@ -0,0 +1,92 @@ +# Alliance flip + +Field positions are written once, in blue alliance coordinates, and `AllianceFlip` turns +them into the matching ones for red. It does the same arithmetic as PathPlanner's and +BLine's `FlippingUtil`, with the same field size, so a target flipped here lands exactly +where a path flipped by either of them does. + +It lives in the library so that code which only aims at something does not have to import +a path follower to do it. + +## The three things it does + +| Call | What it does | Use it for | +| ---- | ------------ | ---------- | +| `AllianceFlip.flip(x)` | Always flips to the other alliance | When you already know you are red | +| `AllianceFlip.flipIfRed(x)` | Flips only if the driver station says red | Almost everything | +| `AllianceFlip.mirrorLeftRight(x)` | Moves to the other side of the field, same alliance | Writing a left target in terms of the right one | + +`flip` and `flipIfRed` take a `Translation2d`, a `Rotation2d`, a `Pose2d` or field +relative `ChassisSpeeds`. `mirrorLeftRight` takes the first three. + +Blue alliance coordinates are WPILib's: the origin in the corner to the right of the blue +drivers, x growing towards the red wall, y growing to their left. + +## Example + +Shelby's shooter targets, from `LaunchConstants`: + +```java +import com.overture.lib.utils.AllianceFlip; + +// Blue alliance coordinates +public static final Translation2d HubPose = new Translation2d(4.626, 4.035); +public static final Translation2d RightPass = new Translation2d(4.0, 2.5); +public static final Translation2d LeftPass = AllianceFlip.mirrorLeftRight(RightPass); + +public static final double FieldMidline = AllianceFlip.getFieldWidth() / 2.0; + +public static Translation2d getHubPose() { + return AllianceFlip.flipIfRed(HubPose); +} +``` + +**Call `flipIfRed` where the value is used, not once at startup.** The alliance is not +known until the driver station connects, and until then it answers for blue. That is why +`HubPose` above stays a blue constant and `getHubPose()` is a method: a +`static final Translation2d Hub = AllianceFlip.flipIfRed(...)` is decided once, when the +class loads, which is usually while the robot boots and before it knows it is red. + +Only field relative speeds are flipped. Robot relative speeds mean the same thing on both +alliances. + +## The two kinds of field + +How red relates to blue changes from season to season, and `flip` follows whichever one is +configured: + +| `FieldSymmetry` | The red half is the blue half... | Position | Heading | Field speeds | Seasons | +| --------------- | -------------------------------- | -------- | ------- | ------------ | ------- | +| `ROTATIONAL` | turned 180 degrees about the field center | `(L - x, W - y)` | `θ - 180°` | `(-vx, -vy, ω)` | 2022, 2025, 2026 | +| `MIRRORED` | seen in a mirror across the middle of the field | `(L - x, y)` | `180° - θ` | `(-vx, vy, -ω)` | 2023, 2024 | + +`L` is the field length and `W` its width. `mirrorLeftRight` is the same on both: +`(x, W - y)` and `-θ`. + +The library ships set up for the current season, 2026: `ROTATIONAL`, 16.54 m by 8.07 m. +To run on another field, say so first thing in the `Robot` constructor: + +```java +// The 2024 field +AllianceFlip.configure(AllianceFlip.FieldSymmetry.MIRRORED, 16.541, 8.211); +``` + +It has to be first because anything that already read the field size keeps the number it +read. `LeftPass` and `FieldMidline` in the example above are computed when +`LaunchConstants` is first used, from the field as it is configured at that moment. + +## Coming from `FlippingUtil` + +| PathPlanner or BLine | `AllianceFlip` | +| -------------------- | -------------- | +| `flipFieldPosition(p)`, `flipFieldRotation(r)`, `flipFieldPose(p)`, `flipFieldSpeeds(s)` | `flip(...)` | +| `if (isRedAlliance()) x = flipField...(x)` | `flipIfRed(x)` | +| `fieldSizeX`, `fieldSizeY` | `getFieldLength()`, `getFieldWidth()` | +| `symmetryType = kRotational` or `kMirrored` | `configure(FieldSymmetry.ROTATIONAL` or `MIRRORED, ...)` | +| BLine's `mirrorFieldPosition`, `mirrorFieldRotation`, `mirrorFieldPose` | `mirrorLeftRight(...)` | +| `flipFeedforwards`, `flipFeedforwardXs`, `flipFeedforwardYs` | Not provided: they reorder per module forces inside a path follower | + +The path follower still flips its own paths with its own copy. PathPlanner 2026.1.2 and +BLine v0.9.2 both ship with the same field as this library, so nothing needs to be kept in +step for 2026. If you call `configure` for another field, the follower has its own field +settings that need changing too. diff --git a/src/main/java/com/overture/lib/utils/AllianceFlip.java b/src/main/java/com/overture/lib/utils/AllianceFlip.java new file mode 100644 index 0000000..8e20d0e --- /dev/null +++ b/src/main/java/com/overture/lib/utils/AllianceFlip.java @@ -0,0 +1,234 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package com.overture.lib.utils; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** + * Moves field coordinates from one alliance to the other, and from one side of the field to the + * other. + * + *

Robot code is written once, in blue alliance coordinates: the origin in the corner to the + * right of the blue drivers, x growing towards the red wall and y growing to their left. {@code + * flip} takes a position, heading, pose or field relative speed written that way and returns the + * one that means the same thing for the red alliance, still measured from the blue origin. + * + *

This is the arithmetic PathPlanner's and BLine's FlippingUtil do, with the same field size, so + * a target flipped here lands exactly where a path flipped by either of them does. It lives in the + * library so that code which only aims at something does not depend on which path follower the + * robot happens to use. + */ +public final class AllianceFlip { + /** How the red half of the field is laid out relative to the blue half. */ + public enum FieldSymmetry { + /** + * The red half is the blue half turned 180 degrees about the middle of the field, so what is on + * the blue drivers' left is on the red drivers' left too. 2022, 2025 and 2026 are like this. + */ + ROTATIONAL, + + /** + * The red half is the blue half seen in a mirror standing across the middle of the field, so + * what is on the blue drivers' left is on the red drivers' right. 2023 and 2024 are like this. + */ + MIRRORED + } + + // The 2026 field. The size is the one PathPlanner and BLine use, on purpose: WPILib's AprilTag + // layout says 16.541 by 8.069, and a millimetre is not worth flipping a target to a slightly + // different place than the path that drives to it. + private static FieldSymmetry symmetry = FieldSymmetry.ROTATIONAL; + private static double fieldLength = 16.54; + private static double fieldWidth = 8.07; + + private AllianceFlip() {} + + /** + * Describes a field other than this season's. + * + *

Only needed to run on an older field, at an off season event for example. Call it first + * thing in the Robot constructor: anything that has already read the field size, a constant + * computed from {@link #getFieldWidth()} say, keeps the number it read. + * + * @param symmetry how the red half relates to the blue half + * @param fieldLengthMeters the size of the field along x, from one alliance wall to the other + * @param fieldWidthMeters the size of the field along y + */ + public static void configure( + FieldSymmetry symmetry, double fieldLengthMeters, double fieldWidthMeters) { + AllianceFlip.symmetry = symmetry; + fieldLength = fieldLengthMeters; + fieldWidth = fieldWidthMeters; + } + + /** + * Returns how the red half of the field relates to the blue half. + * + * @return the field symmetry + */ + public static FieldSymmetry getSymmetry() { + return symmetry; + } + + /** + * Returns the size of the field along x, from one alliance wall to the other. + * + * @return the field length, in meters + */ + public static double getFieldLength() { + return fieldLength; + } + + /** + * Returns the size of the field along y. + * + * @return the field width, in meters + */ + public static double getFieldWidth() { + return fieldWidth; + } + + /** + * Flips a position to the other alliance. + * + * @param position the position, in blue alliance coordinates + * @return the matching position on the other alliance's side + */ + public static Translation2d flip(Translation2d position) { + return switch (symmetry) { + case ROTATIONAL -> new Translation2d( + fieldLength - position.getX(), fieldWidth - position.getY()); + case MIRRORED -> new Translation2d(fieldLength - position.getX(), position.getY()); + }; + } + + /** + * Flips a heading to the other alliance. + * + * @param rotation the heading, in blue alliance coordinates + * @return the matching heading for the other alliance + */ + public static Rotation2d flip(Rotation2d rotation) { + return switch (symmetry) { + case ROTATIONAL -> rotation.minus(Rotation2d.kPi); + case MIRRORED -> Rotation2d.kPi.minus(rotation); + }; + } + + /** + * Flips a pose to the other alliance. + * + * @param pose the pose, in blue alliance coordinates + * @return the matching pose on the other alliance's side + */ + public static Pose2d flip(Pose2d pose) { + return new Pose2d(flip(pose.getTranslation()), flip(pose.getRotation())); + } + + /** + * Flips field relative speeds to the other alliance. + * + *

Field relative only. Robot relative speeds mean the same thing on both alliances and must + * not be flipped. + * + * @param fieldSpeeds the field relative speeds, in blue alliance coordinates + * @return the matching speeds for the other alliance + */ + public static ChassisSpeeds flip(ChassisSpeeds fieldSpeeds) { + return switch (symmetry) { + case ROTATIONAL -> new ChassisSpeeds( + -fieldSpeeds.vxMetersPerSecond, + -fieldSpeeds.vyMetersPerSecond, + fieldSpeeds.omegaRadiansPerSecond); + case MIRRORED -> new ChassisSpeeds( + -fieldSpeeds.vxMetersPerSecond, + fieldSpeeds.vyMetersPerSecond, + -fieldSpeeds.omegaRadiansPerSecond); + }; + } + + /** + * Returns a blue alliance position as the alliance we are on needs it: flipped if we are red, + * untouched if we are blue. + * + *

Call this where the value is used, not once while the robot is starting. The alliance is not + * known until the driver station connects, and until then this answers for blue. + * + * @param bluePosition the position, in blue alliance coordinates + * @return the position for our alliance + */ + public static Translation2d flipIfRed(Translation2d bluePosition) { + return UtilityFunctions.isRedAlliance() ? flip(bluePosition) : bluePosition; + } + + /** + * Returns a blue alliance heading as the alliance we are on needs it. See {@link + * #flipIfRed(Translation2d)} for when to call it. + * + * @param blueRotation the heading, in blue alliance coordinates + * @return the heading for our alliance + */ + public static Rotation2d flipIfRed(Rotation2d blueRotation) { + return UtilityFunctions.isRedAlliance() ? flip(blueRotation) : blueRotation; + } + + /** + * Returns a blue alliance pose as the alliance we are on needs it. See {@link + * #flipIfRed(Translation2d)} for when to call it. + * + * @param bluePose the pose, in blue alliance coordinates + * @return the pose for our alliance + */ + public static Pose2d flipIfRed(Pose2d bluePose) { + return UtilityFunctions.isRedAlliance() ? flip(bluePose) : bluePose; + } + + /** + * Returns blue alliance field relative speeds as the alliance we are on needs them. See {@link + * #flipIfRed(Translation2d)} for when to call it. + * + * @param blueFieldSpeeds the field relative speeds, in blue alliance coordinates + * @return the speeds for our alliance + */ + public static ChassisSpeeds flipIfRed(ChassisSpeeds blueFieldSpeeds) { + return UtilityFunctions.isRedAlliance() ? flip(blueFieldSpeeds) : blueFieldSpeeds; + } + + /** + * Moves a position to the other side of the field, left to right, staying on the same alliance. + * + *

Not an alliance flip, and not related to {@link FieldSymmetry#MIRRORED}. It is for writing + * one of two matching things, a left and a right pass target say, in terms of the other. + * + * @param position the position + * @return the position the same distance from the opposite side wall + */ + public static Translation2d mirrorLeftRight(Translation2d position) { + return new Translation2d(position.getX(), fieldWidth - position.getY()); + } + + /** + * Turns a heading into the one that matches it on the other side of the field, left to right. + * + * @param rotation the heading + * @return the mirrored heading + */ + public static Rotation2d mirrorLeftRight(Rotation2d rotation) { + return rotation.unaryMinus(); + } + + /** + * Moves a pose to the other side of the field, left to right, staying on the same alliance. + * + * @param pose the pose + * @return the mirrored pose + */ + public static Pose2d mirrorLeftRight(Pose2d pose) { + return new Pose2d(mirrorLeftRight(pose.getTranslation()), mirrorLeftRight(pose.getRotation())); + } +} diff --git a/src/test/java/com/overture/lib/utils/AllianceFlipTest.java b/src/test/java/com/overture/lib/utils/AllianceFlipTest.java new file mode 100644 index 0000000..e24bf4c --- /dev/null +++ b/src/test/java/com/overture/lib/utils/AllianceFlipTest.java @@ -0,0 +1,175 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package com.overture.lib.utils; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertSame; + +import com.overture.lib.utils.AllianceFlip.FieldSymmetry; +import edu.wpi.first.hal.AllianceStationID; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; + +/** + * Checks both field symmetries, the left to right mirror and the flip that follows the alliance. + */ +class AllianceFlipTest { + private static final double kEpsilon = 1e-9; + + // Read before any test reconfigures the field, so they are what the library ships with. + private static final FieldSymmetry kShippedSymmetry = AllianceFlip.getSymmetry(); + private static final double kShippedLength = AllianceFlip.getFieldLength(); + private static final double kShippedWidth = AllianceFlip.getFieldWidth(); + + @BeforeAll + static void setUp() { + HAL.initialize(500, 0); + } + + // The field and the alliance are both global, so every test hands them back as it found them. + @AfterEach + void restore() { + AllianceFlip.configure(kShippedSymmetry, kShippedLength, kShippedWidth); + setAlliance(AllianceStationID.Unknown); + } + + private static void setAlliance(AllianceStationID station) { + DriverStationSim.setAllianceStationId(station); + DriverStationSim.notifyNewData(); + } + + private static void assertTranslation(double x, double y, Translation2d actual) { + assertEquals(x, actual.getX(), kEpsilon, "x"); + assertEquals(y, actual.getY(), kEpsilon, "y"); + } + + private static void assertDegrees(double degrees, Rotation2d actual) { + // Compared as an angle, so 180 and -180 count as the same heading. + assertEquals(0.0, actual.minus(Rotation2d.fromDegrees(degrees)).getDegrees(), 1e-6, "heading"); + } + + private static void assertSpeeds(double vx, double vy, double omega, ChassisSpeeds actual) { + assertEquals(vx, actual.vxMetersPerSecond, kEpsilon, "vx"); + assertEquals(vy, actual.vyMetersPerSecond, kEpsilon, "vy"); + assertEquals(omega, actual.omegaRadiansPerSecond, kEpsilon, "omega"); + } + + @Test + void shipsWithThe2026Field() { + assertSame(FieldSymmetry.ROTATIONAL, kShippedSymmetry); + assertEquals(16.54, kShippedLength, kEpsilon); + assertEquals(8.07, kShippedWidth, kEpsilon); + } + + @Test + void rotationalFlipTurnsEverythingAboutTheFieldCenter() { + AllianceFlip.configure(FieldSymmetry.ROTATIONAL, 16.54, 8.07); + + assertTranslation(12.54, 5.57, AllianceFlip.flip(new Translation2d(4.0, 2.5))); + assertDegrees(-150.0, AllianceFlip.flip(Rotation2d.fromDegrees(30.0))); + assertSpeeds(-1.0, -2.0, 3.0, AllianceFlip.flip(new ChassisSpeeds(1.0, 2.0, 3.0))); + + Pose2d flipped = AllianceFlip.flip(new Pose2d(4.0, 2.5, Rotation2d.fromDegrees(30.0))); + assertTranslation(12.54, 5.57, flipped.getTranslation()); + assertDegrees(-150.0, flipped.getRotation()); + + // The one point a half turn leaves where it was. + assertTranslation(8.27, 4.035, AllianceFlip.flip(new Translation2d(8.27, 4.035))); + } + + @Test + void mirroredFlipKeepsTheSideOfTheField() { + AllianceFlip.configure(FieldSymmetry.MIRRORED, 16.54, 8.07); + + // y survives: a mirror across the middle swaps the alliances and nothing else. + assertTranslation(12.54, 2.5, AllianceFlip.flip(new Translation2d(4.0, 2.5))); + assertDegrees(150.0, AllianceFlip.flip(Rotation2d.fromDegrees(30.0))); + assertSpeeds(-1.0, 2.0, -3.0, AllianceFlip.flip(new ChassisSpeeds(1.0, 2.0, 3.0))); + + Pose2d flipped = AllianceFlip.flip(new Pose2d(4.0, 2.5, Rotation2d.fromDegrees(30.0))); + assertTranslation(12.54, 2.5, flipped.getTranslation()); + assertDegrees(150.0, flipped.getRotation()); + } + + @Test + void flippingTwiceComesBackUnderBothSymmetries() { + Pose2d pose = new Pose2d(3.2, 6.9, Rotation2d.fromDegrees(-73.0)); + ChassisSpeeds speeds = new ChassisSpeeds(1.5, -0.4, 2.2); + + for (FieldSymmetry symmetry : FieldSymmetry.values()) { + AllianceFlip.configure(symmetry, 16.54, 8.07); + + Pose2d back = AllianceFlip.flip(AllianceFlip.flip(pose)); + assertTranslation(3.2, 6.9, back.getTranslation()); + assertDegrees(-73.0, back.getRotation()); + assertSpeeds(1.5, -0.4, 2.2, AllianceFlip.flip(AllianceFlip.flip(speeds))); + } + } + + @Test + void usesTheConfiguredFieldSize() { + // The 2024 field: mirrored, and a different size. + AllianceFlip.configure(FieldSymmetry.MIRRORED, 16.541, 8.211); + + assertEquals(16.541, AllianceFlip.getFieldLength(), kEpsilon); + assertEquals(8.211, AllianceFlip.getFieldWidth(), kEpsilon); + assertTranslation(15.541, 1.0, AllianceFlip.flip(new Translation2d(1.0, 1.0))); + assertTranslation(1.0, 7.211, AllianceFlip.mirrorLeftRight(new Translation2d(1.0, 1.0))); + } + + @Test + void mirrorLeftRightSwapsSidesAndIgnoresTheSymmetry() { + for (FieldSymmetry symmetry : FieldSymmetry.values()) { + AllianceFlip.configure(symmetry, 16.54, 8.07); + + // x survives: the robot stays on its own alliance's half. + assertTranslation(4.0, 5.57, AllianceFlip.mirrorLeftRight(new Translation2d(4.0, 2.5))); + assertDegrees(-30.0, AllianceFlip.mirrorLeftRight(Rotation2d.fromDegrees(30.0))); + + Pose2d mirrored = + AllianceFlip.mirrorLeftRight(new Pose2d(4.0, 2.5, Rotation2d.fromDegrees(30.0))); + assertTranslation(4.0, 5.57, mirrored.getTranslation()); + assertDegrees(-30.0, mirrored.getRotation()); + } + } + + @Test + void flipIfRedLeavesBlueAndUnknownAlone() { + Translation2d position = new Translation2d(4.0, 2.5); + Rotation2d rotation = Rotation2d.fromDegrees(30.0); + Pose2d pose = new Pose2d(position, rotation); + ChassisSpeeds speeds = new ChassisSpeeds(1.0, 2.0, 3.0); + + for (AllianceStationID station : + new AllianceStationID[] {AllianceStationID.Blue2, AllianceStationID.Unknown}) { + setAlliance(station); + + assertSame(position, AllianceFlip.flipIfRed(position)); + assertSame(rotation, AllianceFlip.flipIfRed(rotation)); + assertSame(pose, AllianceFlip.flipIfRed(pose)); + assertSame(speeds, AllianceFlip.flipIfRed(speeds)); + } + } + + @Test + void flipIfRedFlipsOnRed() { + setAlliance(AllianceStationID.Red2); + + assertTranslation(12.54, 5.57, AllianceFlip.flipIfRed(new Translation2d(4.0, 2.5))); + assertDegrees(-150.0, AllianceFlip.flipIfRed(Rotation2d.fromDegrees(30.0))); + assertSpeeds(-1.0, -2.0, 3.0, AllianceFlip.flipIfRed(new ChassisSpeeds(1.0, 2.0, 3.0))); + + Pose2d flipped = AllianceFlip.flipIfRed(new Pose2d(4.0, 2.5, Rotation2d.fromDegrees(30.0))); + assertTranslation(12.54, 5.57, flipped.getTranslation()); + assertDegrees(-150.0, flipped.getRotation()); + } +}