dggggc
This commit is contained in:
@ -32,7 +32,7 @@ public class BasePilotable extends SubsystemBase {
|
||||
return gyroscope.getPitch();
|
||||
}
|
||||
|
||||
public void drive(double xSpeed, double zRotation, double d){
|
||||
public void drive(double xSpeed, double zRotation){
|
||||
drive.arcadeDrive(xSpeed, zRotation);
|
||||
}
|
||||
public double distance(){
|
||||
|
@ -6,6 +6,7 @@ package frc.robot.subsystems;
|
||||
|
||||
import org.photonvision.PhotonCamera;
|
||||
import org.photonvision.common.hardware.VisionLEDMode;
|
||||
import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
|
||||
import edu.wpi.first.net.PortForwarder;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
@ -38,6 +39,18 @@ public class Limelight extends SubsystemBase {
|
||||
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() {
|
||||
// This method will be called once per scheduler run
|
||||
|
Reference in New Issue
Block a user