PID Niti

src/main/java/frc/lib/utils/TunableNumber.java

@@ -0,0 +1,118 @@
+
+package frc.lib.utils;
+
+import frc.robot.Constants;
+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.LoggedNetworkNumber;
+
+import static frc.robot.Constants.tuningMode;
+
+/**
+ * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
+ * value not in dashboard.
+ */
+public class TunableNumber implements DoubleSupplier {
+    private static final String tableKey = "/Tuning";
+
+    private final String key;
+    private boolean hasDefault = false;
+    private double defaultValue;
+    private LoggedNetworkNumber dashboardNumber;
+    private Map<Integer, Double> lastHasChangedValues = new HashMap<>();
+
+    /**
+     * Create a new LoggedTunableNumber
+     *
+     * @param dashboardKey Key on dashboard
+     */
+    public TunableNumber(String dashboardKey) {
+        this.key = tableKey + "/" + dashboardKey;
+    }
+
+    /**
+     * Create a new LoggedTunableNumber with the default value
+     *
+     * @param dashboardKey Key on dashboard
+     * @param defaultValue Default value
+     */
+    public TunableNumber(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;
+            if (tuningMode) {
+                dashboardNumber = new LoggedNetworkNumber(key, 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 tuningMode ? dashboardNumber.get() : 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, TunableNumber... tunableNumbers) {
+        if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
+            action.accept(Arrays.stream(tunableNumbers).mapToDouble(TunableNumber::get).toArray());
+        }
+    }
+
+    /** Runs action if any of the tunableNumbers have changed */
+    public static void ifChanged(int id, Runnable action, TunableNumber... tunableNumbers) {
+        ifChanged(id, values -> action.run(), tunableNumbers);
+    }
+
+    @Override
+    public double getAsDouble() {
+        return get();
+    }
+}

src/main/java/frc/robot/Constants.java

@@ -13,4 +13,6 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-}
+  static public final boolean tuningMode = true;
+  static public final String camName = "Arucam OV9281(Back)";
+}

src/main/java/frc/robot/Orchestrator.java

@@ -1,27 +1,53 @@
 package frc.robot;
 
-import java.util.function.BooleanSupplier;
-
+import edu.wpi.first.units.DistanceUnit;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.vision.Vision;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import java.util.function.BooleanSupplier;
 
 
 public class Orchestrator {
     private final Indexer indexer;
     private final Shooter shooter;
+    @AutoLogOutput
+    private double Distance  = 300.0;
+    @AutoLogOutput private final double shootingPower = 0.0;
+    private final Vision vision;
 
-    public Orchestrator(Indexer indexer, Shooter shooter) {
+    public Orchestrator(Indexer indexer, Shooter shooter, Vision vision) {
         this.indexer = indexer;
         this.shooter = shooter;
+        this.vision = vision;
+    }
+    public Command spinUpDistance(Double distance) {
+        Distance = distance;
+        double percent = (8.331 * Math.pow(10, -5) * Math.pow(distance, 2) + 0.0734 * distance + 15.709) / 100;
+        Logger.recordOutput("Shooter/Percent", percent);
+        return shooter.runPercent(percent);
+    }
+    public Command shootCycleDistance() {
+        return Commands.parallel(
+                spinUpDistance(420.0),
+                Commands.sequence(
+                                intakeIfNeeded(),
+                                Commands.waitSeconds(ShooterConstants.SPINUP_SECONDS),
+                                indexer.indexIntoShooter())
+                        .repeatedly());
     }
 
     public Command spinUp(BooleanSupplier fastMode) {
         return Commands.either(
                 shooter.fullSpeed(),
-                shooter.speedUp(),
+                shooter.runPercent(0.8),
                  fastMode);
     }
 
@@ -51,11 +77,40 @@ public Command shootCycle(BooleanSupplier fastMode) {
                         .repeatedly());
     }
 
+    public Command shootOncePID(double setpointRPM) {
+        return Commands.parallel(
+                shooter.toSetpoint(setpointRPM),
+                Commands.sequence(
+                        intakeIfNeeded(),
+                        Commands.waitUntil(shooter::atSetpoint),
+                        indexer.indexIntoShooter()));
+    }
+
+    public Command shootCyclePID(double setpointRPM) {
+        return Commands.parallel(
+                shooter.toSetpoint(setpointRPM),
+                Commands.sequence(
+                                intakeIfNeeded(),
+                                Commands.waitUntil(shooter::atSetpoint),
+                                indexer.indexIntoShooter())
+                        .repeatedly());
+    }
+
+    public Command shootDistance(Measure<DistanceUnit> distance) {
+        return Commands.parallel(
+            shooter.toSetpoint(Units.Meters.of(vision.distanceFromTarget())),
+            Commands.sequence(
+                intakeIfNeeded(),
+                Commands.waitUntil(shooter::atSetpoint),
+                indexer.indexIntoShooter()).repeatedly());
+    }
+
+
     public Command reverseAll() {
         return Commands.parallel(shooter.reverse(), indexer.reverse());
     }
 
     public Command stopAll() {
         return Commands.parallel(shooter.stop(), indexer.stop());
     }
-}
+}

