tuned

diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
index 762552a9e5bf61d9af99a97b03f92e1206a791fe..011379bed2deb572334a49d4e9e2a2dc818e3031 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java
@@ -1,26 +1,21 @@
 package frc.robot.subsystems.spindexer;
 
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC;
-import com.ctre.phoenix6.controls.NeutralOut;
-import com.ctre.phoenix6.controls.TorqueCurrentFOC;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
 
-import org.littletonrobotics.junction.AutoLogOutput;
-import org.littletonrobotics.junction.Logger;
-
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
@@ -52,13 +47,13 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
     TalonFXConfiguration configs = new TalonFXConfiguration();
     // units = amperes
     Slot0Configs slot0 = configs.Slot0;
-    slot0.kP = 2.0;
+    slot0.kP = 10.0;
     slot0.kI = 0.0;
     slot0.kD = 0.0;
 
     // start with feedforward at zero for tuning
-    slot0.kS = 0.0;
-    slot0.kV = 0.0;
+    slot0.kS = 9.0;
+    slot0.kV = 0.11;
     slot0.kA = 0.0;
 
     motorOne.getConfigurator().apply(configs);
@@ -67,6 +62,8 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
     motorTwo.getConfigurator().apply(limitConfig);
     motorTwo.getConfigurator().apply(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive));
 
