From ac0bd5c3dc7f794c6456dc8bec101e9ca4c5f894 Mon Sep 17 00:00:00 2001 From: KangarooKoala <91924258+KangarooKoala@users.noreply.github.com> Date: Fri, 25 Mar 2022 19:55:18 -0700 Subject: [PATCH 1/9] Switch from 2910 to WPILib geometry classes Changed from inches to meters Extracted turret relative velocity to separate method Reformatted pose doc comments Added method to convert from Vector2 to Translation2d to GeoConvertor --- .../robot/subsystem/DrivebaseSubsystem.java | 5 +- .../robot/subsystem/TargetLocalizer.java | 152 ++++++++---------- .../frc/team2412/robot/util/GeoConvertor.java | 4 + 3 files changed, 76 insertions(+), 85 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java index f29150fe..ed805a5a 100644 --- a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java @@ -5,6 +5,7 @@ import com.google.errorprone.annotations.concurrent.GuardedBy; import com.swervedrivespecialties.swervelib.SwerveModule; 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.math.kinematics.SwerveDriveKinematics; @@ -232,9 +233,9 @@ public Vector2 getGyroscopeXY() { return Vector2.ZERO; } - public Rotation2 getGyroscopeUnadjustedAngle() { + public Rotation2d getGyroscopeUnadjustedAngle() { synchronized (sensorLock) { - return gyroscope.getUnadjustedAngle(); + return Rotation2d.fromDegrees(gyroscope.getUnadjustedAngle().toDegrees()); } } diff --git a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java index 16587b57..4ea6f21a 100644 --- a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java +++ b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java @@ -1,15 +1,16 @@ package frc.team2412.robot.subsystem; -import org.frcteam2910.common.math.RigidTransform2; -import org.frcteam2910.common.math.Rotation2; -import org.frcteam2910.common.math.Vector2; - import frc.team2412.robot.Robot; - +import frc.team2412.robot.util.GeoConvertor; import frc.team2412.robot.util.TimeBasedMedianFilter; import static frc.team2412.robot.subsystem.TargetLocalizer.LocalizerConstants.*; +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.util.Units; + public class TargetLocalizer { public static class LocalizerConstants { // TODO tune these more @@ -18,19 +19,20 @@ public static class LocalizerConstants { public static final double FILTER_TIME = 1; // Angles are in degrees public static final double STARTING_TURRET_ANGLE = 0; - // Dimensions are in inches - public static final double VISION_DEFAULT_DISTANCE = 118; + // Dimensions are in meters + public static final double VISION_DEFAULT_DISTANCE = Units.inchesToMeters(118); // Estimated, negative because limelight is in back of turret - public static final double LIMELIGHT_TO_TURRET_CENTER_DISTANCE = -7; - public static final Vector2 ROBOT_CENTRIC_TURRET_CENTER = new Vector2(3.93, -4); + public static final double LIMELIGHT_TO_TURRET_CENTER_DISTANCE = Units.inchesToMeters(-7); + public static final Translation2d ROBOT_CENTRIC_TURRET_CENTER = new Translation2d(Units.inchesToMeters(3.93), + Units.inchesToMeters(-4)); } private final DrivebaseSubsystem drivebaseSubsystem; private final ShooterSubsystem shooterSubsystem; private final ShooterVisionSubsystem shooterVisionSubsystem; private final TimeBasedMedianFilter distanceFilter; - private final Rotation2 gyroAdjustmentAngle; - private final RigidTransform2 startingPose; + private final Rotation2d gyroAdjustmentAngle; + private final Pose2d startingPose; /** * Creates a new {@link TargetLocalizer}. @@ -51,9 +53,8 @@ public TargetLocalizer(DrivebaseSubsystem drivebaseSubsystem, ShooterSubsystem s this.distanceFilter = new TimeBasedMedianFilter(FILTER_TIME); // TODO Handle different starting positions // Also don't forget to convert reference to hub-centric if necessary - this.startingPose = new RigidTransform2(new Vector2(5 * 12, 5 * 12), Rotation2.ZERO); - this.gyroAdjustmentAngle = startingPose.rotation - .rotateBy(drivebaseSubsystem.getGyroscopeUnadjustedAngle().inverse()); + this.startingPose = new Pose2d(Units.feetToMeters(5), Units.feetToMeters(5), new Rotation2d()); + this.gyroAdjustmentAngle = startingPose.getRotation().minus(drivebaseSubsystem.getGyroscopeUnadjustedAngle()); } public double getDistance() { @@ -114,7 +115,7 @@ public double getTargetYaw() { * positive rotation is * clockwise). */ - public Rotation2 getFieldCentricRobotAngle() { + public Rotation2d getFieldCentricRobotAngle() { return drivebaseSubsystem.getGyroscopeUnadjustedAngle().rotateBy(gyroAdjustmentAngle); } @@ -123,8 +124,15 @@ public Rotation2 getFieldCentricRobotAngle() { * * @return The turret angle (0 is intake side, positive is clockwise). */ - public Rotation2 getTurretAngle() { - return Rotation2.fromDegrees(shooterSubsystem.getTurretAngle() + STARTING_TURRET_ANGLE); + public Rotation2d getTurretAngle() { + return Rotation2d.fromDegrees(shooterSubsystem.getTurretAngle() + STARTING_TURRET_ANGLE); + } + + public Translation2d getTurretRelativeVelocity() { + return (drivebaseSubsystem != null) + // might need to do inverse + ? GeoConvertor.vector2ToTranslation2d(drivebaseSubsystem.getVelocity()).rotateBy(getTurretAngle()) + : new Translation2d(); } /** @@ -133,10 +141,7 @@ public Rotation2 getTurretAngle() { * @return that */ public double getLateralVelocity() { - return (drivebaseSubsystem != null) - // might need to do inverse - ? drivebaseSubsystem.getVelocity().rotateBy(getTurretAngle()).x - : 0; + return getTurretRelativeVelocity().getX(); } /** @@ -145,10 +150,7 @@ public double getLateralVelocity() { * @return that */ public double getDepthVelocity() { - return (drivebaseSubsystem != null) - // might need to do inverse - ? drivebaseSubsystem.getVelocity().rotateBy(getTurretAngle()).y - : 0; + return getTurretRelativeVelocity().getX(); } public double getAngularVelocity() { @@ -175,88 +177,72 @@ public double yawAdjustment() { /** * Returns the estimated limelight pose according to vision and the gyroscope. * - * The translation (inches) is relative to the hub, and the rotation is relative - * to straight forward - * from the driver station (Positive rotation is clockwise). If the limelight - * doesn't have a target, - * returns {@link RigidTransform2#ZERO}. + * The translation (inches) is relative to the hub, and the rotation is relative to straight forward + * from the driver station (Positive rotation is clockwise). If the limelight doesn't have a target, + * returns {@code Pose2d()}. * - * For example, returning - * {@code RigidTransform2(Vector2(12, -24), Rotation2.fromDegrees(20))} means - * that, looking out from the driver station, the limelight is one foot to the - * right of and two feet - * in front of the center of the hub and is pointing 20 degrees to the right - * (clockwise). + * For example, returning {@code Pose2d(1, -2, Rotation2d.fromDegrees(20))} means that, looking out + * from the driver station, the limelight is one meter to the right of and two meters in front of + * the center of the hub and is pointing 20 degrees to the right (clockwise). * * @return The estimated limelight pose according to vision and the gyroscope. */ - public RigidTransform2 getVisionGyroLimelightPose() { + public Pose2d getVisionGyroLimelightPose() { if (!hasTarget()) { - return RigidTransform2.ZERO; + return new Pose2d(); } - Rotation2 fieldCentricLimelightAngle = getFieldCentricRobotAngle().rotateBy(getTurretAngle()); - Rotation2 fieldCentricLimelightToHubAngle = fieldCentricLimelightAngle - .rotateBy(Rotation2.fromDegrees(getVisionYaw())); - Rotation2 fieldCentricHubToLimelightAngle = fieldCentricLimelightToHubAngle - .rotateBy(Rotation2.fromDegrees(180)); - Vector2 forwardXTranslation = Vector2.fromAngle(fieldCentricHubToLimelightAngle).scale(getDistance()); - Vector2 forwardYTranslation = forwardXTranslation.rotateBy(Rotation2.fromDegrees(90)); - return new RigidTransform2(forwardYTranslation, fieldCentricLimelightAngle); + Rotation2d fieldCentricLimelightAngle = getFieldCentricRobotAngle().rotateBy(getTurretAngle()); + Rotation2d fieldCentricLimelightToHubAngle = fieldCentricLimelightAngle + .rotateBy(Rotation2d.fromDegrees(getVisionYaw())); + Rotation2d fieldCentricHubToLimelightAngle = fieldCentricLimelightToHubAngle + .rotateBy(Rotation2d.fromDegrees(180)); + Translation2d forwardXTranslation = new Translation2d(getDistance(), fieldCentricHubToLimelightAngle); + Translation2d forwardYTranslation = forwardXTranslation.rotateBy(Rotation2d.fromDegrees(90)); + return new Pose2d(forwardYTranslation, fieldCentricLimelightAngle); } /** * Returns the estimated robot pose according to vision and the gyroscope. * - * The translation (inches) is relative to the hub, and the rotation is relative - * to straight forward - * from the drive station (Positive rotation is clockwise). If the limelight - * doesn't have a target, - * returns {@link RigidTransform2#ZERO}. + * The translation (inches) is relative to the hub, and the rotation is relative to straight forward + * from the drive station (Positive rotation is clockwise). If the limelight doesn't have a target, + * returns {@code Pose2d()}. * - * For example, returning - * {@code RigidTransform2(Vector2(12, -24), Rotation.fromDegrees(20))} means - * that, looking from the driver station, the center of the robot is one foot to - * the right of and - * two feet in front of the center of the hub and is pointing 20 degrees to the - * right (clockwise). + * For example, returning {@code Pose2d(1, -2, Rotation.fromDegrees(20))} means that, looking from + * the driver station, the center of the robot is one meter to the right of and two meters in front + * of the center of the hub and is pointing 20 degrees to the right (clockwise). * * @return The estimated robot pose according to vision and the gyroscope. */ - public RigidTransform2 getVisionGyroRobotPose() { + public Pose2d getVisionGyroRobotPose() { if (!hasTarget()) { - return RigidTransform2.ZERO; + return new Pose2d(); } - Vector2 fieldCentricHubToLimelight = getVisionGyroLimelightPose().translation; - Vector2 robotCentricTurretToLimelight = Vector2.fromAngle(getTurretAngle()) - .scale(LIMELIGHT_TO_TURRET_CENTER_DISTANCE); - Vector2 robotCentricLimelightPosition = ROBOT_CENTRIC_TURRET_CENTER.add(robotCentricTurretToLimelight); - Vector2 fieldCentricRobotToLimelight = robotCentricLimelightPosition.rotateBy(getFieldCentricRobotAngle()) - .rotateBy(Rotation2.fromDegrees(90)); - Vector2 hubToRobot = fieldCentricHubToLimelight.subtract(fieldCentricRobotToLimelight); - return new RigidTransform2(hubToRobot, getFieldCentricRobotAngle()); + Translation2d fieldCentricHubToLimelight = getVisionGyroLimelightPose().getTranslation(); + Translation2d robotCentricTurretToLimelight = new Translation2d(LIMELIGHT_TO_TURRET_CENTER_DISTANCE, + getTurretAngle()); + Translation2d robotCentricLimelightPosition = ROBOT_CENTRIC_TURRET_CENTER.plus(robotCentricTurretToLimelight); + Translation2d fieldCentricRobotToLimelight = robotCentricLimelightPosition.rotateBy(getFieldCentricRobotAngle()) + .rotateBy(Rotation2d.fromDegrees(90)); + Translation2d hubToRobot = fieldCentricHubToLimelight.minus(fieldCentricRobotToLimelight); + return new Pose2d(hubToRobot, getFieldCentricRobotAngle()); } /** - * Returns the estimated robot pose relative to the start according to vision - * and the gyroscope. + * Returns the estimated robot pose relative to the start according to vision and the gyroscope. * - * The translation (inches) is from the starting position, and the rotation is - * relative to the - * starting rotation (Positive is clockwise). If the limelight doesn't have a - * target, returns - * {@link RigidTransform2#ZERO}. + * The translation (inches) is from the starting position, and the rotation is relative to the + * starting rotation (Positive is clockwise). If the limelight doesn't have a target, returns + * {@code Pose2d()}. * - * For example, returning - * {@code RigidTransform2(Vector2(12, -24), Rotation2.fromDegrees(20))} means - * that, looking from the driver station, the robot moved one foot to the right - * and two feet closer, - * and rotated 20 degrees clockwise. + * For example, returning {@code Pose2d(1, -2, Rotation2d.fromDegrees(20))} means that, looking from + * the driver station, the robot moved one foot to the right and two feet closer, and rotated 20 + * degrees clockwise. * - * @return The estimated robot pose relative to the start according to vision - * and the gyroscope. + * @return The estimated robot pose relative to the start according to vision and the gyroscope. */ - public RigidTransform2 getVisionGyroRobotPoseRelativeToStart() { - return hasTarget() ? getVisionGyroRobotPose().transformBy(startingPose.inverse()) : RigidTransform2.ZERO; + public Pose2d getVisionGyroRobotPoseRelativeToStart() { + return hasTarget() ? getVisionGyroRobotPose().transformBy(startingPose.minus(new Pose2d())) : new Pose2d(); } public void limelightOn() { diff --git a/src/main/java/frc/team2412/robot/util/GeoConvertor.java b/src/main/java/frc/team2412/robot/util/GeoConvertor.java index e4ef166e..3407c0ec 100644 --- a/src/main/java/frc/team2412/robot/util/GeoConvertor.java +++ b/src/main/java/frc/team2412/robot/util/GeoConvertor.java @@ -30,4 +30,8 @@ public static Rotation2 rotation2dToRotation2(Rotation2d r) { public static Vector2 translation2dToVector2(Translation2d trans) { return new Vector2(trans.getX(), trans.getY()); } + + public static Translation2d vector2ToTranslation2d(Vector2 vector) { + return new Translation2d(vector.x, vector.y); + } } From 8b6db4dc3931a8e439527bd158dbd37e1e2c06a1 Mon Sep 17 00:00:00 2001 From: KangarooKoala <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 28 Mar 2022 18:33:14 -0700 Subject: [PATCH 2/9] Pose frames of reference Added doc comment to TargetLocalizer specifying field-centric poses Switched back to inches (should discuss and finalize) Switched from hub-centric to field-centric Added null checks for gyro angle Added getGyroscopeYaw() and getFieldCentricTurretAngle() Removed getVisionGyroRobotPoseRelativeToStart() Specified frames of reference Formatted some doc comments --- .../robot/subsystem/DrivebaseSubsystem.java | 5 + .../robot/subsystem/TargetLocalizer.java | 143 +++++++++--------- 2 files changed, 78 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java index ed805a5a..a6960552 100644 --- a/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystem/DrivebaseSubsystem.java @@ -233,6 +233,11 @@ public Vector2 getGyroscopeXY() { return Vector2.ZERO; } + /** + * Returns the unadjusted gyroscope angle. + * + * @return Unadjusted gyroscope angle (yaw, positive rotation is clockwise) + */ public Rotation2d getGyroscopeUnadjustedAngle() { synchronized (sensorLock) { return Rotation2d.fromDegrees(gyroscope.getUnadjustedAngle().toDegrees()); diff --git a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java index 4ea6f21a..2e7c541e 100644 --- a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java +++ b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java @@ -9,8 +9,22 @@ 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.util.Units; +/** + * Class that combines sensor data from multiple subsystems.
+ * + *

+ * Field-centric pose frame of reference: + *

+ *

+ */ public class TargetLocalizer { public static class LocalizerConstants { // TODO tune these more @@ -18,13 +32,14 @@ public static class LocalizerConstants { // Seconds, placeholder duration public static final double FILTER_TIME = 1; // Angles are in degrees + // Positive is clockwise public static final double STARTING_TURRET_ANGLE = 0; - // Dimensions are in meters - public static final double VISION_DEFAULT_DISTANCE = Units.inchesToMeters(118); + // Dimensions are in inches + public static final double VISION_DEFAULT_DISTANCE = 118; // Estimated, negative because limelight is in back of turret - public static final double LIMELIGHT_TO_TURRET_CENTER_DISTANCE = Units.inchesToMeters(-7); - public static final Translation2d ROBOT_CENTRIC_TURRET_CENTER = new Translation2d(Units.inchesToMeters(3.93), - Units.inchesToMeters(-4)); + public static final double TURRET_CENTER_TO_LIMELIGHT_DISTANCE = -7; + public static final Translation2d ROBOT_CENTRIC_TURRET_CENTER = new Translation2d(3.93, -4); + public static final Translation2d FIELD_CENTRIC_HUB_CENTER = new Translation2d(27 * 12, 13.5 * 12); } private final DrivebaseSubsystem drivebaseSubsystem; @@ -36,7 +51,7 @@ public static class LocalizerConstants { /** * Creates a new {@link TargetLocalizer}. - * If {@code drivebase} is null, will assume robot is stationary. + * If {@code drivebaseSubsystem} is null, will assume robot is stationary. * * @param drivebaseSubsystem * The drivebase subsystem. @@ -52,9 +67,8 @@ public TargetLocalizer(DrivebaseSubsystem drivebaseSubsystem, ShooterSubsystem s this.shooterVisionSubsystem = visionSubsystem; this.distanceFilter = new TimeBasedMedianFilter(FILTER_TIME); // TODO Handle different starting positions - // Also don't forget to convert reference to hub-centric if necessary - this.startingPose = new Pose2d(Units.feetToMeters(5), Units.feetToMeters(5), new Rotation2d()); - this.gyroAdjustmentAngle = startingPose.getRotation().minus(drivebaseSubsystem.getGyroscopeUnadjustedAngle()); + this.startingPose = new Pose2d(); + this.gyroAdjustmentAngle = startingPose.getRotation().minus(getGyroscopeYaw()); } public double getDistance() { @@ -89,8 +103,7 @@ public boolean hasTarget() { /** * Returns the yaw (horizontal angle) to the hub. * - * @return The yaw to the hub (0 is straight ahead, positive is clockwise, units - * are degrees). + * @return The yaw to the hub (0 is straight ahead, positive is clockwise, units are degrees). */ public double getVisionYaw() { return shooterVisionSubsystem.getYaw(); @@ -99,8 +112,7 @@ public double getVisionYaw() { /** * Returns the yaw (horizontal angle) to the target position. * - * @return The yaw to the hub plus turret angle bias (0 is straight ahead, - * positive is clockwise, + * @return The yaw to the hub plus turret angle bias (0 is straight ahead, positive is clockwise, * units are degrees). */ public double getTargetYaw() { @@ -108,21 +120,40 @@ public double getTargetYaw() { return getVisionYaw() + shooterSubsystem.getTurretAngleBias(); } + /** + * Returns the gyroscope's yaw. + * + * @return The gyroscope's yaw without the adjustment angle (positive rotation is counterclockwise). + */ + public Rotation2d getGyroscopeYaw() { + return (drivebaseSubsystem != null) ? drivebaseSubsystem.getGyroscopeUnadjustedAngle().unaryMinus() + : new Rotation2d(); + } + /** * Return the robot's angle relative to the field. * - * @return The robot angle (0 is straight forward from the driver station, - * positive rotation is - * clockwise). + * @return The robot angle (0 is straight forward from the driver station, positive rotation is + * counterclockwise). */ public Rotation2d getFieldCentricRobotAngle() { - return drivebaseSubsystem.getGyroscopeUnadjustedAngle().rotateBy(gyroAdjustmentAngle); + return getGyroscopeYaw().rotateBy(gyroAdjustmentAngle); + } + + /** + * Returns the turret's angle relative to the field. + * + * @return The turret angle (0 is straight forward from the driver station, positive rotation is + * counterclockwise). + */ + public Rotation2d getFieldCentricTurretAngle() { + return getFieldCentricRobotAngle().rotateBy(getTurretAngle().unaryMinus()); } /** * Return the turret's angle. * - * @return The turret angle (0 is intake side, positive is clockwise). + * @return The turret angle (0 is intake side, positive rotation is clockwise). */ public Rotation2d getTurretAngle() { return Rotation2d.fromDegrees(shooterSubsystem.getTurretAngle() + STARTING_TURRET_ANGLE); @@ -175,74 +206,46 @@ public double yawAdjustment() { } /** - * Returns the estimated limelight pose according to vision and the gyroscope. + * Returns the estimated limelight pose according to vision and the gyroscope.
* - * The translation (inches) is relative to the hub, and the rotation is relative to straight forward - * from the driver station (Positive rotation is clockwise). If the limelight doesn't have a target, - * returns {@code Pose2d()}. + *

+ * If the limelight doesn't have a target, returns {@code Pose2d()}. + *

* - * For example, returning {@code Pose2d(1, -2, Rotation2d.fromDegrees(20))} means that, looking out - * from the driver station, the limelight is one meter to the right of and two meters in front of - * the center of the hub and is pointing 20 degrees to the right (clockwise). - * - * @return The estimated limelight pose according to vision and the gyroscope. + * @return The field-centric estimated limelight pose according to vision and the gyroscope. */ public Pose2d getVisionGyroLimelightPose() { if (!hasTarget()) { return new Pose2d(); } - Rotation2d fieldCentricLimelightAngle = getFieldCentricRobotAngle().rotateBy(getTurretAngle()); - Rotation2d fieldCentricLimelightToHubAngle = fieldCentricLimelightAngle - .rotateBy(Rotation2d.fromDegrees(getVisionYaw())); - Rotation2d fieldCentricHubToLimelightAngle = fieldCentricLimelightToHubAngle - .rotateBy(Rotation2d.fromDegrees(180)); - Translation2d forwardXTranslation = new Translation2d(getDistance(), fieldCentricHubToLimelightAngle); - Translation2d forwardYTranslation = forwardXTranslation.rotateBy(Rotation2d.fromDegrees(90)); - return new Pose2d(forwardYTranslation, fieldCentricLimelightAngle); + Rotation2d limelightAngle = getFieldCentricTurretAngle(); + Rotation2d hubAngle = limelightAngle.rotateBy(Rotation2d.fromDegrees(-getVisionYaw())); + Translation2d limelightToHub = new Translation2d(getDistance(), hubAngle); + Translation2d translation = FIELD_CENTRIC_HUB_CENTER.minus(limelightToHub); + return new Pose2d(translation, limelightAngle); } /** - * Returns the estimated robot pose according to vision and the gyroscope. - * - * The translation (inches) is relative to the hub, and the rotation is relative to straight forward - * from the drive station (Positive rotation is clockwise). If the limelight doesn't have a target, - * returns {@code Pose2d()}. + * Returns the estimated robot pose according to vision and the gyroscope.
* - * For example, returning {@code Pose2d(1, -2, Rotation.fromDegrees(20))} means that, looking from - * the driver station, the center of the robot is one meter to the right of and two meters in front - * of the center of the hub and is pointing 20 degrees to the right (clockwise). + *

+ * If the limelight doesn't have a target, returns {@code Pose2d()}. + *

* - * @return The estimated robot pose according to vision and the gyroscope. + * @return The estimated field-centric robot pose according to vision and the gyroscope. */ public Pose2d getVisionGyroRobotPose() { if (!hasTarget()) { return new Pose2d(); } - Translation2d fieldCentricHubToLimelight = getVisionGyroLimelightPose().getTranslation(); - Translation2d robotCentricTurretToLimelight = new Translation2d(LIMELIGHT_TO_TURRET_CENTER_DISTANCE, - getTurretAngle()); - Translation2d robotCentricLimelightPosition = ROBOT_CENTRIC_TURRET_CENTER.plus(robotCentricTurretToLimelight); - Translation2d fieldCentricRobotToLimelight = robotCentricLimelightPosition.rotateBy(getFieldCentricRobotAngle()) - .rotateBy(Rotation2d.fromDegrees(90)); - Translation2d hubToRobot = fieldCentricHubToLimelight.minus(fieldCentricRobotToLimelight); - return new Pose2d(hubToRobot, getFieldCentricRobotAngle()); - } - - /** - * Returns the estimated robot pose relative to the start according to vision and the gyroscope. - * - * The translation (inches) is from the starting position, and the rotation is relative to the - * starting rotation (Positive is clockwise). If the limelight doesn't have a target, returns - * {@code Pose2d()}. - * - * For example, returning {@code Pose2d(1, -2, Rotation2d.fromDegrees(20))} means that, looking from - * the driver station, the robot moved one foot to the right and two feet closer, and rotated 20 - * degrees clockwise. - * - * @return The estimated robot pose relative to the start according to vision and the gyroscope. - */ - public Pose2d getVisionGyroRobotPoseRelativeToStart() { - return hasTarget() ? getVisionGyroRobotPose().transformBy(startingPose.minus(new Pose2d())) : new Pose2d(); + Rotation2d robotAngle = getFieldCentricRobotAngle(); + Translation2d limelightTranslation = getVisionGyroLimelightPose().getTranslation(); + Translation2d turretToLimelight = new Translation2d(TURRET_CENTER_TO_LIMELIGHT_DISTANCE, + getFieldCentricTurretAngle()); + Translation2d robotToTurret = ROBOT_CENTRIC_TURRET_CENTER.rotateBy(new Rotation2d(Math.PI / 2)) + .rotateBy(robotAngle); + Translation2d translation = limelightTranslation.minus(robotToTurret.plus(turretToLimelight)); + return new Pose2d(translation, robotAngle); } public void limelightOn() { From 259913cebac27c7e41f7094be1eb32ba90428950 Mon Sep 17 00:00:00 2001 From: KangarooKoala <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 28 Mar 2022 19:19:52 -0700 Subject: [PATCH 3/9] Added correction for different start poses Made startingPose and gyroAdjustmentAngle non-final Added resetPose() and resetPoseFromAutoStartPose() Reset target localizer pose in autonomousInit() --- src/main/java/frc/team2412/robot/Robot.java | 4 +++ .../robot/subsystem/TargetLocalizer.java | 28 ++++++++++++++++--- 2 files changed, 28 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 8629e34c..a54957e1 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -211,6 +211,10 @@ public void autonomousInit() { subsystems.drivebaseSubsystem.resetPose(autonomousChooser.getStartPose()); } + if (subsystems.targetLocalizer != null) { + subsystems.targetLocalizer.resetPoseFromAutoStartPose(autonomousChooser); + } + if (subsystems.shooterSubsystem != null) { new ShooterResetEncodersCommand(subsystems.shooterSubsystem).schedule(); } diff --git a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java index 2e7c541e..14bb5172 100644 --- a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java +++ b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java @@ -3,6 +3,7 @@ import frc.team2412.robot.Robot; import frc.team2412.robot.util.GeoConvertor; import frc.team2412.robot.util.TimeBasedMedianFilter; +import frc.team2412.robot.util.autonomous.AutonomousChooser; import static frc.team2412.robot.subsystem.TargetLocalizer.LocalizerConstants.*; @@ -46,8 +47,8 @@ public static class LocalizerConstants { private final ShooterSubsystem shooterSubsystem; private final ShooterVisionSubsystem shooterVisionSubsystem; private final TimeBasedMedianFilter distanceFilter; - private final Rotation2d gyroAdjustmentAngle; - private final Pose2d startingPose; + private Pose2d startingPose; + private Rotation2d gyroAdjustmentAngle; /** * Creates a new {@link TargetLocalizer}. @@ -66,8 +67,27 @@ public TargetLocalizer(DrivebaseSubsystem drivebaseSubsystem, ShooterSubsystem s this.shooterSubsystem = shooterSubsystem; this.shooterVisionSubsystem = visionSubsystem; this.distanceFilter = new TimeBasedMedianFilter(FILTER_TIME); - // TODO Handle different starting positions - this.startingPose = new Pose2d(); + resetPose(new Pose2d()); + } + + /** + * Rests the current pose according to the current auto path. + * + * @param autoChooser + * The autonomous chooser with the current auto path. + */ + public void resetPoseFromAutoStartPose(AutonomousChooser autoChooser) { + resetPose(autoChooser.getStartPose()); + } + + /** + * Resets the current pose. + * + * @param newPose + * The new (field-centric) pose. + */ + public void resetPose(Pose2d newPose) { + this.startingPose = newPose; this.gyroAdjustmentAngle = startingPose.getRotation().minus(getGyroscopeYaw()); } From 175f84ae2e145db3b890563834e26d6251fa8772 Mon Sep 17 00:00:00 2001 From: KangarooKoala <91924258+KangarooKoala@users.noreply.github.com> Date: Tue, 29 Mar 2022 17:56:35 -0700 Subject: [PATCH 4/9] Moved constants to appropriate subsystems Also fixed typo in docs --- .../team2412/robot/subsystem/ShooterSubsystem.java | 2 +- .../robot/subsystem/ShooterVisionSubsystem.java | 8 ++++++++ .../team2412/robot/subsystem/TargetLocalizer.java | 12 +++--------- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java index 4b42aa0d..9e5aab49 100644 --- a/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystem/ShooterSubsystem.java @@ -450,7 +450,7 @@ public boolean isTurretAtAngle(double angle) { @Config(name = "Reset turret", columnIndex = 9, rowIndex = 2) public void resetTurretEncoder(boolean reset) { if (reset) { - turretMotor.setSelectedSensorPosition(STARTING_TURRET_ANGLE); + turretMotor.setSelectedSensorPosition(STARTING_TURRET_ANGLE * TURRET_DEGREES_TO_ENCODER_TICKS); } } diff --git a/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java index 31e82b60..5383549a 100644 --- a/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystem/ShooterVisionSubsystem.java @@ -17,6 +17,7 @@ public static class ShooterVisionConstants { public static final double RIM_HEIGHT = 104; // 8ft8in public static final double HEIGHT_TO_RIM = RIM_HEIGHT - LIMELIGHT_HEIGHT_OFFSET; public static final double HUB_RADIUS = 24; + public static final double DEFAULT_DISTANCE = 118; // Angles are in degrees public static final double LIMELIGHT_ANGLE_OFFSET = Math.toDegrees(Math.atan2(HEIGHT_TO_RIM, 360 - HUB_RADIUS)); // 10.95 @@ -50,10 +51,17 @@ public double getYaw() { * Note: The limelight returns measurements relative to the center of the targets in its field of * view, which may differ from the center of the hub. * + *

+ * If vision does not have a target, returns {@code DEFAULT_DISTANCE}. + *

+ * * @return The distance in inches. */ @Log(name = "Distance") public double getDistance() { + if (!hasTarget()) { + return DEFAULT_DISTANCE; + } double distanceToHubRim = HEIGHT_TO_RIM / Math.tan(Math.toRadians(getAdjustedPitch())); return distanceToHubRim + HUB_RADIUS; } diff --git a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java index 14bb5172..10e921ff 100644 --- a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java +++ b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java @@ -32,11 +32,7 @@ public static class LocalizerConstants { public static final double TURRET_LATERAL_FF = 0, TURRET_ANGULAR_FF = 4, TURRET_DEPTH_FF = 0; // Seconds, placeholder duration public static final double FILTER_TIME = 1; - // Angles are in degrees - // Positive is clockwise - public static final double STARTING_TURRET_ANGLE = 0; // Dimensions are in inches - public static final double VISION_DEFAULT_DISTANCE = 118; // Estimated, negative because limelight is in back of turret public static final double TURRET_CENTER_TO_LIMELIGHT_DISTANCE = -7; public static final Translation2d ROBOT_CENTRIC_TURRET_CENTER = new Translation2d(3.93, -4); @@ -71,7 +67,7 @@ public TargetLocalizer(DrivebaseSubsystem drivebaseSubsystem, ShooterSubsystem s } /** - * Rests the current pose according to the current auto path. + * Resets the current pose according to the current auto path. * * @param autoChooser * The autonomous chooser with the current auto path. @@ -92,9 +88,7 @@ public void resetPose(Pose2d newPose) { } public double getDistance() { - return hasTarget() - ? distanceFilter.calculate(shooterVisionSubsystem.getDistance() + shooterSubsystem.getDistanceBias()) - : VISION_DEFAULT_DISTANCE; + return distanceFilter.calculate(shooterVisionSubsystem.getDistance() + shooterSubsystem.getDistanceBias()); } public double getAdjustedDistance() { @@ -176,7 +170,7 @@ public Rotation2d getFieldCentricTurretAngle() { * @return The turret angle (0 is intake side, positive rotation is clockwise). */ public Rotation2d getTurretAngle() { - return Rotation2d.fromDegrees(shooterSubsystem.getTurretAngle() + STARTING_TURRET_ANGLE); + return Rotation2d.fromDegrees(shooterSubsystem.getTurretAngle()); } public Translation2d getTurretRelativeVelocity() { From c5b15bc9d9260e144c54128f7e71c714cc93dbd4 Mon Sep 17 00:00:00 2001 From: KangarooKoala <91924258+KangarooKoala@users.noreply.github.com> Date: Wed, 6 Apr 2022 09:40:43 -0700 Subject: [PATCH 5/9] Merge branch 'main' of https://github.com/robototes/RapidReact2022 into pose-estimation Fix typo in getDepthVelocity() Added check if the limelight has a target before filtering --- .../java/frc/team2412/robot/Controls.java | 7 +++- .../java/frc/team2412/robot/Hardware.java | 5 ++- src/main/java/frc/team2412/robot/Robot.java | 7 ++-- .../autonomous/JackFiveBallAutoCommand.java | 16 +++---- .../commands/climb/PostClimbUpComamnd.java | 8 +--- .../robot/subsystem/PostClimbSubsystem.java | 27 ++++++++---- .../robot/subsystem/TargetLocalizer.java | 5 ++- .../util/autonomous/AutonomousChooser.java | 4 +- vendordeps/Phoenix.json | 42 +++++++++---------- vendordeps/PhotonLib-json-1.0.json | 41 ------------------ 10 files changed, 69 insertions(+), 93 deletions(-) delete mode 100644 vendordeps/PhotonLib-json-1.0.json diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 27b40e8d..53a1f704 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.button.Button; import frc.team2412.robot.commands.climb.ClimbSetArmCommand; import frc.team2412.robot.commands.climb.ExtendArmCommand; +import frc.team2412.robot.commands.climb.PostClimbUpComamnd; import frc.team2412.robot.commands.climb.RetractArmCommand; import frc.team2412.robot.commands.index.IndexCommand; import frc.team2412.robot.commands.index.IndexShootCommand; @@ -109,7 +110,11 @@ public void bindClimbControls() { climbArmDownManual.whileHeld(new ClimbSetArmCommand(subsystems.climbSubsystem, -0.4)); climbArmUpManual.whileHeld(new ClimbSetArmCommand(subsystems.climbSubsystem, 0.4)); + } + public void bindPostClimbControls() { + climbArmUp.and(climbArmDown).and(climbArmDownManual).and(climbArmUpManual) + .whenActive(new PostClimbUpComamnd(subsystems.postClimbSubsystem)); } public void bindDriveControls() { @@ -152,7 +157,7 @@ public void bindShooterControls() { driveController.getDPadButton(Direction.RIGHT).whenPressed( new ShooterHoodRPMCommand(subsystems.shooterSubsystem, 0, 0).withInterrupt(b) - .alongWith(new InstantCommand(() -> subsystems.shooterSubsystem.setTurretAngle(90)))); + .alongWith(new InstantCommand(() -> subsystems.shooterSubsystem.setTurretAngle(0)))); if (subsystems.drivebaseSubsystem != null) { subsystems.shooterSubsystem.setDefaultCommand( diff --git a/src/main/java/frc/team2412/robot/Hardware.java b/src/main/java/frc/team2412/robot/Hardware.java index 955dc649..f32938ad 100644 --- a/src/main/java/frc/team2412/robot/Hardware.java +++ b/src/main/java/frc/team2412/robot/Hardware.java @@ -83,7 +83,10 @@ public class Hardware { // climb can ids are range 50-59 public static final int CLIMB_FIXED_MOTOR = 51; public static final int CLIMB_LIMIT_SWITCH = 9; // to be determined - digital I/O pins are 0-9 - public static final int POST_CLIMB_SOLENOID_UPWARDS = 2; // TODO + + public static final int POST_CLIMB_BLOCKING_SOLENOID = 2; // TODO + public static final int POST_CLIMB_FIRING_SOLENOID_FIRE = 3; + public static final int POST_CLIMB_FIRING_SOLENOID_CLOSE = 4; // Other hardware public static final int PDP_ID = 1; // needs to be verified on the bot (Can be found in REV) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 2e6e9283..d6711f38 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -25,7 +25,6 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.team2412.robot.commands.shooter.ShooterResetEncodersCommand; import frc.team2412.robot.sim.PhysicsSim; import frc.team2412.robot.subsystem.TestingSubsystem; import frc.team2412.robot.util.MACAddress; @@ -225,9 +224,9 @@ public void autonomousInit() { subsystems.targetLocalizer.resetPoseFromAutoStartPose(autonomousChooser); } - if (subsystems.shooterSubsystem != null) { - new ShooterResetEncodersCommand(subsystems.shooterSubsystem).schedule(); - } + // if (subsystems.shooterSubsystem != null) { + // new ShooterResetEncodersCommand(subsystems.shooterSubsystem).schedule(); + // } autonomousChooser.scheduleCommand(); } diff --git a/src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java b/src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java index 4cc677e8..daa5ea2e 100644 --- a/src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java +++ b/src/main/java/frc/team2412/robot/commands/autonomous/JackFiveBallAutoCommand.java @@ -3,7 +3,7 @@ import edu.wpi.first.wpilibj2.command.*; import frc.team2412.robot.commands.index.IndexShootCommand; import frc.team2412.robot.commands.index.IndexSpitCommand; -import frc.team2412.robot.commands.intake.IntakeCommand; +import frc.team2412.robot.commands.intake.IntakeSetInCommand; import frc.team2412.robot.commands.intake.IntakeSetStopCommand; import org.frcteam2910.common.control.CentripetalAccelerationConstraint; import org.frcteam2910.common.control.FeedforwardConstraint; @@ -58,7 +58,7 @@ public static class FiveBallConstants { public static final Trajectory PATH_1 = new Trajectory( new SimplePathBuilder(new Vector2(328, 75.551), Rotation2.fromDegrees(-90)) - .arcTo(new Vector2(293.811, 29.771), new Vector2(281.299, 92.401), Rotation2.fromDegrees(-90)) + .arcTo(new Vector2(293.811, 25.771), new Vector2(281.299, 92.401), Rotation2.fromDegrees(-90)) .lineTo(new Vector2(218.203, 60.492), Rotation2.fromDegrees(130)) .build(), NORMAL_SPEED, 0.1); @@ -71,7 +71,7 @@ public static class FiveBallConstants { public static final Trajectory PATH_3 = new Trajectory( new SimplePathBuilder(new Vector2(195.029, 75.188), Rotation2.fromDegrees(125)) - .lineTo(new Vector2(50.456, 85.818), Rotation2.fromDegrees(202)) + .lineTo(new Vector2(50.456, 88.818), Rotation2.fromDegrees(202)) .build(), NORMAL_SPEED, 0.1); @@ -94,19 +94,19 @@ public JackFiveBallAutoCommand(DrivebaseSubsystem drivebaseSubsystem, IntakeSubs ShooterTargetCommand.TurretManager manager = new ShooterTargetCommand.TurretManager(shooterSubsystem, localizer); addCommands( - manager.scheduleCommand().alongWith( manager.disableAt(0), - new ScheduleCommand(new IntakeCommand(intakeSubsystem, indexSubsystem)), - new IndexSpitCommand(indexSubsystem).withTimeout(0.1)), + new IntakeSetInCommand(intakeSubsystem), + // new ScheduleCommand(new IntakeCommand(intakeSubsystem, indexSubsystem)), + new IndexSpitCommand(indexSubsystem).withTimeout(0.05)), new Follow2910TrajectoryCommand(drivebaseSubsystem, PATH_1), manager.enableAt(0), new IndexShootCommand(indexSubsystem).withTimeout(1), new Follow2910TrajectoryCommand(drivebaseSubsystem, PATH_2), - new IndexShootCommand(indexSubsystem).withTimeout(2), + new IndexShootCommand(indexSubsystem).withTimeout(1), manager.disableAt(70), new Follow2910TrajectoryCommand(drivebaseSubsystem, PATH_3), - new WaitCommand(2), + new WaitCommand(1.5), new Follow2910TrajectoryCommand(drivebaseSubsystem, PATH_4), manager.enableAt(70), new IndexShootCommand(indexSubsystem).withTimeout(2), diff --git a/src/main/java/frc/team2412/robot/commands/climb/PostClimbUpComamnd.java b/src/main/java/frc/team2412/robot/commands/climb/PostClimbUpComamnd.java index 73a45cda..ca6c4676 100644 --- a/src/main/java/frc/team2412/robot/commands/climb/PostClimbUpComamnd.java +++ b/src/main/java/frc/team2412/robot/commands/climb/PostClimbUpComamnd.java @@ -14,13 +14,7 @@ public PostClimbUpComamnd(PostClimbSubsystem subsystem) { @Override public void execute() { - subsystem.releaseClamp(); - } - - @Override - public boolean runsWhenDisabled() { - // TODO Auto-generated method stub - return true; + subsystem.armSolenoid(); } @Override diff --git a/src/main/java/frc/team2412/robot/subsystem/PostClimbSubsystem.java b/src/main/java/frc/team2412/robot/subsystem/PostClimbSubsystem.java index 8d3989fc..75c05768 100644 --- a/src/main/java/frc/team2412/robot/subsystem/PostClimbSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystem/PostClimbSubsystem.java @@ -2,6 +2,7 @@ import static frc.team2412.robot.Hardware.*; +import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -9,19 +10,31 @@ public class PostClimbSubsystem extends SubsystemBase implements Loggable { - private Solenoid clampSolenoid; + private Solenoid blockSolenoid; + private DoubleSolenoid firingSolenoid; public PostClimbSubsystem() { - clampSolenoid = new Solenoid(PNEUMATIC_HUB, PneumaticsModuleType.REVPH, POST_CLIMB_SOLENOID_UPWARDS); - setClamp(); + blockSolenoid = new Solenoid(PNEUMATIC_HUB, PneumaticsModuleType.REVPH, POST_CLIMB_BLOCKING_SOLENOID); + firingSolenoid = new DoubleSolenoid(PNEUMATIC_HUB, PneumaticsModuleType.REVPH, POST_CLIMB_FIRING_SOLENOID_FIRE, + POST_CLIMB_FIRING_SOLENOID_CLOSE); + block(); + disarmSolenoid(); } - public void releaseClamp() { - clampSolenoid.set(true); + public void removeBlock() { + blockSolenoid.set(false); } - public void setClamp() { - clampSolenoid.set(false); + public void block() { + blockSolenoid.set(true); + } + + public void armSolenoid() { + firingSolenoid.set(DoubleSolenoid.Value.kForward); + } + + public void disarmSolenoid() { + firingSolenoid.set(DoubleSolenoid.Value.kReverse); } } diff --git a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java index 84c33f97..e484ca63 100644 --- a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java +++ b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java @@ -99,7 +99,8 @@ public void resetPose(Pose2d newPose) { } public double getDistance() { - return distanceFilter.calculate(shooterVisionSubsystem.getDistance() + shooterSubsystem.getDistanceBias()); + double distance = shooterVisionSubsystem.getDistance() + shooterSubsystem.getDistanceBias(); + return hasTarget() ? distanceFilter.calculate(distance) : distance; } public double getAdjustedDistance() { @@ -209,7 +210,7 @@ public double getLateralVelocity() { * @return that */ public double getDepthVelocity() { - return getTurretRelativeVelocity().getX(); + return getTurretRelativeVelocity().getY(); } public double getAngularVelocity() { diff --git a/src/main/java/frc/team2412/robot/util/autonomous/AutonomousChooser.java b/src/main/java/frc/team2412/robot/util/autonomous/AutonomousChooser.java index d6408e56..633eaf0a 100644 --- a/src/main/java/frc/team2412/robot/util/autonomous/AutonomousChooser.java +++ b/src/main/java/frc/team2412/robot/util/autonomous/AutonomousChooser.java @@ -26,6 +26,7 @@ import frc.team2412.robot.commands.diagnostic.DiagnosticIntakeCommandGroup; import frc.team2412.robot.commands.index.IndexShootCommand; import frc.team2412.robot.commands.index.IndexTestCommand; +import frc.team2412.robot.commands.intake.*; import frc.team2412.robot.commands.shooter.ShooterTargetCommand; import frc.team2412.robot.commands.shooter.ShooterTurretSetAngleCommand; @@ -177,7 +178,8 @@ public enum AutonomousMode { Subsystems.SubsystemConstants.SHOOTER_ENABLED), INTAKE_SHOOTER( (subsystems) -> new IndexShootCommand(subsystems.indexSubsystem).alongWith( - new ShooterTargetCommand(subsystems.shooterSubsystem, subsystems.targetLocalizer)), + new ShooterTargetCommand(subsystems.shooterSubsystem, subsystems.targetLocalizer) + .alongWith(new IntakeSetInCommand(subsystems.intakeSubsystem))), "Intake and shoot", Subsystems.SubsystemConstants.INTAKE_ENABLED && Subsystems.SubsystemConstants.INDEX_ENABLED && diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index 02f72003..dd0b3f43 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.21.1", + "version": "5.21.2", "frcYear": 2022, "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ @@ -12,19 +12,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.21.1" + "version": "5.21.2" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.21.1" + "version": "5.21.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.21.1", + "version": "5.21.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.21.1", + "version": "5.21.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -46,7 +46,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "simTalonSRX", - "version": "5.21.1", + "version": "5.21.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +58,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "simTalonFX", - "version": "5.21.1", + "version": "5.21.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -70,7 +70,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "simVictorSPX", - "version": "5.21.1", + "version": "5.21.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "simPigeonIMU", - "version": "5.21.1", + "version": "5.21.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -94,7 +94,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "simCANCoder", - "version": "5.21.1", + "version": "5.21.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -108,7 +108,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.21.1", + "version": "5.21.2", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -120,7 +120,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.21.1", + "version": "5.21.2", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -132,7 +132,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.21.1", + "version": "5.21.2", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -144,7 +144,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.21.1", + "version": "5.21.2", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.21.1", + "version": "5.21.2", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -172,7 +172,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.21.1", + "version": "5.21.2", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -186,7 +186,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "simTalonSRX", - "version": "5.21.1", + "version": "5.21.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -200,7 +200,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "simTalonFX", - "version": "5.21.1", + "version": "5.21.2", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -214,7 +214,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "simVictorSPX", - "version": "5.21.1", + "version": "5.21.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +228,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "simPigeonIMU", - "version": "5.21.1", + "version": "5.21.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +242,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "simCANCoder", - "version": "5.21.1", + "version": "5.21.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/PhotonLib-json-1.0.json b/vendordeps/PhotonLib-json-1.0.json deleted file mode 100644 index 4cc5192b..00000000 --- a/vendordeps/PhotonLib-json-1.0.json +++ /dev/null @@ -1,41 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2022.1.4", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "PhotonLib-cpp", - "version": "v2022.1.4", - "libName": "Photon", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxx86-64" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "PhotonLib-java", - "version": "v2022.1.4" - }, - { - "groupId": "org.photonvision", - "artifactId": "PhotonTargeting-java", - "version": "v2022.1.4" - } - ] -} From a97fe79b4c30e7484eb54fbe905dcdc5576db72c Mon Sep 17 00:00:00 2001 From: KangarooKoala <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 11 Apr 2022 09:46:46 -0700 Subject: [PATCH 6/9] Merge branch 'main' of https://github.com/robototes/RapidReact2022 into pose-estimation From fc46063f6c3b67d2302ac103ddc33bf307b456df Mon Sep 17 00:00:00 2001 From: KangarooKoala <91924258+KangarooKoala@users.noreply.github.com> Date: Wed, 4 May 2022 13:02:19 -0700 Subject: [PATCH 7/9] Add VisionUtil Separate pose estimation logic Change getTurretAngle() to be CCW-positive --- .../robot/subsystem/TargetLocalizer.java | 72 ++++------- .../frc/team2412/robot/util/VisionUtil.java | 116 ++++++++++++++++++ 2 files changed, 139 insertions(+), 49 deletions(-) create mode 100644 src/main/java/frc/team2412/robot/util/VisionUtil.java diff --git a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java index 9ac149aa..4fa95894 100644 --- a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java +++ b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.filter.LinearFilter; import frc.team2412.robot.Robot; import frc.team2412.robot.util.GeoConvertor; +import frc.team2412.robot.util.VisionUtil; import frc.team2412.robot.util.autonomous.AutonomousChooser; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Config; @@ -19,15 +20,7 @@ * Class that combines sensor data from multiple subsystems.
* *

- * Field-centric pose frame of reference: - *

+ * See {@link VisionUtil} for the field coordinate system. *

*/ public class TargetLocalizer implements Loggable { @@ -191,28 +184,28 @@ public Rotation2d getFieldCentricRobotAngle() { } /** - * Returns the turret's angle relative to the field. + * Return the turret's angle where clockwise rotation is positive. * - * @return The turret angle (0 is straight forward from the driver station, positive rotation is - * counterclockwise). + * @return The turret angle (0 is intake side, positive rotation is clockwise). */ - public Rotation2d getFieldCentricTurretAngle() { - return getFieldCentricRobotAngle().rotateBy(getTurretAngle().unaryMinus()); + public Rotation2d getCWPositiveTurretAngle() { + return Rotation2d.fromDegrees(shooterSubsystem.getTurretAngle()); } /** - * Return the turret's angle. + * Returns the turret's angle where counterclockwise is positive. * - * @return The turret angle (0 is intake side, positive rotation is clockwise). + * @return The turret angle (0 is intake side, positive rotation is counterclockwise). */ public Rotation2d getTurretAngle() { - return Rotation2d.fromDegrees(shooterSubsystem.getTurretAngle()); + return getCWPositiveTurretAngle().unaryMinus(); } public Translation2d getTurretRelativeVelocity() { return (drivebaseSubsystem != null) // might need to do inverse - ? GeoConvertor.vector2ToTranslation2d(drivebaseSubsystem.getVelocity()).rotateBy(getTurretAngle()) + ? GeoConvertor.vector2ToTranslation2d(drivebaseSubsystem.getVelocity()) + .rotateBy(getCWPositiveTurretAngle()) : new Translation2d(); } @@ -262,46 +255,27 @@ public double yawAdjustment() { } /** - * Returns the estimated limelight pose according to vision and the gyroscope.
- * - *

- * If the limelight doesn't have a target, returns {@code Pose2d()}. - *

- * - * @return The field-centric estimated limelight pose according to vision and the gyroscope. - */ - public Pose2d getVisionGyroLimelightPose() { - if (!hasTarget()) { - return new Pose2d(); - } - Rotation2d limelightAngle = getFieldCentricTurretAngle(); - Rotation2d hubAngle = limelightAngle.rotateBy(Rotation2d.fromDegrees(-getVisionYaw())); - Translation2d limelightToHub = new Translation2d(getDistance(), hubAngle); - Translation2d translation = FIELD_CENTRIC_HUB_CENTER.minus(limelightToHub); - return new Pose2d(translation, limelightAngle); - } - - /** - * Returns the estimated robot pose according to vision and the gyroscope.
+ * Returns the estimated robot pose according to vision and the gyroscope. * *

- * If the limelight doesn't have a target, returns {@code Pose2d()}. - *

+ * If the limelight doesn't have a target, returns {@code new Pose2d()}. * * @return The estimated field-centric robot pose according to vision and the gyroscope. */ - public Pose2d getVisionGyroRobotPose() { + public Pose2d estimateRobotPoseFromVisionGyro() { if (!hasTarget()) { return new Pose2d(); } + double targetDistance = getDistance(); + Rotation2d targetYaw = Rotation2d.fromDegrees(-getTargetYaw()); + Translation2d targetPosition = FIELD_CENTRIC_HUB_CENTER; Rotation2d robotAngle = getFieldCentricRobotAngle(); - Translation2d limelightTranslation = getVisionGyroLimelightPose().getTranslation(); - Translation2d turretToLimelight = new Translation2d(TURRET_CENTER_TO_LIMELIGHT_DISTANCE, - getFieldCentricTurretAngle()); - Translation2d robotToTurret = ROBOT_CENTRIC_TURRET_CENTER.rotateBy(new Rotation2d(Math.PI / 2)) - .rotateBy(robotAngle); - Translation2d translation = limelightTranslation.minus(robotToTurret.plus(turretToLimelight)); - return new Pose2d(translation, robotAngle); + Rotation2d robotToCameraAngle = getTurretAngle(); + Translation2d robotCentricCameraPosition = ROBOT_CENTRIC_TURRET_CENTER + .rotateBy(new Rotation2d(Math.PI / 2)) + .plus(new Translation2d(TURRET_CENTER_TO_LIMELIGHT_DISTANCE, robotToCameraAngle)); + return VisionUtil.estimateRobotPose(targetPosition, targetDistance, targetYaw, robotAngle, + robotToCameraAngle, robotCentricCameraPosition); } public void limelightOn() { diff --git a/src/main/java/frc/team2412/robot/util/VisionUtil.java b/src/main/java/frc/team2412/robot/util/VisionUtil.java new file mode 100644 index 00000000..6c99cbf5 --- /dev/null +++ b/src/main/java/frc/team2412/robot/util/VisionUtil.java @@ -0,0 +1,116 @@ +package frc.team2412.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; + +/** + * Utility class for estimating the robot pose. + * + *

+ * Field coordinate system: + *

+ * Equivalently, if your alliance wall is on the left: + * + *

+ */ +public class VisionUtil { + /** + * Estimates the camera pose. + * + * @param targetPosition + * The position of the target in the field coordinate system. + * @param targetDistance + * The distance from the camera to the target. + * @param targetYaw + * The CCW-positive angle from the camera to the target. Note that Photon + * returns CW-positive. + * @param cameraAngle + * The angle of the camera in the field coordinate system. + * @return The estimated camera pose in the field coordinate system. + */ + public static Pose2d estimateCameraPose(Translation2d targetPosition, double targetDistance, Rotation2d targetYaw, + Rotation2d cameraAngle) { + Translation2d cameraToTarget = new Translation2d(targetDistance, cameraAngle.plus(targetYaw)); + return new Pose2d(targetPosition.minus(cameraToTarget), cameraAngle); + } + + /** + * Estimates the field-centric robot pose. + * + * @param targetPosition + * The position of the target in the field coordinate system. + * @param targetDistance + * The distance from the camera to the target. + * @param targetYaw + * The CCW-positive angle from the camera to the target. Note that Photon returns + * CW-positive. + * @param cameraAngle + * The angle of the camera in the field coordinate system. + * @param cameraToRobot + * The transformation from the camera to the robot. + * @return The robot pose in the field coordinate system. + */ + public static Pose2d estimateRobotPose(Translation2d targetPosition, double targetDistance, Rotation2d targetYaw, + Rotation2d cameraAngle, Transform2d cameraToRobot) { + return estimateCameraPose(targetPosition, targetDistance, targetYaw, cameraAngle).transformBy(cameraToRobot); + } + + /** + * Calculates the transformation from the camera to the robot. + * + * @param robotCentricCameraPosition + * The position of the camera relative to the robot center. Note that the +X axis must be + * along {@code robotAngle}, and +Y axis along robotAngle is common. + * @param robotToCameraAngle + * The CCW-positive angle from the robot to the camera. + * @param robotAngle + * The angle of the robot in the field coordinate system. + * @return The transformation from the camera to the robot. + */ + public static Transform2d calculateCameraToRobot(Translation2d robotCentricCameraPosition, + Rotation2d robotToCameraAngle) { + return new Transform2d(robotCentricCameraPosition, robotToCameraAngle).inverse(); + } + + /** + * Estimates the field-centric robot pose. + * + * @param targetPosition + * The position of the target in the field coordinate system. + * @param targetDistance + * The distance from the camera to the target. + * @param targetYaw + * The CCW-positive angle from the camera to the target. Note that Photon returns + * CW-positive. + * @param robotAngle + * The angle of the robot in the field coordinate system. + * @param robotToCameraAngle + * The CCW-positive angle from the robot to the camera. + * @param robotCentricCameraPosition + * The position of the camera relative to the robot center. Note that the +X axis must be + * along {@code robotAngle}, and +Y axis along robotAngle is common. + * @return The robot pose in the field coordinate system. + */ + public static Pose2d estimateRobotPose(Translation2d targetPosition, double targetDistance, Rotation2d targetYaw, + Rotation2d robotAngle, Rotation2d robotToCameraAngle, Translation2d robotCentricCameraPosition) { + Translation2d cameraToTarget = new Translation2d(targetDistance, + robotAngle.plus(robotToCameraAngle).plus(targetYaw)); + Translation2d cameraPosition = targetPosition.minus(cameraToTarget); + Translation2d cameraToRobot = robotCentricCameraPosition.rotateBy(robotAngle); + return new Pose2d(cameraPosition.minus(cameraToRobot), robotAngle); + } +} From dd913191b5173d71ba2e67c8c434bc640d4c25a6 Mon Sep 17 00:00:00 2001 From: KangarooKoala <91924258+KangarooKoala@users.noreply.github.com> Date: Wed, 4 May 2022 16:47:16 -0700 Subject: [PATCH 8/9] Misc. fixes and clean-ups Remove doc comment parameter for removed parameter Change ROBOT_CENTRIC_TURRET_CENTER to +X axis forward (intake side) Remove unused startingPose Ignore turret bias in pose estimation --- .../frc/team2412/robot/subsystem/TargetLocalizer.java | 10 ++++------ src/main/java/frc/team2412/robot/util/VisionUtil.java | 2 -- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java index 4fa95894..53ba5d8c 100644 --- a/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java +++ b/src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java @@ -41,7 +41,8 @@ public static class LocalizerConstants { // Dimensions are in inches // Estimated, negative because limelight is in back of turret public static final double TURRET_CENTER_TO_LIMELIGHT_DISTANCE = -7; - public static final Translation2d ROBOT_CENTRIC_TURRET_CENTER = new Translation2d(3.93, -4); + // +X axis is forward + public static final Translation2d ROBOT_CENTRIC_TURRET_CENTER = new Translation2d(-4, -3.93); public static final Translation2d FIELD_CENTRIC_HUB_CENTER = new Translation2d(27 * 12, 13.5 * 12); } @@ -59,7 +60,6 @@ public static class LocalizerConstants { private final LinearFilter distanceFilter; private final LinearFilter yawPass; - private Pose2d startingPose; private Rotation2d gyroAdjustmentAngle; private double turretLateralFF = TURRET_LATERAL_FF; @@ -105,8 +105,7 @@ public void resetPoseFromAutoStartPose(AutonomousChooser autoChooser) { * The new (field-centric) pose. */ public void resetPose(Pose2d newPose) { - this.startingPose = newPose; - this.gyroAdjustmentAngle = startingPose.getRotation().minus(getGyroscopeYaw()); + this.gyroAdjustmentAngle = newPose.getRotation().minus(getGyroscopeYaw()); } public double getDistance() { @@ -267,12 +266,11 @@ public Pose2d estimateRobotPoseFromVisionGyro() { return new Pose2d(); } double targetDistance = getDistance(); - Rotation2d targetYaw = Rotation2d.fromDegrees(-getTargetYaw()); + Rotation2d targetYaw = Rotation2d.fromDegrees(-getVisionYaw()); Translation2d targetPosition = FIELD_CENTRIC_HUB_CENTER; Rotation2d robotAngle = getFieldCentricRobotAngle(); Rotation2d robotToCameraAngle = getTurretAngle(); Translation2d robotCentricCameraPosition = ROBOT_CENTRIC_TURRET_CENTER - .rotateBy(new Rotation2d(Math.PI / 2)) .plus(new Translation2d(TURRET_CENTER_TO_LIMELIGHT_DISTANCE, robotToCameraAngle)); return VisionUtil.estimateRobotPose(targetPosition, targetDistance, targetYaw, robotAngle, robotToCameraAngle, robotCentricCameraPosition); diff --git a/src/main/java/frc/team2412/robot/util/VisionUtil.java b/src/main/java/frc/team2412/robot/util/VisionUtil.java index 6c99cbf5..739156df 100644 --- a/src/main/java/frc/team2412/robot/util/VisionUtil.java +++ b/src/main/java/frc/team2412/robot/util/VisionUtil.java @@ -77,8 +77,6 @@ public static Pose2d estimateRobotPose(Translation2d targetPosition, double targ * along {@code robotAngle}, and +Y axis along robotAngle is common. * @param robotToCameraAngle * The CCW-positive angle from the robot to the camera. - * @param robotAngle - * The angle of the robot in the field coordinate system. * @return The transformation from the camera to the robot. */ public static Transform2d calculateCameraToRobot(Translation2d robotCentricCameraPosition, From 12a52a734dbe5b588bda88a31dc03ac8575f38a2 Mon Sep 17 00:00:00 2001 From: KangarooKoala <91924258+KangarooKoala@users.noreply.github.com> Date: Fri, 13 May 2022 16:45:22 -0700 Subject: [PATCH 9/9] Improve doc comments and correct naming --- src/main/java/frc/team2412/robot/util/VisionUtil.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/VisionUtil.java b/src/main/java/frc/team2412/robot/util/VisionUtil.java index 739156df..1dbab18e 100644 --- a/src/main/java/frc/team2412/robot/util/VisionUtil.java +++ b/src/main/java/frc/team2412/robot/util/VisionUtil.java @@ -74,7 +74,7 @@ public static Pose2d estimateRobotPose(Translation2d targetPosition, double targ * * @param robotCentricCameraPosition * The position of the camera relative to the robot center. Note that the +X axis must be - * along {@code robotAngle}, and +Y axis along robotAngle is common. + * along {@code robotAngle}, and +Y axis along {@code robotAngle} is common. * @param robotToCameraAngle * The CCW-positive angle from the robot to the camera. * @return The transformation from the camera to the robot. @@ -100,7 +100,7 @@ public static Transform2d calculateCameraToRobot(Translation2d robotCentricCamer * The CCW-positive angle from the robot to the camera. * @param robotCentricCameraPosition * The position of the camera relative to the robot center. Note that the +X axis must be - * along {@code robotAngle}, and +Y axis along robotAngle is common. + * along {@code robotAngle}, and +Y axis along {@code robotAngle} is common. * @return The robot pose in the field coordinate system. */ public static Pose2d estimateRobotPose(Translation2d targetPosition, double targetDistance, Rotation2d targetYaw, @@ -108,7 +108,7 @@ public static Pose2d estimateRobotPose(Translation2d targetPosition, double targ Translation2d cameraToTarget = new Translation2d(targetDistance, robotAngle.plus(robotToCameraAngle).plus(targetYaw)); Translation2d cameraPosition = targetPosition.minus(cameraToTarget); - Translation2d cameraToRobot = robotCentricCameraPosition.rotateBy(robotAngle); - return new Pose2d(cameraPosition.minus(cameraToRobot), robotAngle); + Translation2d robotToCamera = robotCentricCameraPosition.rotateBy(robotAngle); + return new Pose2d(cameraPosition.minus(robotToCamera), robotAngle); } }