Create calculatePoseFromTarget

[ACat701]

Reasons why this might not work:

• result.getTimestampSeconds(); is not synced with Timer.getFPGATimestamp();
• I'm not smart
• Also, chasing the target won't work if we're constantly resetting our position based on the target we're chasing, so remember that ig

[IanSzalai]

I'm not really sure how result.getTimestampSeconds() differs from Timer.getFPGATimestamp() but worse case we can purely use the former and it should be fine.

[IanSzalai]

Ideally, the vision subsystem has the bulk of the vision processing code, rather than a command. This would distill into some getRobotPose() (or similarly named) method that is used in the UpdatePoseEstimator command. To my understanding, what complicates this is that if there is no valid pose, you don't want to add a vision measurement. I see two ways around this.

1. Simple include another getter method in the vision system that will simply return true if the vision data is good to use.

   if (subVision.isPoseValid()) {
   addVisionMeasurement(subVision.getRobotPose());
   }
   // method names for example

2. Return null from subVision.getRobotPose() if the data isn't good to use. Then you could either check for null in UpdatePoseEstimator or in subDrivetrain.addVisionMeasurement() eg

   public void addVisionMeasurement(Pose2d visionRobotPose, double timestampSeconds) {
   if (visionRobotPose != null) {
   poseEstimator.addVisionMeasurement(
       visionRobotPose,
       timestampSeconds);
   }
   }

   or

   // execute method of UpdatePoseEstimator
   public void execute() {
   subDrivetrain.updatePoseEstimator();
   if (subVision.getRobotPose() != null) {
   subDrivetrain.addVisionMeasurement(subVision.getRobotPose(), subVision.getRobotPoseTimestamp());
   }
   }

I may be misunderstanding the problem at hand, please let me know if that's the case.

[IanSzalai]

new changes look good to me. do you think that this structure makes the most sense?

[ACat701]

new changes look good to me. do you think that this structure makes the most sense?

I think so. Combining all of the types of pose estimators into one command but keeping them separate in subsystems seems smart. Not having real positions for each apriltag (ie. every tag is at 0 rn) is going to make the whole process a bit weird, but that sounds like a problem for future me

Just committed changes that should (?) fix the "chasing tag" problem. Correct me if we shouldn't use it or if I did it wrong. If we decide to implement a chasing tag code, I will also add a filter on the specific tag that we use to make its pose invalid.