Merge pull request #478 from frc-862/473-faster-point

[#473] - faster point at point
frc-862 · Mar 22, 2024 · d6cf3dd · d6cf3dd
2 parents c271468 + b8ee336
commit d6cf3dd
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -323,7 +323,7 @@ public class VisionConstants {
         public static final Translation2d VISION_LIMIT = new Translation2d(Units.feetToMeters(9),
                 Units.feetToMeters(5));
         public static final double ALIGNMENT_TOLERANCE = 8d; // TODO: make this an actual value
-        public static final PIDController TAG_AIM_CONTROLLER = new PIDController(0.1, 0, 0, 0.01);
+        public static final PIDController TAG_AIM_CONTROLLER = new PIDController(0.2, 0, 0, 0.01);
         public static final PIDController CHASE_CONTROLLER = new PIDController(0.05, 0, 0);
         public static final int TAG_PIPELINE = 0;
         public static final int SPEAKER_PIPELINE = 1;

diff --git a/src/main/java/frc/robot/command/PointAtPoint.java b/src/main/java/frc/robot/command/PointAtPoint.java
@@ -18,7 +18,7 @@
 
 public class PointAtPoint extends Command {
 
-    private static final double MIN_POWER = 0.3;
+    private double minPower = 0.3;
 
     private Swerve drivetrain;
     private XboxController driver;
@@ -104,6 +104,14 @@ public void initLogging() {
         inToleranceLog = new BooleanLogEntry(log, "/PointAtPoint/InTolerance");
     }
 
+
+    public void setDebugging(){
+        headingController.setP(LightningShuffleboard.getDouble("Point-At-Point", "P", headingController.getP()));
+        headingController.setI(LightningShuffleboard.getDouble("Point-At-Point", "I", headingController.getI()));
+        headingController.setD(LightningShuffleboard.getDouble("Point-At-Point", "D", headingController.getD()));
+        minPower = LightningShuffleboard.getDouble("Point-At-Point", "Min Power", minPower);
+    }
+
     @Override
     public void execute() {
         Pose2d pose = drivetrain.getPose();
@@ -114,15 +122,19 @@ public void execute() {
         targetHeading %= 360;
         pidOutput = headingController.calculate((pose.getRotation().getDegrees() + 360) % 360, targetHeading);
 
-        if (!inTolerance() && Math.abs(pidOutput) < MIN_POWER) {
-            pidOutput = Math.signum(pidOutput) * MIN_POWER;
+        // setDebugging();
+
+        if (!inTolerance() && Math.abs(pidOutput) < minPower) {
+            pidOutput = Math.signum(pidOutput) * minPower;
         }
 
+
         drivetrain.setField(-driver.getLeftY(), -driver.getLeftX(), pidOutput);
 
         LightningShuffleboard.setDouble("Point-At-Point", "Target Heading", targetHeading);
         LightningShuffleboard.setDouble("Point-At-Point", "Current Heading",
                 drivetrain.getPose().getRotation().getDegrees());
+        LightningShuffleboard.setBool("Point-At-Point", "In Tolerance", inTolerance());
 
         updateLogging();
     }