VisionSensor.h

 
#pragma once
 
#include <memory>
#include <string>
 
#include <frc/geometry/Pose3d.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>
#include <units/temperature.h>
#include <units/time.h>
#include <units/velocity.h>
 
#include "valkyrie/sensors/BaseSensor.h"
 
namespace valor {
 
#define KLIMELIGHT -29.8f  // degrees; TODO: Make Modular
 
class VisionSensor : public valor::BaseSensor<frc::Pose3d> {
   public:
    VisionSensor(frc::TimedRobot& robot, std::string name, frc::Pose3d _cameraPose);
 
    enum PipeLines {
        PIPELINE_0,
        PIPELINE_1,
        PIPELINE_2,
        PIPELINE_3,
        PIPELINE_4,
        PIPELINE_5,
    };
 
    void reset() override;
 
    void setCameraPose(frc::Pose3d camPose);
 
    void setPipe(PipeLines _pipe);
 
    bool hasTarget();
 
    void setThrottle(int skippedFrames);
 
    void captureVideo(units::second_t captureTime);
 
    units::velocity::meters_per_second_t getError(int pipe, double kPLimeLight = 0.7);
 
   protected:
    units::millisecond_t getTotalLatency();
 
    virtual frc::Pose3d getGlobalPose() = 0;
 
    void calculate() override;
 
    double tx, ty, tv, fps, ramUsage, ta;  
    int pipe;                              
    units::celsius_t cpuTemp, temp;        
 
    frc::Pose3d cameraPose;                       
    std::shared_ptr<nt::NetworkTable> limeTable;  
};
 
}  // namespace valor