ServoPositionController.h

 
#pragma once
 
#include <units/angle.h>
#include <units/angular_velocity.h>
 
#include <frc/Servo.h>
 
#include "rmb/motorcontrol/AngularPositionController.h"
 
namespace rmb {
 
class ServoPositionController : public AngularPositionController {
public:
  ServoPositionController(const ServoPositionController &) = delete;
  ServoPositionController(ServoPositionController &&) = default;
 
  ServoPositionController(int channel) : servo(channel) {}
 
  virtual ~ServoPositionController() = default;
 
  virtual void setPosition(units::radian_t position) {
    units::radian_t tagrePosition =
        std::clamp(position * inversion, minPosition, maxPosition);
    return servo.SetAngle(units::degree_t(tagrePosition).to<double>());
  }
 
  virtual units::radian_t getTargetPosition() const {
    return units::degree_t(servo.GetAngle() * inversion);
  }
 
  virtual units::radian_t getMinPosition() const { return minPosition; }
 
  virtual units::radian_t getMaxPosition() const { return maxPosition; }
 
  virtual void setInverted(bool isInverted) { inversion = isInverted ? -1 : 1; }
 
  virtual bool getInverted() const { return inversion == 1; }
 
  virtual void disable() { servo.SetDisabled(); }
 
  virtual void stop() { servo.SetDisabled(); }
 
private:
  frc::Servo servo;
  int inversion = 1;
  units::radian_t minPosition = 0_deg;
  units::radian_t maxPosition = 180_deg;
};
} // namespace rmb