VisionSubsystem.h

Go to the documentation of this file.

#pragma once

#include <frc2/command/SubsystemBase.h>

#include <frc/smartdashboard/SmartDashboard.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>

#include <wpi/math>

#include "Constants.h"
#include "common/Util.h"

using namespace std;
using namespace frc;
using namespace VisionConstants;

class VisionSubsystem : public frc2::SubsystemBase
{
public:
    VisionSubsystem();

    void Periodic() override;

    bool GetValidTarget();
    double GetDistance();
    double GetAngle();
    void SetLED(bool on);

protected:
    double DegreesToRadians(double degrees);

private:    
    shared_ptr<NetworkTable> m_dashboard;
    shared_ptr<NetworkTable> m_networktable;
    bool m_led;

    double m_tx;
    double m_ty;
    bool m_validTarget;
    vector<double> m_averageDistance;
    vector<double> m_averageAngle;
};