src/main/java/frc/robot/RobotContainer.java

@@ -4,22 +4,22 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.commands.Autos;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.drive.*;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.drive.DriveIOSparkMax;
 import frc.robot.subsystems.indexer.Indexer;
 import frc.robot.subsystems.indexer.IndexerIOSparkMax;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterIOSparkMax;
 import frc.robot.subsystems.vision.Vision;
-import frc.robot.subsystems.vision.VisionIO;
 import frc.robot.subsystems.vision.VisionIOPhotonVision;
 
+import static frc.robot.Constants.camName;
+
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -31,10 +31,10 @@ public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final Drive drive;
     // The robot's subsystems and commands are defined here...
-    private final Vision vision;
    private final Indexer indexer = new Indexer(new IndexerIOSparkMax());
     private final Shooter shooter = new Shooter(new ShooterIOSparkMax());
-    private final Orchestrator orchestrator = new Orchestrator(indexer, shooter);
+    private final Vision vision = new Vision(new VisionIOPhotonVision(camName));
+    private final Orchestrator orchestrator = new Orchestrator(indexer, shooter, vision);
     private final CommandXboxController driverController = new CommandXboxController(0);
     private boolean fastMode = false;
     private final Trigger fastModeTrigger = new Trigger(() -> fastMode);
@@ -44,7 +44,6 @@ public class RobotContainer {
   public RobotContainer() {
     // Configure the trigger bindings
     drive = new Drive(new DriveIOSparkMax());
-    vision =  new Vision(new VisionIOPhotonVision("VGA_USB_Camera"){});
     configureBindings();
   }
 
@@ -75,16 +74,27 @@ private void configureBindings() {
                       .rightTrigger()
                       .whileTrue(orchestrator.shootCycle(() -> fastMode))
                       .onFalse(orchestrator.stopAll());
-              driverController.y().whileTrue(shooter.speedUp()).onFalse(shooter.stop());
-              driverController
-                      .leftTrigger()
-                      .whileTrue(orchestrator.reverseAll())
-                      .onFalse(orchestrator.stopAll());
-
+
               driverController
                       .rightBumper()
                       .onTrue(orchestrator.shootOnce(() -> fastMode))
                       .onFalse(orchestrator.stopAll());
+
+              //driverController.x().whileTrue(orchestrator.shootCycleDistance()).onFalse(shooter.stop());
+              driverController.x().whileTrue(orchestrator.shootCyclePID(2000)).onFalse(shooter.stop()); // TODO: change the RPM
+              driverController.a().whileTrue(orchestrator.shootDistance(Units.Inches.of(168.5))).onFalse(shooter.stop()); // TODO: Test other distances
+              driverController.leftTrigger().onTrue(indexer.indexUntilSwitch());
+
+              driverController.y().onTrue(indexer.indexIntoShooter());
+
+
+              driverController
+                      .leftBumper()
+                      .whileTrue(orchestrator.reverseAll())
+                      .onFalse(orchestrator.stopAll());
+
+
+
     }
 
     /**
@@ -97,4 +107,4 @@ public Command getAutonomousCommand() {
         // An example command will be run in autonomous
         return Commands.none();
     }
-}
+}