counter_yaw_modder.hpp

Go to the documentation of this file.

#pragma once
#include "wrap/trap/algorithms/ipid_modder.hpp"
#include "wrap/rail/telemetry/iangular_velocity_telemetry.hpp"
 
#include <memory>
#include <cmath>
 
namespace fang::turret
{
class CounterYawModder : public trap::algorithms::IPidModder<RPM>
{
public:
    struct Config
    {
        double correctionScale = 1.0;
    };
 
    CounterYawModder(std::unique_ptr<telemetry::IAngularVelocity> baseYawVelocitySensor, const Config& config):
        baseYawVelocitySensor_{std::move(baseYawVelocitySensor)},
        config_{config}
    {}
 
    RPM getModdedOutput(const RPM& output) override
    {
        // These calculations are not unit sensitive
        const RPM kBaseSpeed{baseYawVelocitySensor_->getAngularVelocity()};
        const RPM kCounterRotation{-kBaseSpeed};
 
        // Account for slight innacuracies
        const RPM kCorrection{config_.correctionScale * kCounterRotation};
 
        // The turret is targeting a specific position, but
        // the rotation of the base should not affect the final position
        // or else it would keep chasing said positoin
        return output + kCorrection;
    };
private:
    std::unique_ptr<telemetry::IAngularVelocity> baseYawVelocitySensor_;
    Config config_;
};
}