DifferentialDrive.h

#pragma once
 
#include <initializer_list>
#include <memory>
#include <string>
#include <unordered_map>
 
#include <units/velocity.h>
 
#include <frc/controller/RamseteController.h>
#include <frc/estimator/DifferentialDrivePoseEstimator.h>
#include <frc/interfaces/Gyro.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
#include <frc/trajectory/Trajectory.h>
 
#include <frc2/command/CommandPtr.h>
 
#include <pathplanner/lib/path/PathPlannerTrajectory.h>
 
#include "pathplanner/lib/path/PathPlannerPath.h"
#include "rmb/drive/BaseDrive.h"
#include "rmb/motorcontrol/LinearVelocityController.h"
 
namespace rmb {
 
class DifferentialDrive : public BaseDrive {
public:
  DifferentialDrive(const DifferentialDrive &) = delete;
  DifferentialDrive(DifferentialDrive &&) = delete;
 
  DifferentialDrive(std::unique_ptr<LinearVelocityController> left,
                    std::unique_ptr<LinearVelocityController> right,
                    std::shared_ptr<const frc::Gyro> gyro,
                    frc::DifferentialDriveKinematics kinematics,
                    frc::RamseteController ramseteController,
                    std::string visionTable,
                    const frc::Pose2d &initalPose = frc::Pose2d());
 
  DifferentialDrive(std::unique_ptr<LinearVelocityController> left,
                    std::unique_ptr<LinearVelocityController> right,
                    std::shared_ptr<const frc::Gyro> gyro,
                    frc::DifferentialDriveKinematics kinematics,
                    frc::RamseteController ramseteController,
                    const frc::Pose2d &initalPose = frc::Pose2d());
 
  //---------------
  // Drive Methods
  //---------------
 
  void arcadeDrive(double xSpeed, double zRotation);
 
  void curvatureDrive(double xSpeed, double zRotation, bool turnInPlace);
 
  void tankDrive(double leftSpeed, double rightSpeed);
 
  void driveWheelSpeeds(units::meters_per_second_t leftVelocity,
                        units::meters_per_second_t rightVelocity);
 
  void driveWheelSpeeds(frc::DifferentialDriveWheelSpeeds wheelSpeeds);
 
  frc::DifferentialDriveWheelSpeeds getWheelSpeeds() const;
 
  void driveChassisSpeeds(frc::ChassisSpeeds chassisSpeeds) override;
 
  frc::ChassisSpeeds getChassisSpeeds() const override;
 
  //------------------
  // Odometry Methods
  //------------------
 
  frc::Pose2d getPose() const override;
 
  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 false; }
 
  frc2::CommandPtr followWPILibTrajectory(
      frc::Trajectory trajectory,
      std::initializer_list<frc2::Subsystem *> driveRequirements) override;
 
  frc2::CommandPtr followPPPath(
      std::shared_ptr<pathplanner::PathPlannerPath> path,
      std::initializer_list<frc2::Subsystem *> driveRequirements) override;
 
private:
  //-----------------
  // Drive Variables
  //-----------------
 
  std::unique_ptr<LinearVelocityController> left;
 
  std::unique_ptr<LinearVelocityController> right;
 
  std::shared_ptr<const frc::Gyro> gyro;
 
  frc::DifferentialDriveKinematics kinematics;
 
  frc::RamseteController ramseteController;
 
  //-------------------
  // Odometry Variables
  //-------------------
 
  frc::DifferentialDrivePoseEstimator poseEstimator;
 
  mutable std::mutex visionThreadMutex;
};
} // namespace rmb