shooter_subsystem.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <frc2/command/SubsystemBase.h>
 
#include "Constants.h"
#include "argos_lib/config/config_types.h"
#include "argos_lib/general/interpolation.h"
#include "argos_lib/general/nt_motor_pid_tuner.h"
#include "argos_lib/subsystems/swappable_controllers_subsystem.h"
#include "ctre/Phoenix.h"
#include "networktables/NetworkTable.h"
#include "networktables/NetworkTableEntry.h"
#include "networktables/NetworkTableInstance.h"
#include "networktables/NetworkTableValue.h"
#include "photonlib/PhotonCamera.h"
#include "swerve_drive_subsystem.h"
#include "units/angle.h"
#include "units/angular_velocity.h"
#include "units/length.h"
#include "utils/homing_storage_interface.h"
 
class LimelightTarget {
 private:
  units::degree_t m_pitch;                 
  units::degree_t m_yaw;                   
  double m_bboxHor;                        
  double m_bboxVer;                        
  units::degree_t m_skew;                  
  bool m_hasTargets;                       
  units::millisecond_t m_pipelineLatency;  
  constexpr static units::millisecond_t m_miscLatency{11_ms};  
 
 public:
  LimelightTarget() = default;
 
  struct tValues {
    units::degree_t pitch;              
    units::degree_t yaw;                
    double bboxHor;                     
    double bboxVer;                     
    units::degree_t skew;               
    units::millisecond_t totalLatency;  
  };
 
  tValues GetTarget();
 
  bool HasTarget();
 
  void adjustPerspective(units::degree_t& currentPitch, const units::degree_t& currentYaw);
};
 
struct AimValues {
  units::degree_t turretTarget;                                     
  units::degree_t hoodTarget;                                       
  units::angular_velocity::revolutions_per_minute_t shooterTarget;  
};
 
class CameraInterface {
 public:
  CameraInterface();
 
  LimelightTarget m_target;  
 
  std::optional<LimelightTarget> GetHighestTarget();
 
  units::angle::degree_t VerticalPixelToAngle(int pixels);
 
  units::angle::degree_t HorizontalPixelToAngle(int pixels);
 
  units::angle::degree_t GetNewPitch(
      units::degree_t cx, units::degree_t cy, int bboxHorizontalPixels, int bboxVerticalPixels, units::degree_t skew);
 
  void SetDriverMode(bool mode);
};
 
class ShooterSubsystem : public frc2::SubsystemBase {
 public:
  struct ShooterDistanceSetpoints {
    units::revolutions_per_minute_t shooterSpeed;
    units::degree_t hoodAngle;
  };
 
  struct HubRelativeVelocities {
    units::feet_per_second_t radialVelocity;      
    units::feet_per_second_t tangentialVelocity;  
    units::degrees_per_second_t chassisYawRate;   
  };
 
  struct AimOffsets {
    units::foot_t distanceOffset;  
    units::degree_t yawOffset;     
  };
 
  ShooterSubsystem(const argos_lib::RobotInstance instance,
                   SwerveDriveSubsystem* pDriveSubsystem,
                   argos_lib::SwappableControllersSubsystem* controllers = nullptr);
 
  enum class FixedPosState {
    Front,  
    Left,   
    Right,  
    Back    
  };
 
  std::optional<units::degree_t> GetTurretTargetAngle(LimelightTarget::tValues target);
 
  LimelightTarget::tValues GetCameraTargetValues();
 
  void Periodic() override;
 
  bool AutoAim(bool drivingAdjustment = false);
 
  void Shoot(double ballfiringspeed);
 
  void ManualAim(double turnSpeed, double hoodSpeed);
 
  void CloseLoopShoot(units::revolutions_per_minute_t ShooterWheelSpeed);
 
  units::revolutions_per_minute_t GetShooterWheelSpeed();
 
  void MoveHood(double hoodSpeed);
 
  void MoveTurret(double turnSpeed);
 
  void HoodSetPosition(units::degree_t angle);
 
