Closed ACat701 closed 1 year ago
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.
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.
if (subVision.isPoseValid()) {
addVisionMeasurement(subVision.getRobotPose());
}
// method names for example
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.
new changes look good to me. do you think that this structure makes the most sense?
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.
Reasons why this might not work: