83 lines
3.1 KiB
Java
83 lines
3.1 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_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_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 > x2){
|
|
// if(y1 > y2){
|
|
// return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle;
|
|
// }
|
|
// else{
|
|
// return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+90 - angle;
|
|
// }
|
|
// }
|
|
// else{
|
|
// if(y1 > y2){
|
|
// return Math.atan((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle;
|
|
// }
|
|
// else{
|
|
// return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle;
|
|
// }
|
|
// }
|
|
if(y1 > y2){
|
|
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle;
|
|
}
|
|
else{
|
|
return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)-360 - angle;
|
|
}
|
|
}
|
|
@Override
|
|
public void periodic() {
|
|
// This method will be called once per scheduler run
|
|
}
|
|
}
|