SwerveDrive.h

#pragma once
 
#include <array>
#include <initializer_list>
#include <memory>
#include <mutex>
#include <string>
 
#include <units/angle.h>
#include <units/velocity.h>
 
#include <frc/controller/HolonomicDriveController.h>
#include <frc/estimator/SwerveDrivePoseEstimator.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/interfaces/Gyro.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/kinematics/SwerveDriveKinematics.h>
#include <frc/kinematics/SwerveModulePosition.h>
#include <frc/kinematics/SwerveModuleState.h>
#include <frc/trajectory/Trajectory.h>
 
#include <pathplanner/lib/path/PathPlannerTrajectory.h>
 
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc2/command/Commands.h"
#include "networktables/DoubleArrayTopic.h"
#include "pathplanner/lib/commands/FollowPathHolonomic.h"
#include "pathplanner/lib/path/PathConstraints.h"
#include "pathplanner/lib/path/PathPlannerPath.h"
#include "rmb/drive/BaseDrive.h"
#include "rmb/drive/SwerveModule.h"
#include "units/angular_velocity.h"
 
#include <frc2/command/Command.h>
#include <frc2/command/CommandPtr.h>
 
#include "networktables/DoubleTopic.h"
#include "units/time.h"
 
#include <rmb/sensors/gyro.h>
#include <vector>
 
namespace rmb {
 
template <size_t NumModules> class SwerveDrive : public BaseDrive {
public:
  SwerveDrive(const SwerveDrive &) = delete;
  SwerveDrive(SwerveDrive &&) = delete;
 
  SwerveDrive(std::array<SwerveModule, NumModules> modules,
              std::shared_ptr<const rmb::Gyro> gyro,
              frc::HolonomicDriveController holonomicController,
              std::string visionTable,
              units::meters_per_second_t maxModuleSpeed,
              const frc::Pose2d &initialPose = frc::Pose2d());
 
  SwerveDrive(std::array<SwerveModule, NumModules> modules,
              std::shared_ptr<const rmb::Gyro> gyro,
              frc::HolonomicDriveController holonomicController,
              units::meters_per_second_t maxModuleSpeed,
              const frc::Pose2d &initialPose = frc::Pose2d());
 
  virtual ~SwerveDrive() = default;
 
  void driveCartesian(double xSpeed, double ySpeed, double zRotation,
                      bool fieldOriented);
 
  void drivePolar(double speed, const frc::Rotation2d &angle, double zRotation,
                  bool fieldOriented);
 
  void driveModulePowers(std::array<SwerveModulePower, NumModules> powers);
 
  void driveModuleStates(std::array<frc::SwerveModuleState, NumModules> states);
 
  std::array<frc::SwerveModuleState, NumModules> getModuleStates() const;
 
  std::array<frc::SwerveModulePosition, NumModules> getModulePositions() const;
 
  const std::array<rmb::SwerveModule, NumModules> &getModules() const {
    return modules;
  }
 
  void driveChassisSpeeds(frc::ChassisSpeeds chassisSpeeds) override;
 
  frc::ChassisSpeeds getChassisSpeeds() const override;
 
  //------------------
  // Odometry Methods
  //------------------
 
  frc::Pose2d getPose() const override;
 
  std::array<frc::SwerveModuleState, NumModules> getTargetModuleStates() const;
 
  frc::Pose2d updatePose() override;
 
  void resetPose(const frc::Pose2d &pose = frc::Pose2d()) override;
 
  void addVisionMeasurments(const frc::Pose2d &poseEstimate,
                            units::second_t time) override;
 
  void setVisionSTDevs(wpi::array<double, 3> standardDevs) override;
 
  //----------------------
  // Trajectory Following
  //----------------------
 
  bool isHolonomic() const override { return true; }
 
  // /**
  //  * Generates a command to follow WPILib Trajectory.
  //  *
  //  * @param trajectory       The trajectory to follow.
  //  * @param driveRequirments The subsystems required for driving the robot
  //  * (ie. the one that contains this class)
  //  *
  //  * @return The command to follow a trajectory.
  //  */
  frc2::CommandPtr followWPILibTrajectory(
      frc::Trajectory trajectory,
      std::initializer_list<frc2::Subsystem *> driveRequirements) override;
 
  // /**
  //  * Generates a command to follow PathPlanner Trajectory.
  //  *
  //  * @param trajectory       The trajectory to follow.
  //  * @param driveRequirments The subsystems required for driving the robot
  //  *                         (ie. the one that contains this class)
  //  *
  //  * @return The command to follow a trajectory.
  //  */
  frc2::CommandPtr followPPPath(
      std::shared_ptr<pathplanner::PathPlannerPath> path,
      std::initializer_list<frc2::Subsystem *> driveRequirements) override;
 
  frc2::CommandPtr FollowGeneratedPPPath(
      frc::Pose2d targetPose, pathplanner::PathConstraints constraints,
      std::initializer_list<frc2::Subsystem *> driveRequirements);
 
  void updateNTDebugInfo(bool openLoopVelocity = false);
 
  void stop();
 
private:
  void recomputeOpenloopInverseKinematicsMatrix();
 
  //-----------------
  // Network Tables Debugging
  //-----------------
 
  // std::array<nt::DoublePublisher, NumModules> ntVelocityErrorTopics;
  // std::array<nt::DoublePublisher, NumModules> ntPositionErrorTopics;
  // std::array<nt::DoublePublisher, NumModules> ntPositionTopics;
  // std::array<nt::DoublePublisher, NumModules> ntVelocityTopics;
 
  nt::DoubleArrayPublisher ntPositionTopics;
  nt::DoubleArrayPublisher ntVelocityTopics;
 
  nt::DoubleArrayPublisher ntPositionErrorTopics;
  nt::DoubleArrayPublisher ntVelocityErrorTopics;
 
  nt::DoubleArrayPublisher ntTargetPositionTopics;
  nt::DoubleArrayPublisher ntTargetVelocityTopics;
 
  //-----------------
  // Drive Variables
  //-----------------
 
  std::array<SwerveModule, NumModules> modules;
 
  std::shared_ptr<const rmb::Gyro> gyro;
 
  frc::SwerveDriveKinematics<NumModules> kinematics;
 
  Eigen::Matrix<float, 2 * NumModules, 3> openLoopInverseKinematics;
 
  frc::HolonomicDriveController holonomicController;
 
  //-------------------
  // Odometry Variables
  //-------------------
 
  frc::SwerveDrivePoseEstimator<NumModules> poseEstimator;
 
  mutable std::mutex visionThreadMutex;
 
  units::meters_per_second_t maxModuleSpeed;
 
  units::meter_t largestModuleDistance = 1.0_m;
};
} // namespace rmb
 
#include "SwerveDrive.inl"