2023-02-16 18:46:43 -07:00
|
|
|
package frc4388.robot.subsystems;
|
|
|
|
|
|
2023-02-28 17:05:18 -07:00
|
|
|
import frc4388.robot.subsystems.Apriltags.Tag;
|
2023-02-16 18:46:43 -07:00
|
|
|
|
|
|
|
|
public class Location {
|
2023-02-28 17:05:18 -07:00
|
|
|
final Apriltags apriltag = new Apriltags();
|
2023-02-16 18:46:43 -07:00
|
|
|
|
2023-02-27 18:42:27 -07:00
|
|
|
private boolean isLimelight = false;
|
|
|
|
|
private boolean isApriltag = false;
|
2023-02-16 18:46:43 -07:00
|
|
|
|
|
|
|
|
//Determines which source to get pos and rot from and also resets
|
2023-02-28 17:05:18 -07:00
|
|
|
private void reoderPrio(){
|
2023-02-16 18:46:43 -07:00
|
|
|
isLimelight = false; //If limelight gets position and if within a certain range of poles
|
2023-02-28 17:05:18 -07:00
|
|
|
isApriltag = apriltag.isAprilTag();
|
2023-02-16 18:46:43 -07:00
|
|
|
}
|
|
|
|
|
|
2023-02-28 17:05:18 -07:00
|
|
|
public Tag getPosRot() {
|
|
|
|
|
reoderPrio();
|
|
|
|
|
if(isApriltag){
|
|
|
|
|
return apriltag.getTagPosRot();
|
|
|
|
|
} else if (isLimelight) {
|
|
|
|
|
return null;
|
2023-02-16 18:46:43 -07:00
|
|
|
}
|
|
|
|
|
|
2023-02-16 19:46:21 -07:00
|
|
|
return null;
|
2023-02-16 18:46:43 -07:00
|
|
|
}
|
|
|
|
|
}
|