climber_subsystem.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <frc2/command/SubsystemBase.h>
 
#include <vector>
 
#include "Constants.h"
#include "argos_lib/config/config_types.h"
#include "argos_lib/general/nt_motor_pid_tuner.h"
#include "ctre/Phoenix.h"
#include "units/length.h"
#include "utils/sensor_conversions.h"
 
class ClimberSubsystem : public frc2::SubsystemBase {
 public:
  explicit ClimberSubsystem(const argos_lib::RobotInstance instance, const std::vector<ClimberPoint>* preClimbPoints);
 
  void Periodic() override;
 
  void ManualControl(double hookSpeed, double armSpeed);
 
  void MoveHook(double hookSpeed);
 
  void MoveArm(double armSpeed);
 
  void UpdateHookHome();
 
  void UpdateArmHome();
 
  void ArmSetPosition(units::inch_t extension);
 
  void ArmSetPosition(units::inch_t extension,
                      units::inches_per_second_t cruiseVelocity,
                      units::inches_per_second_squared_t acceleration);
 
  void HooksSetPosition(units::inch_t extension);
 
  void HooksSetPosition(units::inch_t extension,
                        units::inches_per_second_t cruiseVelocity,
                        units::inches_per_second_squared_t acceleration);
 
  bool IsHookHomed();
 
  bool IsArmHomed();
 
  bool IsHookMoving();
 
  bool IsArmMoving();
 
  bool IsManualOverride();
 
  void Disable();
 
  void ManualOverride();
 
  void SetHookSoftLimits();
 
  void DisableHookSoftLimits();
 
  void SetClimbReady();
 
  template <typename T>
  constexpr static bool InThreshold(const T value, const T target, const T threshold) {
    return value >= target - threshold && value <= target + threshold;
  }
 
  void ClimberToSetpoint(ClimberPoint setPoint);
 
  void SetClimbMotorsPID(char slot);
 
  bool ClimberAtPoint(ClimberPoint target);
 
  // NEW STUFF EVALUATE WHAT NEEDS TO BE REMOVED
 
  void NextReadyPoint();
 
  void PreviousReadyPoint();
 
  void AllowReady();
 
  void DisallowReady();
 
  bool IsReadySequenceAllowed();
 
  bool ClimberReadyToClimb();
 
 private:
  const argos_lib::CANAddress m_motorLiftRightAddr;
  const argos_lib::CANAddress m_motorLiftLeftAddr;
  const argos_lib::CANAddress m_motorMoveHookAddr;
  // Components (e.g. motor controllers and sensors) should generally be
  // declared private and exposed only through public methods.
  WPI_TalonFX m_motorLiftRight;  
  WPI_TalonFX m_motorLiftLeft;   
  WPI_TalonFX m_motorMoveHook;   
 
  bool m_hookHomed;  
  bool m_armHomed;   
 
  bool m_allowReady;  
 
  bool m_manualOverride;  
  const std::vector<ClimberPoint>* m_pPreClimbPoints;  
  std::vector<ClimberPoint>::const_iterator
      m_itClimberPoint;  
 
  argos_lib::NTMotorPIDTuner m_armPIDTuner;   
  argos_lib::NTMotorPIDTuner m_hookPIDTuner;  
 
  bool HooksAtPosition(units::inch_t target);
 
  bool ArmsAtPosition(units::inch_t target);
};