TalonFXVelocityController.h

#pragma once
 
#include <optional>
 
#include "rmb/motorcontrol/AngularVelocityController.h"
 
#include "TalonFXPositionController.h"
#include "units/angular_velocity.h"
 
#include <ctre/phoenix6/CANcoder.hpp>
#include <ctre/phoenix6/TalonFX.hpp>
 
namespace rmb {
namespace TalonFXVelocityControllerHelper {
struct PIDConfig {
  double p = 0.0, i = 0.0, d = 0.0, ff = 0.0;
 
  units::second_t rampRate = 1.0_s;
};
 
struct OpenLoopConfig {
  double minOutput = -1.0, maxOutput = 1.0;
  units::second_t rampRate = 1.0_s;
};
 
struct ProfileConfig {
  units::radians_per_second_t maxVelocity = 0.0_rad_per_s,
                              minVelocity = 0.0_rad_per_s;
  units::radians_per_second_squared_t maxAcceleration = 0.0_rad_per_s_sq;
};
} // namespace TalonFXVelocityControllerHelper
 
class TalonFXVelocityController : public AngularVelocityController {
public:
  struct CreateInfo {
    TalonFXPositionControllerHelper::MotorConfig config;
    TalonFXVelocityControllerHelper::PIDConfig pidConfig;
    TalonFXVelocityControllerHelper::ProfileConfig profileConfig;
    TalonFXPositionControllerHelper::FeedbackConfig feedbackConfig;
    TalonFXVelocityControllerHelper::OpenLoopConfig openLoopConfig;
    TalonFXPositionControllerHelper::CurrentLimits currentLimits;
    std::optional<TalonFXPositionControllerHelper::CANCoderConfig>
        canCoderConfig;
  };
 
  TalonFXVelocityController(const CreateInfo &createInfo);
 
  virtual ~TalonFXVelocityController() = default;
 
  //--------------------------------------------------
  // Methods Inherited from AngularVelocityController
  //--------------------------------------------------
 
  void setVelocity(units::radians_per_second_t velocity) override;
 
  units::radians_per_second_t getTargetVelocity() const override;
 
  virtual void setPower(double power) override;
 
  virtual double getPower() const override;
 
  virtual void disable() override;
 
  virtual void stop() override;
 
  //---------------------------------------
  // Methods Inherited from AngularEncoder
  //---------------------------------------
 
  units::radians_per_second_t getVelocity() const override;
 
  units::radian_t getPosition() const override;
 
  void setEncoderPosition(units::radian_t position = 0_rad) override;
 
  //----------------------------------------------------------
  // Methods Inherited from AngularvelocityFeedbackController
  //----------------------------------------------------------
 
  virtual units::radians_per_second_t getTolerance() const override {
    return 0.0_rad_per_s;
  }
 
  void follow(const TalonFXVelocityController &parentController, bool inverted);
 
private:
  mutable ctre::phoenix6::hardware::TalonFX motorcontroller;
 
  units::radians_per_second_t tolerance = 0.0_tps;
 
  TalonFXVelocityControllerHelper::ProfileConfig profileConfig;
 
  const bool usingCANCoder;
 
  mutable std::optional<ctre::phoenix6::hardware::CANcoder> canCoder;
};
 
} // namespace rmb