diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 8fc1ca3..16c67de 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,6 +7,11 @@ package frc4388.robot; + +import frc4388.robot.subsystems.Vision; + +import java.util.Arrays; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -24,7 +29,7 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - + private Vision Vision = new Vision(); /** * This function is run when the robot is first started up and should be @@ -35,6 +40,7 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + } /** @@ -53,6 +59,7 @@ public class Robot extends TimedRobot { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + System.out.println(Arrays.toString(Vision.getAprilTags())); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index b5fe03d..3731849 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -24,7 +24,7 @@ public class Vision { public AprilTag[] getAprilTags() { if (!m_isTags.getBoolean(false)) return new AprilTag[0]; - + double xarr[] = m_xPoses.getDoubleArray(new double[] {}); double yarr[] = m_yPoses.getDoubleArray(new double[] {}); double zarr[] = m_zPoses.getDoubleArray(new double[] {});