swerve_drive_subsystem.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <frc/ADIS16448_IMU.h>
#include <frc/Timer.h>
#include <frc/controller/HolonomicDriveController.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/kinematics/SwerveDriveOdometry.h>
#include <frc/kinematics/SwerveModulePosition.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <frc2/command/SubsystemBase.h>
 
#include <memory>
 
#include "argos_lib/config/config_types.h"
#include "argos_lib/general/nt_motor_pid_tuner.h"
#include "argos_lib/general/nt_subscriber.h"
#include "argos_lib/homing/fs_homing.h"
#include "ctre/Phoenix.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/SwerveDrivePoseEstimator.h"
#include "utils/swerve_trapezoidal_profile.h"
#include "utils/swerve_trapezoidal_spline.h"
 
class SwerveModule {
 public:
  // MOTORS
  WPI_TalonFX m_drive;
  WPI_TalonFX m_turn;
  // ENCODER
  CANCoder m_encoder;
 
  SwerveModule(const argos_lib::CANAddress& driveAddr,
               const argos_lib::CANAddress& turnAddr,
               const argos_lib::CANAddress& encoderAddr);
 
  frc::SwerveModuleState GetState();
  frc::SwerveModulePosition GetPosition();
};
 
class SwerveDriveSubsystem : public frc2::SubsystemBase {
 public:
  enum DriveControlMode { fieldCentricControl, robotCentricControl };
 
  explicit SwerveDriveSubsystem(const argos_lib::RobotInstance instance);
 
  void Disable();
 
  void SwerveDrive(const double fwVelocity, const double sideVelocity, const double rotVelocity);
 
  void SwerveDrive(const units::degree_t& velAngle, const double& velocity, const double& rotVelocity);
 
  void SwerveDrive(const units::degree_t& velAngle, const double& velocity);
 
  void StopDrive();
 
  void Home(const units::degree_t& angle);
 
  void FieldHome(units::degree_t homeAngle = 0_deg, bool updateOdometry = true);
 
  void InitializeOdometry(const frc::Pose2d& currentPose);
 
  frc::Rotation2d GetContinuousOdometryAngle();
 
  frc::Rotation2d GetNearestSquareAngle();
 
  frc::Pose2d GetContinuousOdometry();
 
  frc::Pose2d UpdateEstimatedPose();
 
  units::degree_t GetFieldCentricAngle() const;
 
  frc::Pose2d GetPoseEstimate(const frc::Pose2d& robotPose, const units::millisecond_t& latency);
 
  void SetControlMode(SwerveDriveSubsystem::DriveControlMode controlMode);
 
  void InitializeMotors();
 
  void UpdateFollowerLinearPIDParams(double kP, double kI, double kD);
 
  void UpdateFollowerRotationalPIDParams(double kP, double kI, double kD);
 
  void UpdateFollowerRotationalPIDConstraints(frc::TrapezoidProfile<units::radians>::Constraints constraints);
  void UpdateFollowerRotationalPIDConstraints(frc::TrapezoidProfile<units::degrees>::Constraints constraints);
 
  void StartDrivingProfile(SwerveTrapezoidalProfileSegment newProfile);
 
  void StartDrivingProfile(SwerveTrapezoidalSpline newProfile);
 
  void CancelDrivingProfile();
 
  bool ProfileIsComplete() const;
 
  bool IsFollowingProfile() const;
 
  frc::ChassisSpeeds GetChassisVelocity();
 
  void LockWheels();
 
  bool GetManualOverride();
 
  units::degree_t GetRobotPitch() const { return units::degree_t{m_pigeonIMU.GetRoll()}; }
 
  units::degrees_per_second_t GetRobotPitchRate();
 
 private:
  argos_lib::RobotInstance m_instance;
 
  DriveControlMode m_controlMode;  
 
  SwerveModule m_frontLeft;   
  SwerveModule m_frontRight;  
  SwerveModule m_backRight;   
  SwerveModule m_backLeft;    
 
  // GYROSCOPIC SENSORS
  frc::ADIS16448_IMU m_imu;
  Pigeon2 m_pigeonIMU;
 
  units::degree_t m_fieldHomeOffset;  
 
  struct Velocities {
    const double fwVelocity;
    const double sideVelocity;
    const double rotVelocity;
  };
 
  frc::SwerveDriveKinematics<4> m_swerveDriveKinematics;  
 
  frc::SwerveDriveOdometry<4> m_odometry;      
  units::degree_t m_prevOdometryAngle;         
  units::degree_t m_continuousOdometryOffset;  
 
  frc::SwerveDrivePoseEstimator<4> m_poseEstimator;  
 
  // std::FILE SYSTEM HOMING STORAGE
  argos_lib::SwerveFSHomingStorage m_fsStorage;  
 
  bool m_followingProfile;  
  bool m_profileComplete;   
  bool m_manualOverride;
  std::unique_ptr<SwerveTrapezoidalProfileSegment> m_pActiveSwerveProfile;      
  std::unique_ptr<SwerveTrapezoidalSpline> m_pActiveSwerveSplineProfile;        
  std::chrono::time_point<std::chrono::steady_clock> m_swerveProfileStartTime;  
  frc::ProfiledPIDController<units::radians>::Constraints m_rotationalPIDConstraints;
  frc2::PIDController m_linearPID;  
  frc::HolonomicDriveController m_followerController;  
 
  argos_lib::NTMotorPIDTuner m_driveMotorPIDTuner;  
  argos_lib::NTSubscriber m_linearFollowerTuner_P;
  argos_lib::NTSubscriber m_linearFollowerTuner_I;
  argos_lib::NTSubscriber m_linearFollowerTuner_D;
  argos_lib::NTSubscriber m_rotationalFollowerTuner_P;
  argos_lib::NTSubscriber m_rotationalFollowerTuner_I;
  argos_lib::NTSubscriber m_rotationalFollowerTuner_D;
  argos_lib::NTSubscriber m_rotationalFollowerConstraintTuner_vel;
  argos_lib::NTSubscriber m_rotationalFollowerConstraintTuner_accel;
 
  wpi::array<frc::SwerveModuleState, 4> GetRawModuleStates(SwerveDriveSubsystem::Velocities velocities);
 
  wpi::array<frc::SwerveModuleState, 4> GetCurrentModuleStates();
 
  wpi::array<frc::SwerveModulePosition, 4> GetCurrentModulePositions();
 
  void HomeToFS(const units::degree_t& angle);
 
  void InitializeMotorsFromFS();
 
  units::degree_t GetIMUYaw() const;
  void ResetIMUYaw();
};