AngularFeedforwardController.h

 
#pragma once
 
#include <algorithm>
#include <limits>
 
#include <frc/motorcontrol/MotorController.h>
 
#include "rmb/motorcontrol/AngularVelocityController.h"
#include "rmb/motorcontrol/feedforward/Feedforward.h"
 
namespace rmb {
 
class AngularFeedforwardontroller : public AngularVelocityController {
public:
  AngularFeedforwardontroller(const AngularFeedforwardontroller &) = delete;
  AngularFeedforwardontroller(AngularFeedforwardontroller &&) = default;
 
  AngularFeedforwardontroller(
      std::unique_ptr<frc::MotorController> &&motorController,
      std::unique_ptr<rmb::Feedforward<units::radians>> &&feedforward)
      : motorController(std::move(motorController)),
        feedforward(std::move(feedforward)) {}
 
  void setVelocity(units::radians_per_second_t velocity) {
    targetVelocity = std::clamp(velocity, -maxVelocity, maxVelocity);
    motorController->SetVoltage(feedforward->calculate(targetVelocity));
  }
 
  units::radians_per_second_t getTargetVelocity() const {
    return targetVelocity;
  }
 
  void setMaxVelocity(units::radians_per_second_t max) { maxVelocity = max; }
 
  units::radians_per_second_t getMaxVelocity() const { return maxVelocity; }
 
  void setInverted(bool isInverted) {
    motorController->SetInverted(isInverted);
  }
 
  bool getInverted() const { motorController->GetInverted(); }
 
  void disable() {
    targetVelocity = 0.0_rpm;
    motorController->Disable();
  }
 
  void stop() {
    targetVelocity = 0.0_rpm;
    motorController->StopMotor();
  }
 
private:
  std::unique_ptr<frc::MotorController> motorController;
  std::unique_ptr<rmb::Feedforward<units::radians>> feedforward;
  units::radians_per_second_t targetVelocity = 0.0_rpm;
  units::radians_per_second_t maxVelocity =
      units::radians_per_second_t(std::numeric_limits<double>::infinity());
};
} // namespace rmb