[#289] he doesn't know

frc-862 · Apr 14, 2023 · 9dc1a73 · 9dc1a73
1 parent ab8bf20
commit 9dc1a73
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -33,6 +33,7 @@
 import frc.robot.commands.AutoScore;
 import frc.robot.Constants.LiftConstants.LiftState;
 import frc.robot.commands.Collect;
+import frc.robot.commands.CubeAlign;
 import frc.robot.commands.EleUpInCommunity;
 import frc.robot.commands.SwerveDrive;
 import frc.robot.commands.HoldPower;
@@ -132,9 +133,10 @@ protected void configureButtonBindings() {
         new Trigger(driver::getStartButton).onTrue(new InstantCommand(servoturn::flickServo));
 
         // AutoAlign based on cone or cube
-        new Trigger(driver::getBButton).whileTrue(new ConditionalCommand(new AprilTagLineUp(drivetrain, frontLimelight, collector), new RetroLineUp(drivetrain, frontLimelight, collector),
-                () -> collector.getGamePiece() == GamePiece.CUBE));
+        // new Trigger(driver::getBButton).whileTrue(new ConditionalCommand(new AprilTagLineUp(drivetrain, frontLimelight, collector), new RetroLineUp(drivetrain, frontLimelight, collector),
+        //         () -> collector.getGamePiece() == GamePiece.CUBE));
 
+        new Trigger(driver::getBButton).whileTrue(new CubeAlign(drivetrain, frontLimelight));
         //AUTOBALANCE
         // new Trigger(driver::getBButton).whileTrue(new AutoBalance(drivetrain));
 

diff --git a/src/main/java/frc/robot/commands/CubeAlign.java b/src/main/java/frc/robot/commands/CubeAlign.java
@@ -1,5 +1,6 @@
 package frc.robot.commands;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
@@ -13,20 +14,23 @@
 import frc.thunder.swervelib.DriveController;
 
 public class CubeAlign extends CommandBase {
-    
+
     private Drivetrain drivetrain;
     private LimelightFront limelightFront;
     private double horizOffsetLength;
     private double horizOffsetDegrees;
     private Pose2d translatedPose = new Pose2d();
     private double XOffset = 0;
     private double YOffset = 0;
+    private PIDController xController = new PIDController(0.001, 0, 0);
+    private PIDController yController = new PIDController(0.001, 0, 0);
 
     public CubeAlign(Drivetrain drivetrain, LimelightFront limelightFront) {
         this.drivetrain = drivetrain;
         this.limelightFront = limelightFront;
 
-        addRequirements(drivetrain);
+        xController.setTolerance(3);
+        yController.setTolerance(3);
 
     }
 
@@ -37,27 +41,43 @@ public void initialize() {
 
     @Override
     public void execute() {
-        if (limelightFront.hasVision() && limelightFront.getPipelineNum() == 3){
+        if (limelightFront.hasVision() && limelightFront.getPipelineNum() == 3) {
             horizOffsetLength = limelightFront.getHorizontalOffset();
             horizOffsetLength *= AutoAlignConstants.HORIZONTAL_MULTIPLIER;
-            horizOffsetDegrees = LimelightHelpers.getLimelightNTDouble("Retro", "txp");
-            XOffset = horizOffsetLength / Math.sin(horizOffsetDegrees);
-            YOffset = horizOffsetLength / Math.tan(horizOffsetDegrees);
+            horizOffsetDegrees = LimelightHelpers.getLimelightNTDouble("Retro", "tx");
+
+            if (Math.sin(Math.toRadians(horizOffsetDegrees)) != 0 && Math.tan(Math.toRadians(horizOffsetDegrees)) != 0) {
+                XOffset = horizOffsetLength / Math.sin(Math.toRadians(horizOffsetDegrees));
+                YOffset = horizOffsetLength / Math.tan(Math.toRadians(horizOffsetDegrees));
+
+            } else {
+                XOffset = 0;
+                YOffset = 0;
+            }
+
         }
 
-        translatedPose = new Pose2d(new Translation2d(drivetrain.getPose().getX() - XOffset, drivetrain.getPose().getY() - YOffset), drivetrain.getPose().getRotation());
+        double xPoseOffset = xController.calculate(drivetrain.getPose().getX(), drivetrain.getPose().getX() - XOffset);
+        double yPoseOffset = yController.calculate(drivetrain.getPose().getY(), drivetrain.getPose().getY() - XOffset);
+
+        translatedPose = drivetrain.getPose().transformBy(new Transform2d(new Translation2d(-xPoseOffset, -yPoseOffset), Rotation2d.fromDegrees(0)));
 
-        drivetrain.resetOdometry(translatedPose);
+        // 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);
+        LightningShuffleboard.setDouble("Cube-Align", "horizontal angle offset", horizOffsetDegrees);
+        LightningShuffleboard.setDouble("Cube-Align", "X pose offset", xPoseOffset);
+        LightningShuffleboard.setDouble("Cube-Align", "Y pose offset", yPoseOffset);
+        LightningShuffleboard.setDouble("Cube-Align", "X pose", translatedPose.getX());
+        LightningShuffleboard.setDouble("Cube-Align", "Y pose", translatedPose.getY());
+        LightningShuffleboard.setDouble("Cube-Align", "rotation pose", translatedPose.getRotation().getDegrees());
     }
 
     @Override
     public void end(boolean interrupted) {
-
+        limelightFront.setPipelineNum(0);
     }
 
     @Override