Vision (Basic)

[sillylittlerobots]

Since we are using photonvision, the setup for the robot-vision interface is quite simple.

https://docs.photonvision.org/en/latest/docs/integration/index.html - A really good guide, the whole documentation is what you are looking for

Requirements:

• A function that gets a 2D pose from a known april tag
• A function that can convert the 2D pose to an x and y displacement value for the top view of the field
• A function that gives an orientation from an april tag

Steps:

1. The physical setup for the photonvision system is Alec's responsibility at this point, so whoever works on this should discuss with him.
2. Using the documentation linked and code examples, make functions that can do the 3 things above in a class.
   I've attached an example of this type of class.

[sillylittlerobots]

package frc.robot.Utilities;

import edu.wpi.first.networktables.NetworkTableInstance;
import frc.robot.Constants;
/**
 * Put docs here // TODO
 */
public class Limelight {
    private String names;
    public Limelight(String name) {
      names = name;
    }



    public boolean getTargetAcquired() {
      //setPipeline(0);
      double targetAcquired = NetworkTableInstance.getDefault().getTable(names).getEntry("tv").getDouble(0.0);
      boolean yesorno;
      if(targetAcquired == 1.0){
        yesorno = true;
      }
      else{
        yesorno = false;
      }
      return yesorno; 
    }

    public double getAngleToTarget() {
      //setPipeline(0);
      double tx = NetworkTableInstance.getDefault().getTable(names).getEntry("tx").getDouble(0.0);
      return tx;
    }
    public double getVerticalAngle(){
      double ty = NetworkTableInstance.getDefault().getTable(names).getEntry("ty").getDouble(0.0);
      return ty;
    }

    public void disableVision() {
      setPipeline(0);
    }

    private void setPipeline(int pipelineId) {
      NetworkTableInstance.getDefault().getTable(names).getEntry("pipeline").setNumber(pipelineId);
    }
    

    public void enableTopVision() {
      setPipeline(1);
    }

    public void enableBotVision() {
      setPipeline(2);
    }
    public double getHorizontalDistance(String strn){
      if((Constants.kLimelightMountingAngle + getVerticalAngle())  != Constants.kLimelightMountingAngle && strn == "TOP"){
        return (Constants.kHighTargetHeight - Constants.kLimelightFocalHeight) / Math.tan(Constants.kLimelightMountingAngle + Math.abs(getVerticalAngle()));
      }else if((Constants.kLimelightMountingAngle + getVerticalAngle())  != Constants.kLimelightMountingAngle && strn == "MID" ){
        return (Constants.kMidTargetHeight - Constants.kLimelightFocalHeight) / Math.tan(Constants.kLimelightMountingAngle + Math.abs(getVerticalAngle()));

      }
      else{
        return Constants.kLimelightErrorValue;
      }
    }
}