photon pose estimator work

src/main/java/frc/robot/commands/swerve/SwerveAlignBasic2OdoBased

@@ -0,0 +1,169 @@
+// FRC2106 Junkyard Dogs - Swerve Drive Base Code
+
+package frc.robot.commands.swerve;
+import frc.robot.subsystems.SwerveSubsystem;
+import frc.robot.subsystems.VisionSubsystem;
+import frc.robot.util.Constants.DriveConstants;
+
+import java.sql.Driver;
+import java.util.function.Supplier;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class SwerveAlignBasic extends CommandBase {
+
+
+  private boolean finished;
+  SwerveSubsystem swerveSubsystem;
+  VisionSubsystem visionSubsystem;
+  Supplier<Boolean> switchOverride;
+  Supplier<Double> headingFunction, setpointFunction;
+
+  private PIDController vXController, vYController;
+  private PIDController thetaController;
+
+  private double vX;
+  private double vY;
+  private double vT;
+
+  private double initalHeading;
+
+  private Pose2d startingPose;
+  private Transform3d visionTransform;
+
+  // Command constructor
+  public SwerveAlignBasic(SwerveSubsystem swerveSubsystem, VisionSubsystem visionSubsystem,
+   Supplier<Double> headingFunction, Supplier<Boolean> switchOverride, Supplier<Double> setpointFunction){
+
+    addRequirements(swerveSubsystem, visionSubsystem);
+
+    this.swerveSubsystem = swerveSubsystem;
+    this.visionSubsystem = visionSubsystem;
+    this.switchOverride = switchOverride;
+    this.headingFunction = headingFunction;
+    this.setpointFunction = setpointFunction;
+
+    initalHeading = headingFunction.get();
+
+    vXController = new PIDController(1.8, 0.005, 0);
+    vYController = new PIDController(1.4, 0.005, 0);
+    thetaController = new PIDController(0.1, 0, 0);
+    thetaController.enableContinuousInput(0, 360);
+
+    vX = 0;
+    vY = 0;
+    vT = 0;
+
+    finished = false;
+
+  }
+
+  @Override
+  public void initialize() {
+    finished = false;
+
+    initalHeading = headingFunction.get();
+    startingPose = swerveSubsystem.getOdometryMeters();
+
+    if(visionSubsystem.hasTargets()){
+      visionTransform = visionSubsystem.getTargetTransform();
+      SmartDashboard.putString("Vision Start Transform", visionSubsystem.getTargetTransform().toString());
+    }else{
+      finished = true;
+    }
+
+  }
+
+  @Override
+  public void execute(){
+
+//- 0.28575
+    try
+    {
+
+      double xError = ( visionTransform.getY() + (swerveSubsystem.getOdometryMeters().getX() - startingPose.getX()));
+      double yError = ( visionTransform.getX() + (swerveSubsystem.getOdometryMeters().getY() - startingPose.getY()));
+      //double tError = ( (-visionTransform.getRotation().getAngle() * 360) / (2*Math.PI) - (swerveSubsystem.getRobotDegrees())) + 180;
+ //0.28575
+
+         vX = vXController.calculate(xError, -0.28575); // X-Axis PID
+         vY = vYController.calculate(yError, 0.5); // Y-Axis PID
+         vT = thetaController.calculate( Math.toRadians(swerveSubsystem.getRobotDegrees()), 0); // Rotation PID
+
+       SmartDashboard.putNumber("vX", vX);
+       SmartDashboard.putNumber("vY", vY);
+
+       SmartDashboard.putNumber("X Error", xError);
+       SmartDashboard.putNumber("Y Error", yError);
+
+
+      if(switchOverride.get() == false){
+        finished = true;
+      }
+
+      // Create chassis speeds
+      ChassisSpeeds chassisSpeeds;
+
+      // Apply chassis speeds with desired velocities
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(vX, vY, vT, swerveSubsystem.getRotation2d());
+      //chassisSpeeds = new ChassisSpeeds(vX, vY, vT);
+      //chassisSpeeds = new ChassisSpeeds(0.5,0,0);
+
+      // Create states array
+      SwerveModuleState[] moduleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds);
+
+      // Move swerve modules
+      swerveSubsystem.setModuleStates(moduleStates);
+    }
+    catch (Exception e)
+    {
+      e.printStackTrace();
+      DriverStation.reportError("E  xeption", true);
+      finished = true;
+    }
+
+  }
+
+  // Stop all module motor movement when command ends
+  @Override
+  public void end(boolean interrupted){swerveSubsystem.stopModules();}
+
+  @Override
+  public boolean isFinished(){return finished;}
+
+
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

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

