88 lines
3.2 KiB
Java
88 lines
3.2 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.net.PortForwarder;
|
|
import edu.wpi.first.networktables.NetworkTable;
|
|
import edu.wpi.first.networktables.NetworkTableEntry;
|
|
import edu.wpi.first.networktables.NetworkTableInstance;
|
|
import edu.wpi.first.wpilibj.Timer;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import frc.robot.LimelightHelpers;
|
|
|
|
public class Limelight3 extends SubsystemBase {
|
|
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
|
|
NetworkTableEntry pipeline = table.getEntry("pipeline");
|
|
/** Creates a new Limelight3. */
|
|
public Limelight3() {
|
|
|
|
for(int port = 5800; port <=5807; port++){
|
|
PortForwarder.add(port, "limelight.local", port);
|
|
}
|
|
}
|
|
public double[] getTargetPose(){
|
|
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
|
|
NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace");
|
|
double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]);
|
|
return targetPose;
|
|
}
|
|
public double[] getBotPoseBlue(){
|
|
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
|
|
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpiblue");
|
|
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
|
|
return BotPose;
|
|
}
|
|
public double[] getBotPoseRed(){
|
|
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
|
|
NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_orb_wpired");
|
|
double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
|
|
return BotPose;
|
|
}
|
|
public double getTx(){
|
|
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
|
|
NetworkTableEntry tx = table.getEntry("tx");
|
|
return tx.getDouble(0.0);
|
|
}
|
|
public double getTA(){
|
|
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
|
|
NetworkTableEntry ta = table.getEntry("ta");
|
|
return ta.getDouble(0.0);
|
|
}
|
|
public double getTId(){
|
|
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
|
|
NetworkTableEntry tid = table.getEntry("tid");
|
|
return tid.getDouble(0.0);
|
|
}
|
|
public double getRx(){
|
|
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
|
|
NetworkTableEntry rx = table.getEntry("rx");
|
|
return rx.getDouble(0.0);
|
|
}
|
|
public boolean getV(){
|
|
return LimelightHelpers.getTV("limelight-balon");
|
|
}
|
|
public void Apriltag(){
|
|
pipeline.setNumber(1);
|
|
}
|
|
public void Forme(){
|
|
pipeline.setNumber(0);
|
|
}
|
|
public double Calcule(double x1, double x2, double y1, double y2, double angle)
|
|
{
|
|
if (x1 > 4)
|
|
{
|
|
return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) - angle)/90;
|
|
}
|
|
else
|
|
{
|
|
return (Math.abs(Math.atan2(x2 - x1, y2 - y1)) * (180 / Math.PI) + angle) * -1/90;
|
|
}
|
|
}
|
|
@Override
|
|
public void periodic() {
|
|
// This method will be called once per scheduler run
|
|
}
|
|
}
|