feeding vision into pose estimation

src/main/deploy/pathplanner/autos/One Coral Left.auto

@@ -7,7 +7,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Start to 60 Right"
+            "pathName": "Start to 30 Right"
           }
         },
         {

src/main/deploy/pathplanner/autos/Two Coral Left.auto

@@ -7,7 +7,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Start to 60 Right"
+            "pathName": "Start to 30 Right"
           }
         },
         {
@@ -19,7 +19,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "60 Right to HP"
+            "pathName": "30 Right to HP"
           }
         },
         {
@@ -31,7 +31,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "HP to 300 Left"
+            "pathName": "HP to 330 Left"
           }
         },
         {

src/main/deploy/pathplanner/paths/150 Right to HP.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.289041095890411,
+        "y": 2.9732020547945206
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.950171232876712,
+        "y": 1.6208904109589037
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.4424657534246574,
+        "y": 0.7944777397260271
+      },
+      "prevControl": {
+        "x": 2.389083904109589,
+        "y": 2.5374571917808213
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 54.162347045721745
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 120.96375653207336
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Right Start to 150 Right.path

@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.602996575342465,
+        "y": 2.0115582191780823
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.491095890410959,
+        "y": 1.9965325342465756
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.289041095890411,
+        "y": 2.9882277397260277
+      },
+      "prevControl": {
+        "x": 6.130479452054795,
+        "y": 2.11673801369863
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.7142857142857124,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 120.25643716352937
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -9,6 +9,7 @@ import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.ManipulatorPivot;
+import frc.robot.subsystems.Vision;
 import frc.robot.subsystems.ClimberPivot;
 import frc.robot.subsystems.ClimberRollers;
 import frc.robot.subsystems.Drivetrain;
@@ -19,6 +20,9 @@ import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -45,13 +49,15 @@ public class RobotContainer {
 	private CommandXboxController operator;
 
 	private SendableChooser<Command> autoChooser;
+	private Vision vision;
 
 	public RobotContainer() {
 		climberPivot = new ClimberPivot();
 
 		climberRollers = new ClimberRollers();
 
-		drivetrain = new Drivetrain();
+		vision = new Vision(drivetrain::getGyroValue);
+		drivetrain = new Drivetrain(vision);
 
 		elevator = new Elevator();
 		//elevator = new ElevatorSysID();
@@ -253,7 +259,7 @@ public class RobotContainer {
 		sensorTab.addDouble("ElevMotor1", elevator::getMotor1);
 
 		sensorTab.addDouble("ElevMotor2", elevator::getMotor2);
-  }
+ 	}
 
 	public Command getAutonomousCommand() {
 		return autoChooser.getSelected();

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

@@ -21,6 +21,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AutoConstants;
@@ -35,13 +36,15 @@ public class Drivetrain extends SubsystemBase {
     protected MAXSwerveModule m_rearRight;
 
     // The gyro sensor
-    private AHRS ahrs;
+    private AHRS gyro;
 
     // Odometry class for tracking robot pose
     private SwerveDrivePoseEstimator m_estimator;
 
+    private Vision vision;
+
     /** Creates a new DriveSubsystem. */
-    public Drivetrain() {
+    public Drivetrain(Vision vision) {
         m_frontLeft = new MAXSwerveModule(
             DrivetrainConstants.kFrontLeftDrivingCanId,
             DrivetrainConstants.kFrontLeftTurningCanId,
@@ -66,11 +69,11 @@ public class Drivetrain extends SubsystemBase {
             DrivetrainConstants.kBackRightChassisAngularOffset
         );
 
-        ahrs = new AHRS(NavXComType.kMXP_SPI);
+        gyro = new AHRS(NavXComType.kMXP_SPI);
 
         m_estimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.kDriveKinematics,
-            Rotation2d.fromDegrees(ahrs.getAngle()),
+            Rotation2d.fromDegrees(gyro.getAngle()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
                 m_frontRight.getPosition(),
@@ -109,6 +112,24 @@ public class Drivetrain extends SubsystemBase {
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
         });
+
+
+        // if the detected tags match your alliances reef tags use their pose estimates 
+        if(vision.getOrangeClosestTag() >= 6 || vision.getOrangeClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
+            m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
+            
+        }else if(vision.getOrangeClosestTag() >= 17 || vision.getOrangeClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
+            m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp());
+        }
+
+        if(vision.getBlackClosestTag() >= 6 || vision.getBlackClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){
+            m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp()); 
+        }else if(vision.getBlackClosestTag() >= 17 || vision.getBlackClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){
+            m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp());
+        }
+        
+        
+        
     }
 
     public ChassisSpeeds getCurrentChassisSpeeds() {
@@ -228,11 +249,11 @@ public class Drivetrain extends SubsystemBase {
 
     /** Zeroes the heading of the robot. */
     public void zeroHeading() {
-        ahrs.reset();
+        gyro.reset();
     }
 
     public double getGyroValue() {
-        return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
+        return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
     }
 
     /**
@@ -250,7 +271,7 @@ public class Drivetrain extends SubsystemBase {
      * @return The turn rate of the robot, in degrees per second
      */
     public double getTurnRate() {
-        return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
+        return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
 
     public void addVisionMeasurement(Pose2d pose, double timestamp){

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

@@ -74,7 +74,6 @@ public class Vision{
     }
 
     public Pose2d getBlackGlobalPose(){
-        
         return relativeToGlobalPose2d(getBlackClosestTag(), 
             new Translation2d(getBlackRelativeX(), getBlackRelativeY()), 
             new Rotation2d(gyroAngle.getAsDouble()));
@@ -109,9 +108,8 @@ public class Vision{
     }
 
     public Pose2d getOrangeGlobalPose(){
-        
+        return relativeToGlobalPose2d(getOrangeClosestTag(), 
-        return relativeToGlobalPose2d(getBlackClosestTag(), 
+            new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()), 
-            new Translation2d(getBlackRelativeX(), getBlackRelativeY()), 
             new Rotation2d(gyroAngle.getAsDouble()));
     }