text: // RobotBuilder Version: 2.0
  //
  // This file was generated by RobotBuilder. It contains sections of
  // code that are automatically generated and assigned by robotbuilder.
  // These sections will be updated in the future when you export to
  // C++ from RobotBuilder. Do not put any code or make any change in
  // the blocks indicating autogenerated code or it will be lost on an
  // update. Deleting the comments indicating the section will prevent
  // it from being updated in the future.
  
  #pragma once
  
  #include "frc2/command/SubsystemBase.h"
  #include "networktables/NetworkTable.h"
  #include "networktables/NetworkTableEntry.h"
  #include "networktables/NetworkTableInstance.h"
  #include "frc/smartdashboard/SmartDashboard.h"
  #include "frc/Preferences.h"
  
  #include "Limelight.h"
  #include "DataLogger.h"
  #include "hwcfg.h"
  
  #include <math.h>
  #include <cmath>
  #include <tuple>
  
  #define INCHES_TO_FT (1.0/12.0)
  
  /**
   *
   *
   * @author ExampleAuthor
   */
  class Vision: public frc2::SubsystemBase {
  private:
  	// It's desirable that everything possible is private except
  	// for methods that implement subsystem capabilities
  
  	// custom limelight object for communication with limelight
      const std::string limelightTableName = "limelight";
  	Limelight limelight = Limelight(limelightTableName);
  
  	// network table entries
  	nt::NetworkTableEntry nt_visionDrive;
  	nt::NetworkTableEntry nt_distance;
  	nt::NetworkTableEntry nt_kP_Omega;
  	nt::NetworkTableEntry nt_kI_Omega;
  	nt::NetworkTableEntry nt_angle_DB;
  	nt::NetworkTableEntry nt_kP_Distance;
  
  	// driverstation preferences
  	double acceptableAlignmentError = 0.8; // degrees
  	std::string acceptableAlignmentErrorPrefName = "Vision Acceptable Align Error";
  	double distanceSetpoint = 10; // optimal distance from powerport to shoot from (ft)
  	std::string distanceSetpointPrefName = "Vision Distance Setpoint";
  	bool takePeriodicSnapshots = true;
  	std::string takePeriodicSnapshotsPrefName = "Save Snapshots During VisionDrives";
  
  	// for taking periodic snapshots
  	const int loopsBetweenImages = 10;
  	int loopsSinceLastImage = 0;
  
  	// for distance calculation
  	const double powerportVisionTargetHeight = (6 + 9.25 * INCHES_TO_FT) + 
  	(1 + 5.0 * INCHES_TO_FT)/2; // height of the center of the vision target (ft)
  	const double cameraHeight = 44.0 * INCHES_TO_FT; // height that the camera is mounted at (ft)
  	const double cameraAngle = 35; // angle camera is mounted at from horizontal (degrees)
  	const double cameraDistanceFromFrontBumper = 30.0 * INCHES_TO_FT; // (ft)
  
  	// For VisionDrive PID controller
  	double angleErrorDeadband = 0.5; // degrees
  	double kP_Omega = -0.004; 
  	double kI_Omega = -0.001; 
  	double kP_Distance = -0;
  	double distanceErrorDeadband = 0; // feet 
  	double omegaLimiter = 0.5;
  	double omegaIntegrator = 0;
  	double speedLimiter = 0.5;
  	const double deltaTime = 0.02;
  
  	// for data logging
  	DataLogger visionLogger;
  	bool visionDriveActive = false;
  	double distance = 0;	
  	double distanceError = 0;
  	double distanceError_DB = 0;
  	double angleError = 0;
  	double angleError_DB = 0;
  	double speed = 0;
  	double omega = 0;
  
  	// makes sure the LED and vision processing is on only when vision drive is being used
  	// should be set to true during competitions, false when calibrating
  	bool competitionMode = true;
  	nt::NetworkTableEntry nt_competitionMode;
  
  public:
    Vision();
      enum Pipeline {powerport = 0};
  	void Periodic() override;
  	bool IsAlignedWithTarget();
  	double GetDistanceToPowerport();
  	void VisionDriveInit();
  	std::pair<double, double> AlignWithLockedTarget();
  	std::pair<double, double> DriveToDistanceSetpoint();
  	std::pair<double, double> DriveToLockedTarget();
  	void VisionDriveEnd();
  	void EnableLogging();
  	void DisableLogging();
  };
  