intake_subsystem.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <ctre/Phoenix.h>
#include <frc/filter/LinearFilter.h>
#include <frc2/command/SubsystemBase.h>
 
#include <TimeOfFlight.h>
 
#include "argos_lib/config/config_types.h"
#include "argos_lib/general/hysteresis_filter.h"
#include "units/length.h"
 
class IntakeSubsystem : public frc2::SubsystemBase {
 public:
  explicit IntakeSubsystem(argos_lib::RobotInstance instance);
 
  void Periodic() override;
  void IntakeForward();
  void IntakeReverse();
  void IntakeFastReverse();
 
  void IntakeCone();
  void EjectCone();
  void EjectConeForReal();
  void IntakeCube();
  void EjectCube();
  void IntakeStop();
  void Disable();
 
  units::inch_t GetIntakeDistance();
  bool TofConeDetected();
  bool TofCubeDetected();
  bool IsConeDetected();  
  bool IsCubeDetected();  
  bool IsGamepieceLost();
 
 private:
  // Components (e.g. motor controllers and sensors) should generally be
  // declared private and exposed only through public methods.
  WPI_TalonSRX m_intakeMotor;
  argos_lib::RobotInstance m_robotInstance;
 
  frc::TimeOfFlight
      m_coneLeftIntakeSensor;  
  frc::TimeOfFlight
      m_coneRightIntakeSensor;  
  frc::TimeOfFlight
      m_cubeIntakeSensor;  
  bool m_haveCone;
  bool m_haveCube;
 
  frc::LinearFilter<units::inch_t> m_coneOffsetFilter;
  units::inch_t
      m_filteredConeOffset;  
 
  bool IsGamePieceDetected();
  std::optional<units::inch_t> ReadSensorDistance(frc::TimeOfFlight& sensor);
  std::optional<units::inch_t> GetRawConePosition();
};