TalonFXPositionController.h

#pragma once
 
#include <ctre/phoenix6/CANcoder.hpp>
#include <ctre/phoenix6/TalonFX.hpp>
 
#include "rmb/motorcontrol/AngularPositionController.h"
 
#include "units/angle.h"
#include "units/angular_acceleration.h"
#include "units/angular_velocity.h"
#include "units/base.h"
#include "units/current.h"
#include "units/time.h"
 
#include <optional>
 
namespace rmb {
namespace TalonFXPositionControllerHelper {
struct MotorConfig {
  int id;
  bool inverted = false;
  bool brake = false;
  double minOutput = -1.0,
         maxOutput = 1.0; /*< Applies to both open and closed loop*/
};
 
struct CurrentLimits {
  units::ampere_t statorCurrentLimit = 0.0_A;
  bool statorCurrentLimitEnable = false;
  units::ampere_t supplyCurrentLimit = 0_A;
 
  bool supplyCurrentLimitEnable = false;
  units::ampere_t supplyCurrentThreshold = 0_A;
  units::second_t supplyTimeThreshold = 0.0_s;
};
 
struct OpenLoopConfig {
  units::second_t rampRate = 0.0_s;
};
 
struct PIDConfig {
  double p = 0.0, i = 0.0, d = 0.0, ff = 0.0;
  // units::turn_t tolerance = 0.0_rad; /*< Can't find a way to implement in the
  // new Pheonix V6 API*/ double closedLoopMaxPercentOutput = 1.0; /*< Can't
  // find a way to implement in the new Pheonix V6 API*/
 
  units::second_t rampRate = 0.0_s; /*< Amount of time it takes to go from 0 to
                                       full throttle. 0_s disables*/
};
 
struct Range {
  units::radian_t minPosition =
      -std::numeric_limits<units::radian_t>::infinity();
  units::radian_t maxPosition =
      std::numeric_limits<units::radian_t>::infinity();
 
  bool continuousWrap =
      true; /*< If false, the encoder will keep track of overflow, meaning the
                position value of the internal encoder, say will go from 2047 to
                2048 to 2049 as you keep turning it. Otherwise it will go from
                2047 to 0*/
};
 
struct ProfileConfig {
  bool useSmartMotion = /*< Note to future users: Don't use smart motion!*/
      false;
  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;
};
 
enum LimitSwitchConfig { Disabled, NormalyOpen, NormalyClosed };
 
struct FeedbackConfig {
  double sensorToMechanismRatio = 1.0;
  LimitSwitchConfig forwardSwitch = Disabled, reverseSwitch = Disabled;
};
 
struct CANCoderConfig {
  int id;
 
  units::radian_t magnetOffset = 0.0_rad;
};
 
} // namespace TalonFXPositionControllerHelper
class TalonFXPositionController : public AngularPositionController {
public:
  struct CreateInfo {
    TalonFXPositionControllerHelper::MotorConfig config;
    TalonFXPositionControllerHelper::PIDConfig pidConfig;
    TalonFXPositionControllerHelper::Range range;
    TalonFXPositionControllerHelper::FeedbackConfig feedbackConfig;
    TalonFXPositionControllerHelper::OpenLoopConfig openLoopConfig;
    TalonFXPositionControllerHelper::CurrentLimits currentLimits;
    std::optional<TalonFXPositionControllerHelper::CANCoderConfig>
        canCoderConfig;
  };
 
  TalonFXPositionController(const CreateInfo &createInfo);
 
  virtual ~TalonFXPositionController() = default;
 
  void setPosition(units::radian_t position) override;
 
  void setPower(double power) override;
 
  virtual double getPower() const override;
 
  units::radian_t getTargetPosition() const override;
 
  units::radian_t getMinPosition() const override;
 
  units::radian_t getMaxPosition() const override;
 
  void disable() override;
 
  void stop() override;
 
  units::radians_per_second_t getVelocity() const override;
 
  units::radian_t getPosition() const override;
 
  void setEncoderPosition(units::radian_t position = 0_rad) override;
 
  units::radian_t getTolerance() const override { return 0.0_tr; }
 
  void follow(const TalonFXPositionController &parentController, bool inverted);
 
private:
  // mutable ctre::phoenix::motorcontrol::can::WPI_TalonFX motorcontroller;
  mutable ctre::phoenix6::hardware::TalonFX motorcontroller;
 
  // std::optional<ctre::phoenix::sensors::WPI_CANCoder> canCoder;
  mutable std::optional<ctre::phoenix6::hardware::CANcoder> canCoder;
 
  TalonFXPositionControllerHelper::Range range;
 
  float sensorToMechanismRatio = 0.0;
 
  const bool usingCANCoder;
};
} // namespace rmb