Pose estimation v2

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();
         }

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,14 @@ public Vector2 getGyroscopeXY() {
         return Vector2.ZERO;
     }
 
-    public Rotation2 getGyroscopeUnadjustedAngle() {
+    /**
+     * Returns the unadjusted gyroscope angle.
+     *
+     * @return Unadjusted gyroscope angle (yaw, positive rotation is clockwise)
+     */
+    public Rotation2d getGyroscopeUnadjustedAngle() {
         synchronized (sensorLock) {
-            return gyroscope.getUnadjustedAngle();
+            return Rotation2d.fromDegrees(gyroscope.getUnadjustedAngle().toDegrees());
         }
     }
 

src/main/java/frc/team2412/robot/subsystem/TargetLocalizer.java

@@ -1,40 +1,58 @@
 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 frc.team2412.robot.util.autonomous.AutonomousChooser;
 
 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;
+
+/**
+ * Class that combines sensor data from multiple subsystems. <br>
+ *
+ * <p>
+ * Field-centric pose frame of reference:
+ * <ul>
+ * <li>Units are inches</li>
+ * <li>Origin is right corner of alliance wall</li>
+ * <li>Positive X axis points away from alliance wall</li>
+ * <li>Positive Y axis points left from alliance wall</li>
+ * <li>0 rotation points away from alliance wall</li>
+ * <li>Positive rotation is counterclockwise</li>
+ * </ul>
+ * </p>
+ */
 public class TargetLocalizer {
     public static class LocalizerConstants {
         // TODO tune these more
         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 LIMELIGHT_TO_TURRET_CENTER_DISTANCE = -7;
-        public static final Vector2 ROBOT_CENTRIC_TURRET_CENTER = new Vector2(3.93, -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;
     private final ShooterSubsystem shooterSubsystem;
     private final ShooterVisionSubsystem shooterVisionSubsystem;
     private final TimeBasedMedianFilter distanceFilter;
-    private final Rotation2 gyroAdjustmentAngle;
-    private final RigidTransform2 startingPose;
+    private Pose2d startingPose;
+    private Rotation2d gyroAdjustmentAngle;
 
     /**
      * 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.
@@ -49,11 +67,28 @@ public TargetLocalizer(DrivebaseSubsystem drivebaseSubsystem, ShooterSubsystem s
         this.shooterSubsystem = shooterSubsystem;
         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 RigidTransform2(new Vector2(5 * 12, 5 * 12), Rotation2.ZERO);
-        this.gyroAdjustmentAngle = startingPose.rotation
-                .rotateBy(drivebaseSubsystem.getGyroscopeUnadjustedAngle().inverse());
+        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());
     }
 
     public double getDistance() {
@@ -88,8 +123,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();
@@ -98,33 +132,58 @@ 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() {
         // return 0;
         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 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 Rotation2 getFieldCentricRobotAngle() {
-        return drivebaseSubsystem.getGyroscopeUnadjustedAngle().rotateBy(gyroAdjustmentAngle);
+    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 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 +192,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 +201,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() {
@@ -173,90 +226,46 @@ 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}.
+     * Returns the estimated limelight pose according to vision and the gyroscope. <br>
      *
-     * 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).
+     * <p>
+     * If the limelight doesn't have a target, returns {@code Pose2d()}.
+     * </p>
      *
-     * @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 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 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 {@link RigidTransform2#ZERO}.
+     * Returns the estimated robot pose according to vision and the gyroscope. <br>
      *
-     * 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).
+     * <p>
+     * If the limelight doesn't have a target, returns {@code Pose2d()}.
+     * </p>
      *
-     * @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 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());
-    }
-
-    /**
-     * 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}.
-     *
-     * 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.
-     *
-     * @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;
+        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() {

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);
+    }
 }