added Tunable Double, started auto align controller

src/main/java/common/core/controllers/LoggedTunableNumber.java

@@ -0,0 +1,123 @@
+package common.core.controllers;
+
+// Copyright (c) 2024 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+// PLEASE DON'T HURT ME IM JUST TRYING THIS OUT
+// NEVERMIND DON'T USE THIS IT'S JUST A WEIRD VERSION OF A DOUBLE SUPPLIER THAT WORKS WITH LOGGING
+
+
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
+
+/**
+ * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
+ * value not in dashboard.
+ */
+public class LoggedTunableNumber implements DoubleSupplier {
+  private static final String tableKey = "TunableNumbers";
+
+  private final String key;
+  private boolean hasDefault = false;
+  private double defaultValue;
+  private LoggedDashboardNumber dashboardNumber;
+  private Map<Integer, Double> lastHasChangedValues = new HashMap<>();
+
+  /**
+   * Create a new LoggedTunableNumber
+   *
+   * @param dashboardKey Key on dashboard
+   */
+  public LoggedTunableNumber(String dashboardKey) {
+    this.key = tableKey + "/" + dashboardKey;
+  }
+
+  /**
+   * Create a new LoggedTunableNumber with the default value
+   *
+   * @param dashboardKey Key on dashboard
+   * @param defaultValue Default value
+   */
+  public LoggedTunableNumber(String dashboardKey, double defaultValue) {
+    this(dashboardKey);
+    initDefault(defaultValue);
+  }
+
+  /**
+   * Set the default value of the number. The default value can only be set once.
+   *
+   * @param defaultValue The default value
+   */
+  public void initDefault(double defaultValue) {
+    if (!hasDefault) {
+      hasDefault = true;
+      this.defaultValue = defaultValue;
+    }
+  }
+
+  /**
+   * Get the current value, from dashboard if available and in tuning mode.
+   *
+   * @return The current value
+   */
+  public double get() {
+    if (!hasDefault) {
+      return 0.0;
+    } else {
+      return defaultValue;
+    }
+  }
+
+  /**
+   * Checks whether the number has changed since our last check
+   *
+   * @param id Unique identifier for the caller to avoid conflicts when shared between multiple
+   *     objects. Recommended approach is to pass the result of "hashCode()"
+   * @return True if the number has changed since the last time this method was called, false
+   *     otherwise.
+   */
+  public boolean hasChanged(int id) {
+    double currentValue = get();
+    Double lastValue = lastHasChangedValues.get(id);
+    if (lastValue == null || currentValue != lastValue) {
+      lastHasChangedValues.put(id, currentValue);
+      return true;
+    }
+
+    return false;
+  }
+
+  /**
+   * Runs action if any of the tunableNumbers have changed
+   *
+   * @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
+   *     objects. Recommended approach is to pass the result of "hashCode()"
+   * @param action Callback to run when any of the tunable numbers have changed. Access tunable
+   *     numbers in order inputted in method
+   * @param tunableNumbers All tunable numbers to check
+   */
+  public static void ifChanged(
+      int id, Consumer<double[]> action, LoggedTunableNumber... tunableNumbers) {
+    if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
+      action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray());
+    }
+  }
+
+  /** Runs action if any of the tunableNumbers have changed */
+  public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
+    ifChanged(id, values -> action.run(), tunableNumbers);
+  }
+
+  @Override
+  public double getAsDouble() {
+    return get();
+  }
+}
+

src/main/java/common/core/controllers/PIDFFConfig.java

@@ -62,4 +62,31 @@ public PIDFFConfig(double kP, double kI, double kD) {
     public PIDFFConfig() {
         this(0, 0, 0);
     }
+
+    public double getkP(){
+        return kP;
+    }
+
+    public double getkI(){
+        return kI;
+    }
+
+    public double getkD(){
+        return kD;
+    }
+    public double getkS(){
+        return kS;
+    }
+
+    public double getkV(){
+        return kP;
+    }
+
+    public double getkG(){
+        return kG;
+    }
+
+    public double getkA(){
+        return kA;
+    }
 }

src/main/java/common/core/controllers/SwerveController.java

