tilt_pid_modder.hpp

Go to the documentation of this file.

#pragma once
#include "wrap/trap/algorithms/ipid_modder.hpp"
#include "wrap/rail/telemetry/iangular_position_telemetry.hpp"
 
#include <memory>
#include <cmath>
 
namespace fang::turret
{
template <typename Output> 
class TiltPidModder : public trap::algorithms::IPidModder<Output>
{
public:
    struct Config
    {
        double correctionScaler
    };
 
    TiltPidModder(std::unique_ptr<telemetry::IAngularPosition> fieldPitchSensor, const Config& config):
        fieldPitchSensor_{std::move(fieldPitchSensor)},
        config_{config}
    {}
 
    Output getModdedOutput(const Output& output) override
    {
        // These calculations are not unit sensitive
        const double kRawAngle{static_cast<double>(fieldPitchSensor_->getAngularPosition())};
 
        // Solve for the equilibrium of an tall object hinged at the bottom
        // which is under the influence of gravity
        // The moment at the hinge should keep it in equilibrium in all positions
 
        // If the head is tilted down, the angle is negative, a positive force must counteract it
        // Therefore, a negative is required.
        // This provides the archetype
        const double kPhysicsCorrection{-std::sin(kRawAngle)};
 
        // It must be scaled to various turret head sizes
        const double kCorrection{config_.correctionScaler * kPhysicsCorrection};
        const Output kOutputCorrection{static_cast<Output>(kCorrection)};
 
        // The turret is targeting a specific position, but
        // The correct should remove the influence of gravity
        // So that its ability to achieve the desired position is unhindered
        return Output{kOutputCorrection + output};
    };
private:
    std::unique_ptr<telemetry::IAngularPosition> fieldPitchSensor_;
    Config config_;
};
}