[#29] fix stuff

frc-862 · Mar 2, 2024 · f39deb0 · f39deb0
1 parent 62b4633
commit f39deb0
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 14 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -152,11 +152,11 @@ public static class AutonomousConstants {
                                                        // constants
 
         // For Pathfinding
-        public static final Pose2d TARGET_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0d));
         // TODO get real poses to pathfind to
 
         // For AlignToTag      1.85, 6.5
-        public static final Pose2d AMP_POSE = new Pose2d(new Translation2d(3, 5), new Rotation2d(0d)); 
+        public static final Pose2d AMP_POSE = new Pose2d(new Translation2d(3, 5), new Rotation2d(0d));
+        public static final double AMP_TOLERANCE = 0.1; 
 
         // TODO get real poses to pathfind to
         public static final Pose2d TARGET_POSE =

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -29,7 +29,7 @@
 import frc.robot.Constants.CollisionConstants.CollisionType;
 import frc.robot.Constants.VisionConstants;
 import frc.robot.Constants.LEDsConstants.LED_STATES;
-import frc.robot.command.AlignToTag;
+import frc.robot.command.AlignToAmp;
 import frc.robot.command.ChasePieces;
 import frc.robot.command.Index;
 import frc.robot.command.MoveToPose;
@@ -200,7 +200,7 @@ protected void configureButtonBindings() {
 
 		new Trigger(() -> driver.getPOV() == 0).toggleOnTrue(leds.enableState(LED_STATES.DISABLED));
 
-		new Trigger(driver::getAButton).whileTrue(new AlignToTag(AutonomousConstants.AMP_POSE, drivetrain));
+		new Trigger(driver::getAButton).whileTrue(new AlignToAmp(AutonomousConstants.AMP_POSE, drivetrain));
 
 		// Test auto align
 		/* copilot */

diff --git a/...in/java/frc/robot/command/AlignToTag.java → ...in/java/frc/robot/command/AlignToAmp.java b/...in/java/frc/robot/command/AlignToTag.java → ...in/java/frc/robot/command/AlignToAmp.java
@@ -9,11 +9,14 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Constants.AutonomousConstants;
 import frc.robot.subsystems.Swerve;
+import frc.thunder.shuffleboard.LightningShuffleboard;
 
 
 
-public class AlignToTag extends Command {
+public class AlignToAmp extends Command {
 
   public Pose2d target;
   public Swerve drivetrain;
@@ -23,7 +26,7 @@ public class AlignToTag extends Command {
   private PathPlannerPath path;
   private boolean aligning;
   /** Creates a new AlignToTag. */
-  public AlignToTag(Pose2d target, Swerve drivetrain) {
+  public AlignToAmp(Pose2d target, Swerve drivetrain) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.target = target;
     this.drivetrain = drivetrain;
@@ -37,21 +40,24 @@ public void initialize() {
     path = PathPlannerPath.fromPathFile("Amp autoalign");
     moveToPose = new MoveToPose(target, drivetrain);
     moveToPose.initialize();
-
+    initLogging();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
     if(!aligning){
       moveToPose.execute();
-      if(moveToPose.isFinished()){
-        aligning = true;
-      }
-    } else {
-      AutoBuilder.followPath(path);
+      aligning = moveToPose.isFinished();
     }
   }
+
+  public void initLogging() {
+    LightningShuffleboard.setDoubleSupplier("AlignToAmp", "Target X", () -> target.getTranslation().getX());
+    LightningShuffleboard.setDoubleSupplier("AlignToAmp", "Target Y", () -> target.getTranslation().getY());
+    LightningShuffleboard.setBoolSupplier("AlignToAmp", "Aligning", () -> aligning);
+    LightningShuffleboard.setBoolSupplier("AlignToAmp", "isFinished", () -> isFinished());
+  }
 
   // Called once the command ends or is interrupted.
   @Override
@@ -62,7 +68,9 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return drivetrain.getPose().get().getTranslation().getX() == target.getTranslation().getX() 
-    && drivetrain.getPose().get().getTranslation().getY() == target.getTranslation().getY();
+    return Math.abs(drivetrain.getPose().get().getTranslation().getX() - target.getTranslation().getX()) 
+    < AutonomousConstants.AMP_TOLERANCE 
+    && Math.abs(drivetrain.getPose().get().getTranslation().getY() - target.getTranslation().getY()) 
+    < AutonomousConstants.AMP_TOLERANCE;
   }
 }