SparkMaxVelocityController.h

#pragma once
 
#include <initializer_list>
 
#include <rev/CANSparkMax.h>
 
#include <units/angle.h>
#include <units/angular_acceleration.h>
#include <units/angular_velocity.h>
#include <units/current.h>
#include <units/time.h>
 
#include "rmb/motorcontrol/AngularVelocityController.h"
 
namespace rmb {
 
namespace SparkMaxVelocityControllerHelper {
struct MotorConfig {
  int id;
  rev::CANSparkMax::MotorType motorType =
      rev::CANSparkMax::MotorType::kBrushless;
  bool inverted = false;
 
  units::ampere_t currentLimit = 40.0_A;
  units::second_t openLoopRampRate = 0.5_s;
};
 
struct PIDConfig {
  double p = 0.0, i = 0.0, d = 0.0, ff = 0.0;
  units::radians_per_second_t tolerance = 0.0_rad_per_s;
  double iZone = 0.0, iMaxAccumulator = 0.0;
  double maxOutput = 1.0, minOutput = -1.0;
};
 
struct ProfileConfig {
  bool useSmartMotion = 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;
  rev::SparkMaxPIDController::AccelStrategy accelStrategy =
      rev::SparkMaxPIDController::AccelStrategy::kTrapezoidal;
 
  units::second_t closedLoopRampRate = 0.5_s;
};
 
enum EncoderType { HallSensor, Quadrature, Alternate, Absolute };
enum LimitSwitchConfig { Disabled, NormalyOpen, NormalyClosed };
 
struct FeedbackConfig {
  double gearRatio = 1.0;
  EncoderType encoderType = HallSensor;
  int countPerRev = 42;
  LimitSwitchConfig forwardSwitch = Disabled, reverseSwitch = Disabled;
};
} // namespace SparkMaxVelocityControllerHelper
 
class SparkMaxVelocityController : public AngularVelocityController {
public:
  using MotorConfig = SparkMaxVelocityControllerHelper::MotorConfig;
  using PIDConfig = SparkMaxVelocityControllerHelper::PIDConfig;
  using ProfileConfig = SparkMaxVelocityControllerHelper::ProfileConfig;
  using EncoderType = SparkMaxVelocityControllerHelper::EncoderType;
  using LimitSwitchConfig = SparkMaxVelocityControllerHelper::LimitSwitchConfig;
  using FeedbackConfig = SparkMaxVelocityControllerHelper::FeedbackConfig;
 
  struct CreateInfo {
    const MotorConfig motorConfig;
    const PIDConfig pidConfig = {};
    const ProfileConfig profileConfig = {};
    const FeedbackConfig feedbackConfig = {};
    std::initializer_list<const MotorConfig> followers;
  };
 
  SparkMaxVelocityController(SparkMaxVelocityController &&) = delete;
  SparkMaxVelocityController(const SparkMaxVelocityController &) = delete;
 
  SparkMaxVelocityController(const CreateInfo &createInfo);
 
  rev::CANSparkMax &getMotor();
 
  rev::SparkMaxPIDController &getPIDCOntroller();
 
  std::unique_ptr<rev::MotorFeedbackSensor> &getFeedbackSensor();
 
  //--------------------------------------------------
  // 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;
 
private:
  rev::CANSparkMax sparkMax;
  std::vector<std::unique_ptr<rev::CANSparkMax>> followers;
  rev::CANSparkMax::ControlType controlType;
 
  rev::SparkMaxPIDController pidController;
  units::radians_per_second_t targetVelocity = 0.0_rpm;
  units::radians_per_second_t tolerance;
 
  std::unique_ptr<rev::MotorFeedbackSensor> encoder;
  EncoderType encoderType;
  double gearRatio;
};
} // namespace rmb