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 <units/angle.h>
#include <units/angular_velocity.h>
#include <units/length.h>
#include <wpi/DataLog.h>
 
#include <stop_token>
#include <string>
#include <thread>
 
#include "argos_lib/config/config_types.h"
#include "argos_lib/general/interpolation.h"
#include "argos_lib/general/nt_subscriber.h"
#include "argos_lib/general/odometry_aim.h"
#include "constants/interpolation_maps.h"
#include "networktables/NetworkTable.h"
#include "networktables/NetworkTableEntry.h"
#include "networktables/NetworkTableInstance.h"
#include "networktables/NetworkTableValue.h"
#include "shooter_subsystem.h"
#include "swerve_drive_subsystem.h"
 
enum class whichCamera { PRIMARY_CAMERA = 0, SECONDARY_CAMERA };
 
class LimelightTarget {
 private:
  frc::Pose3d m_robotPose;              
  frc::Pose3d m_robotPoseWPI;           
  frc::Pose3d m_targetPose;             
  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;
  frc::LinearFilter<units::meter_t> m_zFilter;
  bool m_resetFilterFlag;
  double m_tid;  
 
 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_zFilter{frc::LinearFilter<units::meter_t>::SinglePoleIIR(0.01, 0.02_s)}
      , m_resetFilterFlag{false} {}
 
  struct tValues {
    frc::Pose3d robotPose;              
    frc::Pose3d robotPoseWPI;           
    frc::Pose3d tagPose;                
    bool hasTargets;                    
    units::degree_t m_pitch;            
    units::degree_t m_yaw;              
    double m_area;                      
    double tagID;                       
    units::millisecond_t totalLatency;  
  };
 
  tValues GetTarget(bool filter, std::string cameraName);
 
  bool HasTarget();
 
  void ResetFilters(std::string cameraName);
 
  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,
                  ShooterSubsystem* pShooterSubsytem);
 
  enum class InterpolationMode { LinearInterpolation, Polynomial, Trig };
 
  [[nodiscard]] std::optional<units::inch_t> GetDistanceToSpeaker();
 
  [[nodiscard]] std::optional<units::degree_t> GetOrientationToSpeaker();
 
  [[nodiscard]] std::optional<units::inch_t> GetCalculatedDistanceToSpeaker();
 
  [[nodiscard]] LimelightTarget::tValues GetPrimaryCameraTargetValues();
 
  [[nodiscard]] LimelightTarget::tValues GetSecondaryCameraTargetValues();
 
  [[nodiscard]] std::optional<units::degree_t> GetHorizontalOffsetToTarget();
 
  void SetPipeline(uint16_t tag);
 
  void RequestFilterReset();
 
  void SetAimWhileMove(bool val);
  [[nodiscard]] bool IsAimWhileMoveActive();
 
  void SetOdometryAiming(bool val);
  [[nodiscard]] bool IsOdometryAimingActive();
 
  void SetEnableStaticRotation(bool val);
  [[nodiscard]] bool IsStaticRotationEnabled();
 
  void Periodic() override;
 
  void Disable();
 
  [[nodiscard]] units::degree_t getShooterAngle(units::inch_t distance, const InterpolationMode mode);
  [[nodiscard]] std::optional<units::degree_t> getShooterAngle();
 
  [[nodiscard]] std::optional<units::degree_t> getShooterOffset();
 
  [[nodiscard]] units::degree_t getFeederAngle();
 
  [[nodiscard]] std::optional<units::degree_t> getFeederOffset();
 
  [[nodiscard]] std::optional<units::degree_t> getShooterAngleWithInertia(double medialSpeedPct);
 
  [[nodiscard]] std::optional<double> getRotationSpeedWithInertia(double lateralSpeedPct);
 
  [[nodiscard]] std::optional<units::degree_t> getFeederAngleWithInertia(double medialSpeedPct);
 
  [[nodiscard]] std::optional<double> getFeedOffsetWithInertia(double lateralSpeedPct);
 
  [[nodiscard]] units::angular_velocity::revolutions_per_minute_t getShooterSpeed(const units::inch_t distance,
                                                                                  const InterpolationMode mode) const;
  [[nodiscard]] std::optional<units::angular_velocity::revolutions_per_minute_t> getShooterSpeed();
 
  [[nodiscard]] std::optional<units::inch_t> GetDistanceToTrap();
 
  [[nodiscard]] std::optional<units::inch_t> GetDistanceToStageCenter();
 
  [[nodiscard]] std::optional<units::degree_t> GetHorizontalOffsetToTrap();
 
  [[nodiscard]] std::optional<units::degree_t> GetOrientationToTrap();
 
  [[nodiscard]] std::optional<whichCamera> getWhichCamera(bool forFeeder = false);
 
  [[nodiscard]] std::optional<LimelightTarget::tValues> GetSeeingCamera(bool forFeeder = false);
 
 private:
  constexpr static char primaryCameraTableName[12]{"/limelight"};
  constexpr static char secondaryCameraTableName[18]{"/limelight-front"};
 
  // 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;     
  ShooterSubsystem* m_pShooterSubsystem;       
  LimelightTarget::tValues m_oldTargetValues;  
  bool m_usePolynomial;                        
  bool m_useTrigonometry;                      
  bool m_isAimWhileMoveActive;                 
  bool m_enableStaticRotation;                 
  bool m_isOdometryAimingActive;               
  argos_lib::InterpolationMap<decltype(shooterRange::shooterAngle.front().inVal),
                              shooterRange::shooterAngle.size(),
                              decltype(shooterRange::shooterAngle.front().outVal)>
      m_shooterAngleMap;  
  argos_lib::InterpolationMap<decltype(shooterRange::shooterSpeed.front().inVal),
                              shooterRange::shooterSpeed.size(),
                              decltype(shooterRange::shooterSpeed.front().outVal)>
      m_shooterSpeedMap;  
  argos_lib::InterpolationMap<decltype(shooterRange::feederAngle.front().inVal),
                              shooterRange::feederAngle.size(),
                              decltype(shooterRange::feederAngle.front().outVal)>
      m_feederAngleMap;  
  argos_lib::NTSubscriber
      m_primaryCameraFrameUpdateSubscriber;  
  argos_lib::NTSubscriber
      m_secondaryCameraFrameUpdateSubscriber;  
  std::jthread m_yawUpdateThread;
 
  wpi::log::StructLogEntry<frc::Pose2d> m_frontCameraMegaTag2PoseLogger;
  wpi::log::StructLogEntry<frc::Pose2d> m_rearCameraMegaTag2PoseLogger;
 
  void UpdateYaw(std::stop_token stopToken);
};