drive_until_pitch.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <frc/filter/SlewRateLimiter.h>
#include <frc2/command/CommandBase.h>
#include <frc2/command/CommandHelper.h>
 
#include <chrono>
 
#include "Constants.h"
#include "subsystems/swerve_drive_subsystem.h"
#include "units/time.h"
 
class DriveUntilPitch : public frc2::CommandHelper<frc2::CommandBase, DriveUntilPitch> {
 public:
  DriveUntilPitch(SwerveDriveSubsystem* swerveDrive,
                  units::degree_t velAngle,
                  double power,
                  double initialPower,
                  units::degree_t pitchGoal,
                  ApproachDirection approachDirection,
                  units::time::second_t timeout);
 
  void Initialize() override;
 
  void Execute() override;
 
  void End(bool interrupted) override;
 
  bool IsFinished() override;
 
 private:
  SwerveDriveSubsystem* m_pDrive;    
  const units::degree_t m_velAngle;  
  const double m_power;              
  const double m_initialPower;       
  std::chrono::time_point<std::chrono::high_resolution_clock> m_startTime;  
  const units::degree_t m_pitchGoal;            
  const ApproachDirection m_approachDirection;  
  const units::time::second_t m_timeout;        
  frc::SlewRateLimiter<units::scalar> m_velocityRamper;  
};