GamePieceSensor.h

 
#pragma once
 
#include "valkyrie/sensors/VisionSensor.h"
#include <frc/geometry/Pose3d.h>
#include <frc/estimator/SwerveDrivePoseEstimator.h>
#include <wpi/sendable/SendableBuilder.h>
#include <units/length.h>
 
namespace valor {
 
class GamePieceSensor : public VisionSensor {
   public:
    GamePieceSensor(frc::TimedRobot& robot, const char* name, frc::Pose3d cameraPose, frc::SwerveDrivePoseEstimator<4>* estimator);
 
    frc::Pose3d getGlobalPose();
    double GetRotationCmdDeg();
    bool detectedTarget();
    double getTa();
 
    void updateRelativeToCenter();
    void LoggablePeriodic() override;
 
   private:
    frc::SwerveDrivePoseEstimator<4>* estimator;
 
    struct RelativePose2d {
        units::meter_t x{0_m};
        units::meter_t y{0_m};
    };
 
    RelativePose2d relativePoseFromCenter;
    RelativePose2d relativePoseFromCamera;
 
    double rotationCmdDeg = 0.0;
};
 
}  // namespace valor