lifter_subsystem.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <argos_lib/config/config_types.h>
#include <ctre/Phoenix.h>
#include <frc/geometry/Translation2d.h>
#include <frc2/command/SubsystemBase.h>
 
#include <string>
 
#include "argos_lib/general/log.h"
#include "argos_lib/general/nt_motor_pid_tuner.h"
#include "argos_lib/homing/fs_homing.h"
#include "constants/interpolation_maps.h"
#include "utils/lifter_kinematics.h"
 
/* —————————————————————————— SUBSYSTEM CLASS —————————————————————————— */
 
enum class WristPosition { RollersDown, RollersUp, Unknown };
 
std::string ToString(WristPosition position);
 
class LifterSubsystem : public frc2::SubsystemBase {
 public:
  struct LifterPosition {
    units::degree_t wristAngle;
    ArmState state;
  };
  explicit LifterSubsystem(argos_lib::RobotInstance instance);
 
  void SetShoulderSpeed(double speed);
 
  void SetArmExtensionSpeed(double speed);
 
  void SetArmExtension(units::inch_t extension);
 
  bool IsExtensionAt(units::inch_t extension);
 
  void SetWristSpeed(double speed);
 
  void StopShoulder();
 
  void StopArmExtension();
 
  void StopWrist();
 
  bool IsShoulderManualOverride();
 
  void SetShoulderManualOverride(bool overrideState);
 
  bool IsExtensionManualOverride();
 
  void SetExtensionManualOverride(bool overrideState);
 
  bool IsWristManualOverride();
 
  void SetWristManualOverride(bool overrideState);
 
  void InitializeWristHomes();
 
  void SetWristAngle(units::degree_t wristAngle);
 
  units::degree_t GetWristAbsoluteAngle();
  double GetLastWristTimestamp();
 
  void UpdateWristHome();
 
  void Periodic() override;
 
  void Disable();
 
  bool IsArmExtensionMoving();
 
  bool IsShoulderMoving();
 
  bool ArmExtensionHomeFileExists();
 
  void UpdateArmExtensionHome();
 
  void InitializeArmExtensionHome();
 
  void InitializeShoulderHome();
 
  void UpdateShoulderHome();
 
  void SetShoulderAngle(units::degree_t angle);
 
  bool IsShoulderAt(units::degree_t angle);
 
  frc::Translation2d GetEffectorPose(const WristPosition wristPosition);
 
  frc::Translation2d GetArmPose();
 
  LifterPosition SetEffectorPose(const frc::Translation2d& desPose, WristPosition effectorPosition);
 
  LifterPosition SetLifterPose(const frc::Translation2d& desPose);
 
  bool IsArmExtensionHomed();
 
  units::degree_t GetWristAngle();
 
  WristPosition GetWristPosition();
 
  units::inch_t GetArmExtension();
 
  units::degree_t GetShoulderAngle();
 
  units::degree_t GetShoulderBoomAngle();
 
  LifterPosition GetLifterPosition();
 
  ArmState ConvertEffectorPose(const frc::Translation2d& pose, WristPosition effectorPosition) const;
 
  ArmState ConvertLifterPose(const frc::Translation2d& pose) const;
 
  frc::Translation2d ConvertStateToEffectorPose(const ArmState& state, WristPosition effectorPosition) const;
  frc::Translation2d ConvertStateToLifterPose(const ArmState& state) const;
 
  units::inch_t ConvertShoulderAngle(units::degree_t angle) const;
  units::inches_per_second_t ConvertShoulderVelocity(units::degree_t angle,
                                                     units::degrees_per_second_t angularVelocity) const;
 
  bool IsShoulderMPComplete();
  bool IsExtensionMPComplete();
  bool IsWristMPComplete();
 
  ctre::phoenix::motion::BufferedTrajectoryPointStream& GetShoulderMPStream();
  ctre::phoenix::motion::BufferedTrajectoryPointStream& GetExtensionMPStream();
  ctre::phoenix::motion::BufferedTrajectoryPointStream& GetWristMPStream();
 
  void StopMotionProfile();
  void StartMotionProfile(size_t shoulderStreamSize, size_t extensionStreamSize, size_t wristStreamSize);
 
  void ResetPathFaults();
  bool IsFatalPathFault();
 
  void DisableArmExtensionSoftLimits();
 
  void EnableArmExtensionSoftLimits();
 
 private:
  // Components (e.g. motor controllers and sensors) should generally be
  // declared private and exposed only through public methods.
 
  // Shoulder motors are attached in parallel mechanically to operate shoulder, back motor follows front motor
  WPI_TalonFX m_shoulderDrive;      
  WPI_TalonFX m_armExtensionMotor;  
  WPI_TalonFX m_wrist;              
  CANCoder m_shoulderEncoder;       
  CANCoder m_wristEncoder;          
  CANCoder m_extensionEncoder;      
  LifterKinematics m_kinematics;    
  argos_lib::ArgosLogger m_logger;  
  argos_lib::FSHomingStorage<units::degree_t> m_shoulderHomeStorage;     
  argos_lib::FSHomingStorage<units::degree_t> m_wristHomingStorage;      
  argos_lib::FSHomingStorage<units::degree_t> m_extensionHomingStorage;  
  argos_lib::NTMotorPIDTuner m_extensionTuner;  
  argos_lib::NTMotorPIDTuner m_wristTuner;      
  argos_lib::NTMotorPIDTuner m_shoulderTuner;   
  bool m_shoulderHomed;                         
  bool m_extensionHomed;                        
  bool m_wristHomed;                            
  bool m_shoulderManualOverride;   
  bool m_extensionManualOverride;  
  bool m_wristManualOverride;      
 
  ctre::phoenix::motion::BufferedTrajectoryPointStream m_shoulderStream;
  ctre::phoenix::motion::BufferedTrajectoryPointStream m_extensionStream;
  ctre::phoenix::motion::BufferedTrajectoryPointStream m_wristStream;
 
  void EnableWristSoftLimits();
 
  void DisableWristSoftLimits();
 
  void EnableShoulderSoftLimits();
 
  void DisableShoulderSoftLimits();
};