From 23cf2332cff8bafd336a50b1869191418c17a46d Mon Sep 17 00:00:00 2001 From: daniel Date: Tue, 3 Mar 2026 00:19:16 +0200 Subject: [PATCH 1/7] plssssss --- .../java/frc/robot/odometry/OdometryPlus.java | 57 +++++++++++++++++++ 1 file changed, 57 insertions(+) create mode 100644 src/main/java/frc/robot/odometry/OdometryPlus.java diff --git a/src/main/java/frc/robot/odometry/OdometryPlus.java b/src/main/java/frc/robot/odometry/OdometryPlus.java new file mode 100644 index 0000000000..2e5fedcae6 --- /dev/null +++ b/src/main/java/frc/robot/odometry/OdometryPlus.java @@ -0,0 +1,57 @@ +package frc.robot.odometry; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.kinematics.Kinematics; +import edu.wpi.first.math.kinematics.Odometry; + +import java.util.function.Supplier; + +public class OdometryPlus extends Odometry { + + private final Supplier robotAnglesSupplier; + + /** + * Constructs an Odometry object. + * + * @param kinematics The kinematics of the drivebase. + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current encoder readings. + * @param initialPoseMeters The starting position of the robot on the field. + */ + public OdometryPlus(Kinematics kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPoseMeters) { + super(kinematics, gyroAngle, wheelPositions, initialPoseMeters); + robotAnglesSupplier = null; + } + + public OdometryPlus( + Kinematics kinematics, + Rotation2d gyroAngle, + T wheelPositions, + Pose2d initialPoseMeters, + Supplier robotAnglesSupplier + ) { + super(kinematics, gyroAngle, wheelPositions, initialPoseMeters); + this.robotAnglesSupplier = robotAnglesSupplier; + } + + @Override + public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { + Pose2d lastPose = getPoseMeters(); + Pose2d rawPose = super.update(gyroAngle, wheelPositions); + + if (robotAnglesSupplier == null) + return rawPose; + + Rotation3d angles = robotAnglesSupplier.get(); + double scale = Math.max(0, Math.cos(angles.rotateBy(new Rotation3d(0, 0, -angles.getZ())).getY())); + + Pose2d corrected = new Pose2d(lastPose.interpolate(rawPose, scale).getTranslation(), rawPose.getRotation()); + + resetTranslation(corrected.getTranslation()); + + return corrected; + } + +} From 2f95ef3b81120dfdd81d777648c246b47f23ad28 Mon Sep 17 00:00:00 2001 From: daniel Date: Tue, 3 Mar 2026 00:33:47 +0200 Subject: [PATCH 2/7] changes --- src/main/java/frc/robot/odometry/OdometryPlus.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/odometry/OdometryPlus.java b/src/main/java/frc/robot/odometry/OdometryPlus.java index 2e5fedcae6..50e6a34bc1 100644 --- a/src/main/java/frc/robot/odometry/OdometryPlus.java +++ b/src/main/java/frc/robot/odometry/OdometryPlus.java @@ -45,7 +45,8 @@ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { return rawPose; Rotation3d angles = robotAnglesSupplier.get(); - double scale = Math.max(0, Math.cos(angles.rotateBy(new Rotation3d(0, 0, -angles.getZ())).getY())); + // did it for case robot is above 90 , i think the pose wont matter when the robo flip -_- + double scale = Math.max(0, Math.cos(angles.rotateBy(new Rotation3d(0, 0, -angles.getZ())).getY())); Pose2d corrected = new Pose2d(lastPose.interpolate(rawPose, scale).getTranslation(), rawPose.getRotation()); From 0a1ea6b73a30c2efee165288e2fb8436d87788e6 Mon Sep 17 00:00:00 2001 From: daniel Date: Tue, 3 Mar 2026 01:33:07 +0200 Subject: [PATCH 3/7] plssss i was dumb af --- src/main/java/frc/robot/odometry/OdometryPlus.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/odometry/OdometryPlus.java b/src/main/java/frc/robot/odometry/OdometryPlus.java index 50e6a34bc1..ebf0935340 100644 --- a/src/main/java/frc/robot/odometry/OdometryPlus.java +++ b/src/main/java/frc/robot/odometry/OdometryPlus.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.kinematics.Kinematics; import edu.wpi.first.math.kinematics.Odometry; @@ -45,10 +46,12 @@ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { return rawPose; Rotation3d angles = robotAnglesSupplier.get(); - // did it for case robot is above 90 , i think the pose wont matter when the robo flip -_- - double scale = Math.max(0, Math.cos(angles.rotateBy(new Rotation3d(0, 0, -angles.getZ())).getY())); + // in case angle above 90 so wont minus , i dont think pose is relevant when robot flips + double scale = Math.max(0, Math.cos(angles.rotateBy(new Rotation3d(0, 0, -angles.getZ())).getY())); - Pose2d corrected = new Pose2d(lastPose.interpolate(rawPose, scale).getTranslation(), rawPose.getRotation()); + Transform2d delta = new Transform2d(lastPose, rawPose); + + Pose2d corrected = lastPose.plus(new Transform2d(delta.getTranslation().times(scale), delta.getRotation())); resetTranslation(corrected.getTranslation()); From 7b59b902617e7296d803cdcec8bb123a9473d3bb Mon Sep 17 00:00:00 2001 From: daniel Date: Tue, 3 Mar 2026 16:42:24 +0200 Subject: [PATCH 4/7] changes --- .../java/frc/robot/odometry/OdometryPlus.java | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/odometry/OdometryPlus.java b/src/main/java/frc/robot/odometry/OdometryPlus.java index ebf0935340..d685d8f2cc 100644 --- a/src/main/java/frc/robot/odometry/OdometryPlus.java +++ b/src/main/java/frc/robot/odometry/OdometryPlus.java @@ -11,10 +11,10 @@ public class OdometryPlus extends Odometry { - private final Supplier robotAnglesSupplier; + private final Supplier robotOrientationSupplier; /** - * Constructs an Odometry object. + * Constructs a 2d odometry object that takes into consideration the robot's 3d orientation. * * @param kinematics The kinematics of the drivebase. * @param gyroAngle The angle reported by the gyroscope. @@ -23,7 +23,7 @@ public class OdometryPlus extends Odometry { */ public OdometryPlus(Kinematics kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPoseMeters) { super(kinematics, gyroAngle, wheelPositions, initialPoseMeters); - robotAnglesSupplier = null; + robotOrientationSupplier = null; } public OdometryPlus( @@ -31,31 +31,32 @@ public OdometryPlus( Rotation2d gyroAngle, T wheelPositions, Pose2d initialPoseMeters, - Supplier robotAnglesSupplier + Supplier robotOrientationSupplier ) { super(kinematics, gyroAngle, wheelPositions, initialPoseMeters); - this.robotAnglesSupplier = robotAnglesSupplier; + this.robotOrientationSupplier = robotOrientationSupplier; } @Override public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { Pose2d lastPose = getPoseMeters(); - Pose2d rawPose = super.update(gyroAngle, wheelPositions); + Pose2d unCompensatedPose = super.update(gyroAngle, wheelPositions); - if (robotAnglesSupplier == null) - return rawPose; + if (robotOrientationSupplier == null) + return unCompensatedPose; - Rotation3d angles = robotAnglesSupplier.get(); + Rotation3d robotOrientation = robotOrientationSupplier.get(); // in case angle above 90 so wont minus , i dont think pose is relevant when robot flips - double scale = Math.max(0, Math.cos(angles.rotateBy(new Rotation3d(0, 0, -angles.getZ())).getY())); + double scale = Math.max(0, Math.cos(robotOrientation.rotateBy(new Rotation3d(0, 0, -robotOrientation.getZ())).getY())); - Transform2d delta = new Transform2d(lastPose, rawPose); + Transform2d unCompensatedDistance = new Transform2d(lastPose, unCompensatedPose); - Pose2d corrected = lastPose.plus(new Transform2d(delta.getTranslation().times(scale), delta.getRotation())); + Pose2d compensatedByPitch = lastPose + .plus(new Transform2d(unCompensatedDistance.getTranslation().times(scale), unCompensatedDistance.getRotation())); - resetTranslation(corrected.getTranslation()); + resetTranslation(compensatedByPitch.getTranslation()); - return corrected; + return compensatedByPitch; } } From b4ea190aae17e779377e254f1f45804bacd36622 Mon Sep 17 00:00:00 2001 From: daniel Date: Wed, 4 Mar 2026 00:30:41 +0200 Subject: [PATCH 5/7] not finished --- src/main/java/frc/robot/odometry/OdometryPlus.java | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/odometry/OdometryPlus.java b/src/main/java/frc/robot/odometry/OdometryPlus.java index d685d8f2cc..7d5a16b9a3 100644 --- a/src/main/java/frc/robot/odometry/OdometryPlus.java +++ b/src/main/java/frc/robot/odometry/OdometryPlus.java @@ -16,15 +16,14 @@ public class OdometryPlus extends Odometry { /** * Constructs a 2d odometry object that takes into consideration the robot's 3d orientation. * - * @param kinematics The kinematics of the drivebase. - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The current encoder readings. + * @param kinematics The kinematics of the chassis. + * @param gyroAngle The given robot's yaw. + * @param wheelPositions The current wheel positions. * @param initialPoseMeters The starting position of the robot on the field. */ public OdometryPlus(Kinematics kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPoseMeters) { - super(kinematics, gyroAngle, wheelPositions, initialPoseMeters); - robotOrientationSupplier = null; - } + this(kinematics, gyroAngle, wheelPositions, initialPoseMeters, null); + } public OdometryPlus( Kinematics kinematics, From 4c71e123bab64695249b638cbbdbe3db4ac1eebb Mon Sep 17 00:00:00 2001 From: daniel Date: Thu, 26 Mar 2026 12:00:30 +0200 Subject: [PATCH 6/7] commit --- .../java/frc/robot/odometry/OdometryPlus.java | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/odometry/OdometryPlus.java b/src/main/java/frc/robot/odometry/OdometryPlus.java index 7d5a16b9a3..5b25944e51 100644 --- a/src/main/java/frc/robot/odometry/OdometryPlus.java +++ b/src/main/java/frc/robot/odometry/OdometryPlus.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.kinematics.Kinematics; import edu.wpi.first.math.kinematics.Odometry; - import java.util.function.Supplier; public class OdometryPlus extends Odometry { @@ -22,8 +21,9 @@ public class OdometryPlus extends Odometry { * @param initialPoseMeters The starting position of the robot on the field. */ public OdometryPlus(Kinematics kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPoseMeters) { - this(kinematics, gyroAngle, wheelPositions, initialPoseMeters, null); - } + super(kinematics, gyroAngle, wheelPositions, initialPoseMeters); + this.robotOrientationSupplier = () -> new Rotation3d(0, 0, this.getHeading().getRadians()); + } public OdometryPlus( Kinematics kinematics, @@ -43,11 +43,8 @@ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { if (robotOrientationSupplier == null) return unCompensatedPose; - Rotation3d robotOrientation = robotOrientationSupplier.get(); - // in case angle above 90 so wont minus , i dont think pose is relevant when robot flips - double scale = Math.max(0, Math.cos(robotOrientation.rotateBy(new Rotation3d(0, 0, -robotOrientation.getZ())).getY())); - + double scale = Math.cos(robotOrientation.rotateBy(new Rotation3d(0, 0, -robotOrientation.getZ())).getY()); Transform2d unCompensatedDistance = new Transform2d(lastPose, unCompensatedPose); Pose2d compensatedByPitch = lastPose @@ -58,4 +55,8 @@ public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { return compensatedByPitch; } + public Rotation2d getHeading() { + return this.getPoseMeters().getRotation(); + } + } From 11bab970aa1cb5a47a0dbb1e1dc4a2eb9f6b0c5c Mon Sep 17 00:00:00 2001 From: daniel Date: Wed, 8 Apr 2026 11:53:03 +0300 Subject: [PATCH 7/7] random save --- build.gradle | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/build.gradle b/build.gradle index 1da22ecd7b..eece959407 100644 --- a/build.gradle +++ b/build.gradle @@ -107,14 +107,9 @@ dependencies { repositories { - maven { - url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") - credentials { - username = "Mechanical-Advantage-Bot" - password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" - } - } - mavenLocal() + maven { + url = uri("https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release") + } } test {