Runs apriltag pose fusion while robot is disabled, but not during auton/teleop by default

Use the FusePose Command to update the pose estimate from vision when its actually wanted.
If it only works well while the robot is still, can just run it during scoring, for instance.

This should give flexibility

Signed-off-by: Eric Ward <ezeward4@gmail.com>

Author: bathtubed

src/main/cpp/Robot.cpp

@@ -10,7 +10,7 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <frc2/command/CommandScheduler.h>
 
-void Robot::RobotInit() {}
+void Robot::RobotInit() { m_disabledCommand = m_container.FusePose(); }
 
 void Robot::DriverStationConnected() {}
 
@@ -85,7 +85,15 @@ void Robot::RobotPeriodic() {
  * can use it to reset any subsystem information you want to clear when the
  * robot is disabled.
  */
-void Robot::DisabledInit() {}
+void Robot::DisabledInit() {
+  if (m_disabledCommand)
+    m_disabledCommand->Schedule();
+}
+
+void Robot::DisabledExit() {
+  if (m_disabledCommand)
+    m_disabledCommand->Cancel();
+}
 
 void Robot::DisabledPeriodic() {}
 

src/main/include/Robot.h

@@ -17,6 +17,7 @@ class Robot : public frc::TimedRobot {
   void DriverStationConnected() override;
   void RobotPeriodic() override;
   void DisabledInit() override;
+  void DisabledExit() override;
   void DisabledPeriodic() override;
   void AutonomousInit() override;
   void AutonomousPeriodic() override;