CANrangeSensor.h

#pragma once
 
#include <memory>
#include <string>
 
#include <ctre/phoenix6/core/CoreCANrange.hpp>
#include <frc/TimedRobot.h>
#include <units/length.h>
#include <valkyrie/sensors/LaserProximitySensor.h>
 
namespace valor {
namespace sensors {

class CANrangeSensor : public LaserProximitySensor, public ctre::phoenix6::hardware::core::CoreCANrange {
   public:
    CANrangeSensor(int deviceId, std::string canbus);
 
    inline CANrangeSensor() : ctre::phoenix6::hardware::core::CoreCANrange{-1} {}
 
    inline void ApplyConfig() { GetConfigurator().Apply(config); }
 
    void SetThresholdDistance(units::millimeter_t threshold, bool saveImmediately);
 
    inline void SetThresholdDistance(units::millimeter_t threshold) { SetThresholdDistance(threshold, false); }
 
    void SetHysteresis(units::millimeter_t hysteresis, bool saveImmediately = false);
    void SetFOV(std::pair<units::degree_t, units::degree_t> fov = {15_deg, 15_deg},
                std::pair<units::degree_t, units::degree_t> center = {0_deg, 0_deg}, bool saveImmediately = false);
 
    bool IsHealthy() const override;
 
   private:
    ctre::phoenix6::configs::CANrangeConfiguration config;
};
 
}  // namespace sensors
}  // namespace valor