log devilbot state

DevilBotz2876 · Mar 27, 2024 · ed6f833 · ed6f833
1 parent 1c6a725
commit ed6f833
Showing 1 changed file with 8 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/util/DevilBotState.java b/src/main/java/frc/robot/util/DevilBotState.java
@@ -1,11 +1,14 @@
 package frc.robot.util;
 
+import java.util.Optional;
+
+import org.littletonrobotics.junction.Logger;
+
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import frc.robot.config.RobotConfig;
 import frc.robot.config.RobotConfig.ArmConstants;
 import frc.robot.config.RobotConfig.ShooterConstants;
-import java.util.Optional;
 
 import org.littletonrobotics.junction.AutoLogOutput;
 
@@ -206,6 +209,7 @@ public static double getVisionRobotYawToTarget() {
         yawToTarget -= visionYawToTarget.get();
       }
     }
+    Logger.recordOutput("DevilBotState/yawToTarget", yawToTarget);
     return yawToTarget;
   }
 
@@ -220,7 +224,9 @@ public static double getVisionRobotXToAmpTarget() {
 
 
     double xToTarget = distanceToTarget * Math.sin(Math.toRadians(RobotConfig.drive.getAngle())+Math.toRadians(yawToTarget));
-
+
+    Logger.recordOutput("DevilBotState/distanceToTarget", yawToTarget);
+    Logger.recordOutput("DevilBotState/xToTarget", yawToTarget);
     return xToTarget;
 }