  units::degree_t GetHoodPosition();
 
  void UpdateHoodHome();
 
  bool IsHoodMoving();
 
  bool IsHoodHomed();
 
  void ClearHoodHome();
 
  void UpdateTurretHome();
 
  void InitializeTurretHome();
 
  bool IsTurretHomed();
 
  void TurretSetPosition(units::degree_t angle);
 
  std::optional<units::degree_t> TurretGetPosition();
 
  void SetTurretSoftLimits();
 
  void DisableTurretSoftLimits();
 
  void SetHoodSoftLimits();
 
  void DisableHoodSoftLimits();
 
  bool IsManualOverride();
 
  void Disable();
 
  units::inch_t GetTargetDistance(units::degree_t targetVerticalAngle);
 
  units::inch_t GetPolynomialOffset(units::inch_t actualDistance);
 
  ShooterDistanceSetpoints GetShooterDistanceSetpoints(units::inch_t distanceToTarget) const;
 
  ShooterDistanceSetpoints SetShooterDistance(units::inch_t distanceToTarget);
 
  void FixedShooterPosition(FixedPosState);
 
  units::angular_velocity::revolutions_per_minute_t GetShooterSpeed();
 
  void ManualOverride();
 
  void SetCameraDriverMode(bool driverMode);
 
  static bool InAcceptableRanges(const AimValues targets, const AimValues real);
 
  void StopFeedback() const;
 
  void AimedFeedback() const;
 
 private:
  HubRelativeVelocities ChassisVelocitiesToHubVelocities(const frc::ChassisSpeeds robotChassisSpeed,
                                                         units::degree_t hubTurretAngle);
 
  AimOffsets DrivingAimOffsets(const HubRelativeVelocities robotVelocity,
                               units::foot_t hubDistance,
                               units::degree_t hubAngle,
                               units::second_t targetStaleness);
 
  const argos_lib::CANAddress m_shooterWheelLeftAddr;
  const argos_lib::CANAddress m_shooterWheelRightAddr;
  const argos_lib::CANAddress m_hoodMotorAddr;
  const argos_lib::CANAddress m_turretMotorAddr;
 
  // Components (e.g. motor controllers and sensors) should generally be
  // declared private and exposed only through public methods.
  WPI_TalonFX m_shooterWheelLeft;   
  WPI_TalonFX m_shooterWheelRight;  
  WPI_TalonSRX m_hoodMotor;         
  WPI_TalonSRX m_turretMotor;       
 
  CameraInterface m_cameraInterface;  
 
  FSHomingStorage<units::degree_t> m_turretHomingStorage;  
 
  bool m_hoodHomed;    
  bool m_turretHomed;  
  bool m_manualOverride;
  bool m_useCalculatedPitch;  
 
  argos_lib::InterpolationMap<decltype(shooterRange::shooterSpeed.front().inVal),
                              shooterRange::shooterSpeed.size(),
                              decltype(shooterRange::shooterSpeed.front().outVal)>
      m_shooterSpeedMap;  
  argos_lib::InterpolationMap<decltype(shooterRange::hoodAngle.front().inVal),
                              shooterRange::hoodAngle.size(),
                              decltype(shooterRange::hoodAngle.front().outVal)>
      m_hoodAngleMap;  
  argos_lib::InterpolationMap<decltype(shooterRange::lateralSpeed.front().inVal),
                              shooterRange::lateralSpeed.size(),
                              decltype(shooterRange::lateralSpeed.front().outVal)>
      m_lateralSpeedMap;
 
  argos_lib::NTMotorPIDTuner m_hoodPIDTuner;     
  argos_lib::NTMotorPIDTuner m_shooterPIDTuner;  
  argos_lib::NTMotorPIDTuner m_turretPIDTuner;   
 
  argos_lib::RobotInstance
      m_instance;  
  SwerveDriveSubsystem* m_pDriveSubsystem;                   
  argos_lib::SwappableControllersSubsystem* m_pControllers;  
};