vision_subsystem.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <frc/filter/LinearFilter.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Transform3d.h>
#include <frc2/command/SubsystemBase.h>
 
#include "argos_lib/config/config_types.h"
#include "networktables/NetworkTable.h"
#include "networktables/NetworkTableEntry.h"
#include "networktables/NetworkTableInstance.h"
#include "networktables/NetworkTableValue.h"
#include "swerve_drive_subsystem.h"
 
class LimelightTarget {
 private:
  frc::Pose3d m_robotPose;              
  frc::Pose3d m_robotPoseWPI;           
  frc::Pose3d m_robotPoseTagSpace;      
  bool m_hasTargets;                    
  units::degree_t m_pitch;              
  units::degree_t m_yaw;                
  double m_area;                        
  units::millisecond_t m_totalLatency;  
  frc::LinearFilter<units::degree_t> m_txFilter;
  frc::LinearFilter<units::degree_t> m_tyFilter;
  bool m_resetFilterFlag;
 
 public:
  LimelightTarget()
      : m_txFilter{frc::LinearFilter<units::degree_t>::SinglePoleIIR(0.01, 0.02_s)}
      , m_tyFilter{frc::LinearFilter<units::degree_t>::SinglePoleIIR(0.01, 0.02_s)}
      , m_resetFilterFlag{false} {}
 
  struct tValues {
    frc::Pose3d robotPose;              
    frc::Pose3d robotPoseWPI;           
    frc::Pose3d robotPoseTagSpace;      
    bool hasTargets;                    
    units::degree_t m_pitch;            
    units::degree_t m_yaw;              
    double m_area;                      
    units::millisecond_t totalLatency;  
  };
 
  tValues GetTarget(bool filter);
 
  bool HasTarget();
 
  void ResetFilters();
 
  void ResetOnNextTarget();
};
 
class CameraInterface {
 public:
  CameraInterface();
 
  LimelightTarget m_target;  
 
  std::optional<LimelightTarget> GetClosestTarget();
 
  void SetDriverMode(bool mode);
 
  void RequestTargetFilterReset();
};
 
class VisionSubsystem : public frc2::SubsystemBase {
 public:
  VisionSubsystem(const argos_lib::RobotInstance instance, SwerveDriveSubsystem* pDriveSubsystem);
 
  std::optional<units::degree_t> GetOffsetToTarget(LimelightTarget::tValues target);
 
  LimelightTarget::tValues GetCameraTargetValues();
 
  LimelightTarget::tValues m_oldTargetValues;
 
  std::optional<units::degree_t> GetHorizontalOffsetToTarget();
 
  std::optional<units::inch_t> GetDistanceToPoleTape();
 
  void SetReflectiveVisionMode(bool mode);
 
  bool AimToPlaceCone();
 
  void RequestFilterReset();
 
  void Periodic() override;
 
  void Disable();
 
 private:
  // Components (e.g. motor controllers and sensors) should generally be
  // declared private and exposed only through public methods.
  CameraInterface m_cameraInterface;  
 
  argos_lib::RobotInstance
      m_instance;  
  SwerveDriveSubsystem* m_pDriveSubsystem;  
};