@@ -0,0 +1,91 @@
+package common.core.controllers;
+
+import java.util.function.Supplier;
+
+import common.core.swerve.SwerveBase;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import edu.wpi.first.wpilibj.Timer;
+
+public class SwerveController {
+    private final Supplier<Pose2d> goalPoseSupplier;
+    private final SwerveBase swerve;
+    private final Supplier<Translation2d> feedfowardSupplier;
+    private final driveController driveController;
+    private final angleController angleController;
+    private final TunableDouble timeTolerance = new TunableDouble(0.5, "SwerveController", "timeTolerance", 4,0);
+    private Timer toleranceTimer = new Timer();
+
+
+    public SwerveController(Supplier<Pose2d> goalPoseSupplier, SwerveBase swerve, Supplier<Translation2d> feedforwardSupplier, driveController driveController, angleController angleController) {
+        this.goalPoseSupplier = goalPoseSupplier;
+        this.swerve = swerve;
+        this.feedfowardSupplier = feedforwardSupplier;
+        this.driveController = driveController;
+        this.angleController = angleController;
+
+        toleranceTimer.restart();
+        resetControllers();
+
+    }
+
+    private void resetControllers(){
+        Pose2d currentPose = swerve.getPose(); //this is sus idk if it works...i think it does
+        Pose2d goalPose = goalPoseSupplier.get();
+        ChassisSpeeds fieldVelocity = swerve.getFieldVelocity();
+        double totalLinearVelocity = Math.hypot(fieldVelocity.vxMetersPerSecond, fieldVelocity.vxMetersPerSecond);
+        double totalAngularVelocity = fieldVelocity.omegaRadiansPerSecond;
+        driveController.reset();
+        angleController.reset();
+        driveController.setSetpoint(new State(currentPose.getTranslation().getDistance(goalPose.getTranslation()), totalLinearVelocity));
+        angleController.setSetpoint(new State(currentPose.getRotation().minus(goalPose.getRotation()).getDegrees(), totalAngularVelocity));
+    }
+
+    public void update(){
+        // what we wanna do is get to our goal 
+
+
+    }
+
+    public class driveController extends TrapController {
+
+        private final TunableDouble linearTolerance = new TunableDouble(0, "SwerveController", "linearTolerance", 2, 0); //placeholder value
+        // private final TunableDouble maxLinearVelocity = new TunableDouble(15.0, "SwerveController", "maxLinearVelocity", 2, 1);
+        // private final TunableDouble maxLinearAcceleration = new TunableDouble(30.0, "SwerveController", "maxLinearAcceleration", 2, 2);
+
+        public driveController(PIDFFConfig config, Constraints constraints){
+            super(config, constraints);
+
+            final TunableDouble linearkP = new TunableDouble(config.kP, "SwerveController", "linearkP", 0,0);
+            final TunableDouble linearkI = new TunableDouble(config.kI, "SwerveController", "linearkI", 0,1);
+            final TunableDouble linearkD = new TunableDouble(config.kD, "SwerveController", "linearkD", 0,2);
+
+            driveController.setTolerance(linearTolerance.getAsDouble());
+        }
+
+    }
+
+    public class angleController extends TrapController {
+
+        private final TunableDouble angleTolerance = new TunableDouble(0, "SwerveController", "angleTolerance", 3, 1); //placeholder value
+        // private final TunableDouble maxAngularVelocity = new TunableDouble(12.0, "SwerveController", "maxLinearVelocity", 3, 1);
+        // private final TunableDouble maxAngularAcceleration = new TunableDouble(4.8, "SwerveController", "maxLinearAcceleration", 3, 2);
+
+        public angleController(PIDFFConfig config, Constraints constraints){
+            super(config, constraints);
+
+            final TunableDouble thetakP = new TunableDouble(config.kP, "SwerveController", "thetakP", 1,0);
+            final TunableDouble thetakI = new TunableDouble(config.kI, "SwerveController", "thetakI", 1,1);
+            final TunableDouble thetakD = new TunableDouble(config.kD, "SwerveController", "thetakD", 1,2);
+
+            angleController.enableContinuousInput(-Math.PI, Math.PI);
+            angleController.setTolerance(angleTolerance.getAsDouble());
+
+        }
+
+    }
+}

src/main/java/common/core/controllers/TunableDouble.java

@@ -0,0 +1,27 @@
+package common.core.controllers;
+
+import java.util.function.DoubleSupplier;
+
+import common.utility.shuffleboard.NAR_Shuffleboard;
+
+public class TunableDouble implements DoubleSupplier {
+
+    public double tunableDouble;
+    public String tabName;
+    public DoubleSupplier tunableDoubleSupplier;
+
+    public TunableDouble(double tunableDouble, String tabName, String name, int x, int y){
+        this.tunableDouble = tunableDouble;
+        this.tabName = tabName;
+        NAR_Shuffleboard.addData(tabName, name, tunableDouble, x,y);
+        NAR_Shuffleboard.addData(tabName, "TOGGLE", false, 1, 0).withWidget("Toggle Button").getEntry();
+        tunableDoubleSupplier = NAR_Shuffleboard.debug(tabName, "Debug " + name, 0, 1,2);
+    }
+
+    @Override
+    public double getAsDouble() {
+        return (NAR_Shuffleboard.getBoolean(tabName, "TOGGLE").getAsBoolean()) ? tunableDoubleSupplier.getAsDouble() : tunableDouble;
+    }
+
+
+}

src/main/java/common/core/swerve/SwerveBase.java

@@ -263,4 +263,5 @@ public void initStateCheck() {
             NarwhalDashboard.getInstance().checkState("Module" + module.moduleNumber, ()-> module.getRunningState());
         }
     }
+
 }