swerve_drive_subsystem.h

Go to the documentation of this file.

 
 
#pragma once
 
#include <frc/ADIS16448_IMU.h>
#include <frc/controller/HolonomicDriveController.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/kinematics/SwerveDriveOdometry.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 "ctre/Phoenix.h"
#include "utils/file_system_homing_storage.h"
#include "utils/network_tables_wrapper.h"
#include "utils/swerve_trapezoidal_profile.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();
};
 
class SwerveDriveSubsystem : public frc2::SubsystemBase {
 public:
  enum DriveControlMode { fieldCentricControl, robotCentricControl };
 
  explicit SwerveDriveSubsystem(std::shared_ptr<NetworkTablesWrapper> networkTable,
                                const argos_lib::RobotInstance instance);
 
  void Disable();
 
  void SwerveDrive(const double& fwVelocity, const double& sideVelocity, const double& rotVelocity);
 
  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::Pose2d GetContinuousOdometry();
 
  frc::Pose2d UpdateOdometry();
 
  units::degree_t GetFieldCentricAngle() const;
 
  frc::Pose2d GetPoseEstimate();
 
  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::degrees>::Constraints constraints);
 
  void StartDrivingProfile(SwerveTrapezoidalProfileSegment newProfile);
 
  void CancelDrivingProfile();
 
  bool ProfileIsComplete() const;
 
  frc::ChassisSpeeds GetChassisVelocity();
 
 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;  
 
  // POINTER TO NETWORK TABLE CLASS OBJECT
  std::shared_ptr<NetworkTablesWrapper> m_pNetworkTable;  
 
  // std::FILE SYSTEM HOMING STORAGE
  FileSystemHomingStorage m_fsStorage;  
 
  bool m_followingProfile;  
  bool m_profileComplete;   
  std::unique_ptr<SwerveTrapezoidalProfileSegment> m_pActiveSwerveProfile;      
  std::chrono::time_point<std::chrono::steady_clock> m_swerveProfileStartTime;  
  frc2::PIDController m_linearPID;  
  frc::ProfiledPIDController<units::radians>
      m_rotationalPID;  
  frc::HolonomicDriveController m_followerController;  
 
  argos_lib::NTMotorPIDTuner m_driveMotorPIDTuner;  
 
  wpi::array<frc::SwerveModuleState, 4> GetRawModuleStates(SwerveDriveSubsystem::Velocities velocities);
 
  wpi::array<frc::SwerveModuleState, 4> GetCurrentModuleStates();
 
  void HomeToFS(const units::degree_t& angle);
 
  void InitializeMotorsFromFS();
 
  units::degree_t GetIMUYaw() const;
  void ResetIMUYaw();
};