robot-2023/src/main/java/frc/robot/subsystems/Limelight.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.subsystems;

import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;

import org.photonvision.PhotonCamera;
import org.photonvision.common.hardware.VisionLEDMode;


import edu.wpi.first.net.PortForwarder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;


public class Limelight extends SubsystemBase {
  ShuffleboardTab teb = Shuffleboard.getTab("teb");
  PhotonCamera limelight = new PhotonCamera("limelight");
  /** Creates a new Limelight. */
  public Limelight() {
    CameraServer.startAutomaticCapture();
    teb .add("limelight", 0.1);
    PortForwarder.add(5800, "photonvision.local", 5800);
  }

  public void cube()  {
    limelight.setLED(VisionLEDMode.kOff);
    limelight.setPipelineIndex(3);
  }

  public void cone() {
    limelight.setLED(VisionLEDMode.kOff);
    limelight.setPipelineIndex(2);
  }

  public void apriltag() {
    limelight.setLED(VisionLEDMode.kOff);
    limelight.setPipelineIndex(0);
  }

  public void tape() {
    limelight.setLED(VisionLEDMode.kOn);
    limelight.setPipelineIndex(1);
  }

  public double getYaw() {
    var result = limelight.getLatestResult();
    if(result.hasTargets()){
    return  result.getBestTarget().getYaw();
    }
    return 0;
  
  }
  public void pilote(){
    limelight.setLED(VisionLEDMode.kOff);
    limelight.setDriverMode(true);
  }
  @Override
  public void periodic() {
  }
}