TrajectoryRunnerProperties.hpp

#pragma once
 
#include <ThunderLib/Types/CanonicalAngle.hpp>
#include <frc/controller/HolonomicDriveController.h>
#include <frc/geometry/Pose2d.h>
#include <frc/kinematics/ChassisSpeeds.h>
#include <functional>
#include <memory>
 
namespace thunder {
 
struct TrajectoryRunnerProperties {
  struct DriveControllerInputData {
    frc::Pose2d currentPose;
    frc::Pose2d targetPose;
    CanonicalAngle heading;
    units::meters_per_second_t linearVelocity;
  };
 
  using GetPoseFunc = std::function<frc::Pose2d()>;
  using ResetPoseFunc = std::function<void(const frc::Pose2d&)>;
  using GetSpeedsFunc = std::function<frc::ChassisSpeeds()>;
  using SetSpeedsFunc = std::function<void(const frc::ChassisSpeeds&)>;
  using SetSpeedsWithDiagnosticsFunc =
      std::function<void(const frc::ChassisSpeeds&, const DriveControllerInputData&)>;
  using ControlFunc = std::function<void(const DriveControllerInputData&)>;
  using StopFunc = std::function<void()>;
 
  const GetPoseFunc getPose = nullptr;
 
  const ResetPoseFunc resetPose = nullptr;
 
  const SetSpeedsFunc setSpeeds = nullptr;
 
  const SetSpeedsWithDiagnosticsFunc setSpeedsWithDiagnostics = nullptr;
 
  const std::shared_ptr<frc::HolonomicDriveController> controller = nullptr;
 
  const ControlFunc control = nullptr;
 
  const StopFunc stop = nullptr;
 
  TrajectoryRunnerProperties(GetPoseFunc getPoseFunc,
                             ResetPoseFunc resetPoseFunc,
                             SetSpeedsFunc setSpeedsFunc,
                             std::shared_ptr<frc::HolonomicDriveController> holonomicDriveController)
      : getPose(std::move(getPoseFunc)),
        resetPose(std::move(resetPoseFunc)),
        setSpeeds(std::move(setSpeedsFunc)),
        controller(std::move(holonomicDriveController)) {}
 
  TrajectoryRunnerProperties(GetPoseFunc getPoseFunc,
                             ResetPoseFunc resetPoseFunc,
                             SetSpeedsWithDiagnosticsFunc setSpeedsWithDiagnosticsFunc,
                             std::shared_ptr<frc::HolonomicDriveController> holonomicDriveController)
      : getPose(std::move(getPoseFunc)),
        resetPose(std::move(resetPoseFunc)),
        setSpeedsWithDiagnostics(std::move(setSpeedsWithDiagnosticsFunc)),
        controller(std::move(holonomicDriveController)) {}
 
  TrajectoryRunnerProperties(GetPoseFunc getPose, ResetPoseFunc resetPose, ControlFunc control, StopFunc stop)
      : getPose(std::move(getPose)),
        resetPose(std::move(resetPose)),
        control(std::move(control)),
        stop(std::move(stop)) {}
 
  bool isValid() const {
    if (getPose == nullptr)
      return false;
 
    if (setSpeeds != nullptr || setSpeedsWithDiagnostics != nullptr) {
      if (controller == nullptr)
        return false;
    } else {
      return control != nullptr && stop != nullptr;
    }
 
    return true;
  }
 
  explicit operator bool() const { return isValid(); }
};
 
}  // namespace thunder