69 lines
2.6 KiB
Java
69 lines
2.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.net.PortForwarder;
|
|
import edu.wpi.first.networktables.NetworkTable;
|
|
import edu.wpi.first.networktables.NetworkTableEntry;
|
|
import edu.wpi.first.networktables.NetworkTableInstance;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import frc.robot.LimelightHelpers;
|
|
|
|
public class Limelight3G extends SubsystemBase {
|
|
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
|
|
NetworkTableEntry pipeline = table.getEntry("pipeline");
|
|
/** Creates a new LimeLight3. */
|
|
public Limelight3G() {
|
|
for(int port = 5800; port <=5807; port++){
|
|
PortForwarder.add(port, "limelight.local", port);
|
|
}
|
|
}
|
|
public double[] getBotPoseBlue(){
|
|
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-tag");
|
|
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-tag");
|
|
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-tag");
|
|
NetworkTableEntry tx = table.getEntry("tx");
|
|
return tx.getDouble(0.0);
|
|
}
|
|
public double getTId(){
|
|
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
|
|
NetworkTableEntry tid = table.getEntry("tid");
|
|
return tid.getDouble(0.0);
|
|
}
|
|
public double getTA(){
|
|
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag");
|
|
NetworkTableEntry ta = table.getEntry("ta");
|
|
return ta.getDouble(0.0);
|
|
}
|
|
public boolean getV(){
|
|
return LimelightHelpers.getTV("limelight-tag");
|
|
}
|
|
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
|
|
}
|
|
}
|