+    tuner = new SpindexerTuning(motorOne, motorTwo);
+
     if (!Constants.DISABLE_SMART_DASHBOARD) {
       SmartDashboard.putData("Spindexer Run Forward", new InstantCommand(() -> maxSpindexer()));
       SmartDashboard.putData("Spindexer Run Reverse", new InstantCommand(() -> reverseSpindexer()));
@@ -105,12 +102,12 @@ public class Spindexer extends SubsystemBase implements SpindexerIO {
     return motorOne.getTorqueCurrent().getValueAsDouble();
   }
 
-  private final SpindexerTuning tuner = new SpindexerTuning(motorOne, motorTwo);
+  private SpindexerTuning tuner;
 
   @Override
   public void periodic() {
-    motorTwo.setControl(new Follower(IdConstants.SPINDEXER_ONE_ID, MotorAlignmentValue.Opposed));
     tuner.periodic();
+
   }
 
   public void periodicDEATH() {

diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerTuning.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerTuning.java
index b63857575d7d63150ca35782ab9dfdc342ab707c..3c7ea95a556537a983c399765b72b4bf57f59cfc 100644 (file)
--- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerTuning.java
+++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerTuning.java
@@ -9,7 +9,10 @@ import com.ctre.phoenix6.controls.MotionMagicVelocityTorqueCurrentFOC;
 import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
 public class SpindexerTuning {
@@ -17,14 +20,15 @@ public class SpindexerTuning {
   private final TalonFX motor1;
   private final TalonFX motor2;
 
-  private final MotionMagicVelocityTorqueCurrentFOC request = new MotionMagicVelocityTorqueCurrentFOC(0);
+  private final MotionMagicVelocityTorqueCurrentFOC request =
+      new MotionMagicVelocityTorqueCurrentFOC(0);
 
   private final NeutralOut neutralOut = new NeutralOut();
 
   private double kS = 0.0;
   private double kV = 0.0;
   private double kA = 0.0;
-  private double kP = 0.0;
+  private double kP = 1.0;
   private double kI = 0.0;
   private double kD = 0.0;
 
@@ -32,86 +36,83 @@ public class SpindexerTuning {
   private double maxAccel = 0.0;
   private double maxJerk = 0.0;
 
-  public SpindexerTuning(TalonFX motor1, TalonFX motor2) {
-    this.motor1 = motor1;
-    this.motor2 = motor2;
+  private final ShuffleboardTab tuningTab =
+      Shuffleboard.getTab("Indexer Tuning");
 
-    SmartDashboard.putNumber("Indexer Tuning/kS", 0.0);
-    SmartDashboard.putNumber("Indexer Tuning/kV", 0.0);
-    SmartDashboard.putNumber("Indexer Tuning/kA", 0.0);
-    SmartDashboard.putNumber("Indexer Tuning/kP", 1.0);
-    SmartDashboard.putNumber("Indexer Tuning/kI", 0.0);
-    SmartDashboard.putNumber("Indexer Tuning/kD", 0.0);
-    SmartDashboard.putNumber("Indexer Tuning/maxAccel", 0.0);
-    SmartDashboard.putNumber("Indexer Tuning/maxJerk", 0.0);
+  private final GenericEntry kSEntry =
+      tuningTab.add("kS", 0.0).getEntry();
 
-    SmartDashboard.putNumber(
-        "Indexer Tuning/Target Velocity RPS",
-        0.0);
+  private final GenericEntry kVEntry =
+      tuningTab.add("kV", 0.0).getEntry();
 
-    SmartDashboard.putBoolean(
-        "Indexer Tuning/Enabled",
-        false);
+  private final GenericEntry kAEntry =
+      tuningTab.add("kA", 0.0).getEntry();
 
-    SmartDashboard.putData("Indexer Tuning/update", new InstantCommand(() -> applyGains()));
-  }
+  private final GenericEntry kPEntry =
+      tuningTab.add("kP", 1.0).getEntry();
 
-  public void periodic() {
+  private final GenericEntry kIEntry =
+      tuningTab.add("kI", 0.0).getEntry();
 
-    boolean enabled = SmartDashboard.getBoolean(
-        "Indexer Tuning/Enabled",
-        false);
+  private final GenericEntry kDEntry =
+      tuningTab.add("kD", 0.0).getEntry();
 
-    if (!enabled) {
-      motor1.setControl(neutralOut);
-      return;
-    }
+  private final GenericEntry maxAccelEntry =
+      tuningTab.add("Max Accel", 0.0).getEntry();
 
-    kS = SmartDashboard.getNumber(
-        "Indexer Tuning/kS",
-        kS);
+  private final GenericEntry maxJerkEntry =
+      tuningTab.add("Max Jerk", 0.0).getEntry();
 
-    kV = SmartDashboard.getNumber(
-        "Indexer Tuning/kV",
-        kV);
+  private final GenericEntry targetVelocityEntry =
+      tuningTab.add("Target Velocity RPS", 0.0).getEntry();
 
-    kA = SmartDashboard.getNumber(
-        "Indexer Tuning/kA",
-        kA);
+  private final GenericEntry enabledEntry =
+      tuningTab.add("Enabled", false).getEntry();
 
-    kP = SmartDashboard.getNumber(
-        "Indexer Tuning/kP",
-        kP);
+  public SpindexerTuning(TalonFX motor1, TalonFX motor2) {
+    this.motor1 = motor1;
+    this.motor2 = motor2;
 
-    kI = SmartDashboard.getNumber(
-        "Indexer Tuning/kI",
-        kI);
+    tuningTab
+        .add("Update Gains", new InstantCommand(this::applyGains))
+        .withPosition(0, 6)
+        .withSize(2, 1);
+  }
 
-    kD = SmartDashboard.getNumber(
-        "Indexer Tuning/kD",
-        kD);
+  public void periodic() {
 
-    maxAccel = SmartDashboard.getNumber(
-        "Indexer Tuning/maxAccel",
-        maxAccel);
+    boolean enabled = enabledEntry.getBoolean(false);
 
-    maxJerk = SmartDashboard.getNumber(
-        "Indexer Tuning/maxJerk",
-        maxJerk);
+    if (!enabled) {
+      motor1.setControl(neutralOut);
+      motor2.setControl(neutralOut);
+      return;
+    }
 
-    // low = 6-7 rps, high: 60-67 rps for tuning: https://v6.docs.ctr-electronics.com/en/stable/docs/api-reference/device-specific/talonfx/manual-pid-tuning.html#flywheel-tuning-with-torquecurrentfoc
-    targetVelocity = SmartDashboard.getNumber(
-        "Indexer Tuning/Target Velocity RPS",
-       targetVelocity);
+    kS = kSEntry.getDouble(kS);
+    kV = kVEntry.getDouble(kV);
+    kA = kAEntry.getDouble(kA);
+
+    kP = kPEntry.getDouble(kP);
+    kI = kIEntry.getDouble(kI);
+    kD = kDEntry.getDouble(kD);
 
-    // applyGains();
+    maxAccel = maxAccelEntry.getDouble(maxAccel);
+    maxJerk = maxJerkEntry.getDouble(maxJerk);
+
+    // low = 6-7 RPS high = 60-67 RPS https://v6.docs.ctr-electronics.com/en/stable/docs/api-reference/device-specific/talonfx/manual-pid-tuning.html#flywheel-tuning-with-torquecurrentfoc
+    targetVelocity =
+        targetVelocityEntry.getDouble(targetVelocity);
 
     motor1.setControl(
         request.withVelocity(targetVelocity));
+
     motor2.setControl(
         request.withVelocity(targetVelocity));
 
-    Logger.recordOutput("Indexer Tuning/Target Velocity RPS", targetVelocity);
+    Logger.recordOutput(
+        "Indexer Tuning/Target Velocity RPS",
+        targetVelocity);
 
     Logger.recordOutput(
         "Indexer Tuning/Actual Velocity RPS",
@@ -129,23 +130,34 @@ public class SpindexerTuning {
         "Indexer Tuning/Closed Loop Reference",
         motor1.getClosedLoopReference().getValueAsDouble());
 
-    Logger.recordOutput("Indexer Tuning/Max Accel", maxAccel);
-    Logger.recordOutput("Indexer Tuning/Max Jerk", maxJerk);
+    Logger.recordOutput(
+        "Indexer Tuning/Max Accel",
+        maxAccel);
+
+    Logger.recordOutput(
+        "Indexer Tuning/Max Jerk",
+        maxJerk);
 
-    Logger.recordOutput("Indexer Tuning/Actual Acceleration r/s^2", motor1.getAcceleration().getValueAsDouble());
+    Logger.recordOutput(
+        "Indexer Tuning/Actual Acceleration r/s^2",
+        motor1.getAcceleration().getValueAsDouble());
 
-    Logger.recordOutput("Indexer Tuning/Acceleration Cap", motor1.isSafetyEnabled());
+    Logger.recordOutput(
+        "Indexer Tuning/Acceleration Cap",
+        motor1.isSafetyEnabled());
   }
 
   private void applyGains() {
+    System.out.println("UPDATING GAINS");
 
     TalonFXConfiguration config = new TalonFXConfiguration();
+
     // acceleration = rotations/sec^2
     // jerk = rotations/sec^3
     MotionMagicConfigs motionMagic = config.MotionMagic;
 
-    motionMagic.MotionMagicAcceleration = maxAccel; // 0 means no lim
-    motionMagic.MotionMagicJerk = maxJerk;
+    motionMagic.MotionMagicAcceleration = maxAccel;
+    motionMagic.MotionMagicJerk = maxJerk;  //0 means no lim
 
     Slot0Configs slot0 = config.Slot0;
 
@@ -160,4 +172,4 @@ public class SpindexerTuning {
     motor1.getConfigurator().apply(config);
     motor2.getConfigurator().apply(config);
   }
-}
+}
\ No newline at end of file