65 lines
1.6 KiB
Java
65 lines
1.6 KiB
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() {
|
|
}
|
|
}
|