BaseDrive.h

#pragma once
 
#include "pathplanner/lib/path/PathPlannerPath.h"
#include <initializer_list>
#include <memory>
#include <string>
#include <unordered_map>
 
#include <units/time.h>
 
#include <wpi/array.h>
 
#include <frc/interfaces/Gyro.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <frc/trajectory/Trajectory.h>
 
#include <networktables/DoubleArrayTopic.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>
 
#include <frc2/command/CommandPtr.h>
#include <frc2/command/Subsystem.h>
 
#include <pathplanner/lib/path/PathPlannerTrajectory.h>
 
namespace rmb {
 
class BaseDrive {
protected:
  BaseDrive() = default;
 
  // TODO: Modifyable for compatability with LimeLight.
 
  BaseDrive(std::string visionTable);
 
  virtual ~BaseDrive();
 
public:
  //---------------
  // Drive Methods
  //---------------
 
  virtual void driveChassisSpeeds(frc::ChassisSpeeds chassisSpeeds) = 0;
 
  virtual frc::ChassisSpeeds getChassisSpeeds() const = 0;
 
  //------------------
  // Odometry Methods
  //------------------
 
  virtual frc::Pose2d getPose() const = 0;
 
  virtual frc::Pose2d updatePose() = 0;
 
  virtual void addVisionMeasurments(const frc::Pose2d &poseEstimate,
                                    units::second_t time) = 0;
 
  virtual void setVisionSTDevs(wpi::array<double, 3> standardDevs) = 0;
 
  virtual void resetPose(const frc::Pose2d &pose = frc::Pose2d()) = 0;
 
  //----------------------
  // Trajectory Following
  //----------------------
 
  virtual bool isHolonomic() const = 0;
 
  virtual frc2::CommandPtr followWPILibTrajectory(
      frc::Trajectory trajectory,
      std::initializer_list<frc2::Subsystem *> driveRequirements) = 0;
 
  virtual frc2::CommandPtr followWPILibTrajectoryGroup(
      std::vector<frc::Trajectory> trajectoryGroup,
      std::initializer_list<frc2::Subsystem *> driveRequirements);
 
  virtual frc2::CommandPtr
  followPPPath(std::shared_ptr<pathplanner::PathPlannerPath> path,
               std::initializer_list<frc2::Subsystem *> driveRequirements) = 0;
 
  //   virtual frc2::CommandPtr followPPTrajectoryGroup(
  //       std::vector<pathplanner::PathPlannerTrajectory> trajectoryGroup,
  //       std::initializer_list<frc2::Subsystem *> driveRequirements);
 
  virtual frc2::CommandPtr followPPPathWithEvents(
      pathplanner::PathPlannerPath path,
      std::unordered_map<std::string, std::shared_ptr<frc2::Command>> eventMap,
      std::initializer_list<frc2::Subsystem *> driveRequirements);
 
  //   virtual frc2::CommandPtr followPPTrajectoryGroupWithEvents(
  //       std::vector<pathplanner::PathPlannerTrajectory> trajectoryGroup,
  //       std::unordered_map<std::string, std::shared_ptr<frc2::Command>>
  //       eventMap, std::initializer_list<frc2::Subsystem *>
  //       driveRequirements);
 
  virtual frc2::CommandPtr fullPPAuto(
      pathplanner::PathPlannerPath path,
      std::unordered_map<std::string, std::shared_ptr<frc2::Command>> eventMap,
      std::initializer_list<frc2::Subsystem *> driveRequirements);
 
  virtual frc2::CommandPtr fullPPAuto(
      std::vector<pathplanner::PathPlannerTrajectory> trajectoryGroup,
      std::unordered_map<std::string, std::shared_ptr<frc2::Command>> eventMap,
      std::initializer_list<frc2::Subsystem *> driveRequirements);
 
protected:
  //---------------
  // Vision Thread
  //---------------
 
  nt::DoubleArraySubscriber poseSubscriber;
 
  nt::DoubleArraySubscriber stdDevSubscriber;
 
  NT_Listener poseListener;
 
  NT_Listener stdDevListener;
};
} // namespace rmb