@@ -11,10 +11,12 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.smartdashboard.field2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -87,7 +89,7 @@ public SwerveModulePosition[] getModulePositions(){
 
   // BROKEN FOR 2023
   // Create odometer for error correction
-  private SwerveDriveOdometry odometer;
+  private drivePoseEstimator;
   // Swerve subsystem constructor
   public SwerveSubsystem() {
 
@@ -104,9 +106,14 @@ public SwerveSubsystem() {
         }
     }).start();
 
+    /*
     odometer = new SwerveDriveOdometry(DriveConstants.kDriveKinematics, 
     new Rotation2d(0), getModulePositions());
-
+    */
+    drivePoseEstimator = new SwerveDrivePoseEstimator(kDriveKinematics, getHeading, getModulePositions, new Pose2d(0,0,Rotation2d.fromDegrees(getHeading())) );
+
+    private final field2d m_field = new field2d();
+    SmartDashboard.putData("Field",m_field);
   }
 
   // Reset gyro heading 
@@ -196,12 +203,17 @@ public double getRobotDegrees(){
   public void periodic(){
 
     // Periodicly update odometer for it to caculate position
-    odometer.update(getRotation2d(), getModulePositions());
-
+    //odometer.update(getRotation2d(), getModulePositions());
+    drivePoseEstimator.update(Rotation2d.fromDegrees(gyro.getAngle()),getModulePositions())
     // Odometry
     SmartDashboard.putNumber("Heading", getHeading());
    SmartDashboard.putString("Field Location", getPose().getTranslation().toString());
    SmartDashboard.putNumber("ROBOT DEGREES NAVX", getRobotDegrees());
+
+
+   //update the odometry with the measurements from vision if there is a vision target and its close to the current estimated position TODO: make this only add the vision measurement if close to previous estimates
+   if(VisionSubsystem.hasTargets()){
+    drivePoseEstimator.addVisionMeasurement(VisionSubsystem.getEstimatedGlobalPose(), Timer.getFPGATimestamp() )}
    // SmartDashboard.putNumber("R2D deg", getGyroDegrees());
 
     // Update robot monitor
@@ -212,6 +224,9 @@ public void periodic(){
     backLeft.update();
     backRight.update();
 
+    //put estimated robot pose on smartdashboard
+    m_field.setRobotPose(drivePoseEstimator.getPoseMeters());
+
   }
 
 

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

@@ -10,14 +10,17 @@
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.PhotonUtils;
 import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFieldLayout.loadFromResource;  
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
@@ -46,6 +49,8 @@ public class VisionSubsystem extends SubsystemBase{
     // Pose estimator
     private PhotonPoseEstimator poseEstimator;
 
+    private Transform3d robotToCam; 
+
     // Subsystem Constructor
     public VisionSubsystem(){
 
@@ -60,6 +65,12 @@ public VisionSubsystem(){
 
         visionCamera.setLED(VisionLEDMode.kDefault);
 
+        AprilTagFieldLayout aprilTagFieldLayout = new AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile);
+
+        robotToCam = new Transform3d(new Translation3d(/*distance forward of center*/0.216535,/* distance left of center*/0.28575 ,/*distance above floor*/0.3900163904 ), new Rotation3d(0,0,0));
+
+        poseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP , visionCamera, robotToCam);
+
     }
 
     public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {