[#289] Added stuff

frc-862 · Apr 13, 2023 · ab8bf20 · ab8bf20
1 parent 80147c9
commit ab8bf20
Showing 1 changed file with 14 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/commands/CubeAlign.java b/src/main/java/frc/robot/commands/CubeAlign.java
@@ -8,13 +8,16 @@
 import frc.robot.Constants.AutoAlignConstants;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.LimelightFront;
+import frc.thunder.limelightlib.LimelightHelpers;
+import frc.thunder.shuffleboard.LightningShuffleboard;
 import frc.thunder.swervelib.DriveController;
 
 public class CubeAlign extends CommandBase {
 
     private Drivetrain drivetrain;
     private LimelightFront limelightFront;
-    private double horizOffset;
+    private double horizOffsetLength;
+    private double horizOffsetDegrees;
     private Pose2d translatedPose = new Pose2d();
     private double XOffset = 0;
     private double YOffset = 0;
@@ -35,15 +38,21 @@ public void initialize() {
     @Override
     public void execute() {
         if (limelightFront.hasVision() && limelightFront.getPipelineNum() == 3){
-            horizOffset = limelightFront.getHorizontalOffset();
-            horizOffset *= AutoAlignConstants.HORIZONTAL_MULTIPLIER;
-            XOffset = horizOffset / Math.sin(0d/*GET FROM VISION*/);
-            YOffset = horizOffset / Math.tan(0d/*GET FROM VISION*/);
+            horizOffsetLength = limelightFront.getHorizontalOffset();
+            horizOffsetLength *= AutoAlignConstants.HORIZONTAL_MULTIPLIER;
+            horizOffsetDegrees = LimelightHelpers.getLimelightNTDouble("Retro", "txp");
+            XOffset = horizOffsetLength / Math.sin(horizOffsetDegrees);
+            YOffset = horizOffsetLength / Math.tan(horizOffsetDegrees);
         }
 
         translatedPose = new Pose2d(new Translation2d(drivetrain.getPose().getX() - XOffset, drivetrain.getPose().getY() - YOffset), drivetrain.getPose().getRotation());
 
         drivetrain.resetOdometry(translatedPose);
+
+        LightningShuffleboard.setDouble("Cube-Align", "X offset", XOffset);
+        LightningShuffleboard.setDouble("Cube-Align", "Y offset", YOffset);
+        LightningShuffleboard.setDouble("Cube-Align", "horizontal distance offset", horizOffsetLength);
+        LightningShuffleboard.setDouble("Cube-Align", "horizontal angle offset", horizOffsetLength);
     }
 
     @Override