Move getCollectionTrajectory to pose estimator

src/main/java/frc/robot/RobotContainer.java

@@ -35,6 +35,7 @@
 import frc.robot.subsystems.Manipulator.IRSensorState;
 import frc.robot.subsystems.Manipulator.ManipulatorState;
 import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.subsystems.drivetrain.PoseEstimator;
 import frc.robot.utils.GlobalDebuggable;
 
 /**
@@ -50,6 +51,7 @@ public class RobotContainer {
             new Funnel());
 
     private final LimeLight m_limeLight = new LimeLight();
+    private final PoseEstimator m_poseEstimator = new PoseEstimator(m_drivetrain, m_limeLight);
 
     private final Compressor m_compressor = new Compressor(PneumaticsModuleType.REVPH);
 
@@ -95,7 +97,7 @@ public RobotContainer() {
      * Use this method to define your trigger->command mappings.
      */
     private void configureBindings() {
-        _driverController.cross().onTrue(_autoFactory.createfollow(m_limeLight.getCollectionTrajectory()));
+        _driverController.cross().onTrue(_autoFactory.createfollow(m_poseEstimator.getCollectionTrajectory()));
 
         _driverController.options().onTrue(
                 new InstantCommand(() -> _fieldRelative = !_fieldRelative)); // toggle field

src/main/java/frc/robot/constants/LimelightConstants.java

@@ -1,9 +1,6 @@
 package frc.robot.constants;
 
-import com.pathplanner.lib.PathPoint;
-
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import frc.robot.motors.PIDFGains;
 
 /**
@@ -29,14 +26,4 @@ public class LimelightConstants {
 
     public static final double pipeLineRetroReflective = 0;
     public static final double pipeLineAprilTags = 1;
-
-    // TODO: these values are only approximation from PP
-    public static final Translation2d collectionTranslation = new Translation2d(14.0, 7.5);
-    public static final Rotation2d collectionHolonomicRotation = Rotation2d.fromDegrees(90);
-    public static final Rotation2d collectionHeading = Rotation2d.fromDegrees(90);
-    public static final PathPoint collectionPathPoint = new PathPoint(
-            collectionTranslation,
-            collectionHeading,
-            collectionHolonomicRotation);
-
 }

src/main/java/frc/robot/subsystems/LimeLight.java

@@ -4,11 +4,6 @@
 
 package frc.robot.subsystems;
 
-import com.pathplanner.lib.PathConstraints;
-import com.pathplanner.lib.PathPlanner;
-import com.pathplanner.lib.PathPlannerTrajectory;
-import com.pathplanner.lib.PathPoint;
-
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -19,7 +14,6 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.constants.AutonomousConstants;
 import frc.robot.constants.LimelightConstants;
 
 public class LimeLight extends SubsystemBase {
@@ -100,21 +94,4 @@ public BotPose getBotPose() {
                 Timer.getFPGATimestamp() - (poseComponents[6] / 1000.0));
 
     }
-
-    public PathPlannerTrajectory getCollectionTrajectory() {
-        Pose2d botPose = getBotPose().getPose();
-        if (botPose == null) {
-            return new PathPlannerTrajectory();
-        }
-
-        // heading straight to the collection using vector subtraction
-        Rotation2d startingHeading = LimelightConstants.collectionTranslation
-                .minus(botPose.getTranslation()).getAngle();
-
-        return PathPlanner.generatePath(
-                new PathConstraints(AutonomousConstants.kMaxSpeedMetersPerSecond,
-                        AutonomousConstants.kMaxAccelerationMetersPerSecondSquared),
-                new PathPoint(botPose.getTranslation(), startingHeading, botPose.getRotation()),
-                LimelightConstants.collectionPathPoint);
-    }
 }

src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java

@@ -164,6 +164,15 @@ public SwerveModulePosition[] getSwerveModulePositions() {
         return swerveModulePositions;
     }
 
+    public ChassisSpeeds getChassisSpeeds() {
+        SwerveModuleState[] states = new SwerveModuleState[this._modules.length];
+
+        for (int i = 0; i < states.length; i++) {
+            states[i] = this._modules[i].getState();
+        }
+        return DrivetrainConstants.kinematics.toChassisSpeeds(states);
+    }
+
     public void restartControllers() {
         vision_xController.reset();
         vision_yController.reset();

src/main/java/frc/robot/subsystems/drivetrain/PoseEstimator.java

@@ -2,11 +2,17 @@
 
 import java.util.Map;
 
+import com.pathplanner.lib.PathConstraints;
+import com.pathplanner.lib.PathPlanner;
+import com.pathplanner.lib.PathPlannerTrajectory;
+import com.pathplanner.lib.PathPoint;
+
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 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.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Timer;
@@ -15,6 +21,7 @@
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.AutonomousConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.subsystems.LimeLight;
 
@@ -24,7 +31,16 @@ public class PoseEstimator extends SubsystemBase {
     private final Drivetrain drivetrain;
 
     private double lastVisionTimestamp = -1;
-    private final Pose2d robotAtSSS = new Pose2d(14.2, 7.6, Rotation2d.fromDegrees(90));
+
+    // TODO: these values are only approximation from PP
+    private static final Translation2d collectionTranslation = new Translation2d(14.0, 7.5);
+    private static final Rotation2d collectionHolonomicRotation = Rotation2d.fromDegrees(90);
+    private static final Rotation2d collectionHeading = Rotation2d.fromDegrees(90);
+    private static final PathPoint collectionPathPoint = new PathPoint(
+            collectionTranslation,
+            collectionHeading,
+            collectionHolonomicRotation);
+
     private final double xTolerance = 0.2; // in meters
     private final double yTolerance = 0.1; // in meters
     private final double tTolerance = 10; // in degrees
@@ -74,22 +90,22 @@ public PoseEstimator(Drivetrain drivetrain, LimeLight limeLight) {
         tab.addBoolean("X at SSS", this::xAtSSS).withPosition(0, 2).withSize(1, 1);
         tab.addNumber("X error", this::xError).withWidget(BuiltInWidgets.kNumberBar)
                 .withProperties(Map.of(
-                        "min", robotAtSSS.getX() - (xTolerance * 5),
-                        "max", robotAtSSS.getX() + (xTolerance * 5)))
+                        "min", collectionTranslation.getX() - (xTolerance * 5),
+                        "max", collectionTranslation.getX() + (xTolerance * 5)))
                 .withPosition(1, 2)
                 .withSize(1, 1);
         tab.addBoolean("Y at SSS", this::yAtSSS).withPosition(0, 3).withSize(1, 1);
         tab.addNumber("Y error", this::yError).withWidget(BuiltInWidgets.kNumberBar)
                 .withProperties(Map.of(
-                        "min", robotAtSSS.getY() - (xTolerance * 5),
-                        "max", robotAtSSS.getY() + (xTolerance * 5)))
+                        "min", collectionTranslation.getY() - (xTolerance * 5),
+                        "max", collectionTranslation.getY() + (xTolerance * 5)))
                 .withPosition(1, 3)
                 .withSize(1, 1);
         tab.addBoolean("T at SSS", this::tAtSSS).withPosition(0, 4).withSize(1, 1);
         tab.addNumber("T error", this::tError).withWidget(BuiltInWidgets.kNumberBar)
                 .withProperties(Map.of(
-                        "min", robotAtSSS.getRotation().getDegrees() - (tTolerance * 5),
-                        "max", robotAtSSS.getRotation().getDegrees() + (tTolerance * 5)))
+                        "min", collectionHolonomicRotation.getDegrees() - (tTolerance * 5),
+                        "max", collectionHolonomicRotation.getDegrees() + (tTolerance * 5)))
                 .withPosition(1, 4)
                 .withSize(1, 1);
         tab.add("Field", field2d).withPosition(2, 0).withSize(6, 4);
@@ -116,15 +132,15 @@ private boolean isVisionSynced() {
     }
 
     private double xError() {
-        return getCurrentPose().minus(robotAtSSS).getX();
+        return getCurrentPose().getTranslation().minus(collectionTranslation).getX();
     }
 
     private double yError() {
-        return getCurrentPose().minus(robotAtSSS).getY();
+        return getCurrentPose().getTranslation().minus(collectionTranslation).getY();
     }
 
     private double tError() {
-        return getCurrentPose().minus(robotAtSSS).getRotation().getDegrees();
+        return getCurrentPose().getRotation().minus(collectionHolonomicRotation).getDegrees();
     }
 
     private boolean xAtSSS() {
@@ -174,4 +190,17 @@ public void resetFieldPosition() {
         setCurrentPose(new Pose2d());
     }
 
+    public PathPlannerTrajectory getCollectionTrajectory() {
+        var pose = getCurrentPose();
+        var speeds = drivetrain.getChassisSpeeds();
+
+        var velocity = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+        var heading = Rotation2d.fromRadians(Math.atan2(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond));
+        return PathPlanner.generatePath(
+                new PathConstraints(AutonomousConstants.kMaxSpeedMetersPerSecond,
+                        AutonomousConstants.kMaxAccelerationMetersPerSecondSquared),
+                new PathPoint(pose.getTranslation(), heading.plus(pose.getRotation()), pose.getRotation(), velocity),
+                collectionPathPoint);
+    }
+
 }