drive_until_pitch_rate.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 <units/angle.h>
#include <units/angular_velocity.h>
#include <units/time.h>
 
#include <chrono>
 
#include "Constants.h"
#include "subsystems/swerve_drive_subsystem.h"
 
class DriveUntilPitchRate : public frc2::CommandHelper<frc2::CommandBase, DriveUntilPitchRate> {
 public:
  DriveUntilPitchRate(SwerveDriveSubsystem* swerveDrive,
                      units::degree_t velAngle,
                      double power,
                      double initialPower,
                      units::degrees_per_second_t pitchRateGoal,
                      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::degrees_per_second_t m_pitchRateGoal;  
  const ApproachDirection m_approachDirection;        
  const units::time::second_t m_timeout;  
  frc::SlewRateLimiter<units::scalar> m_velocityRamper;  
};