From 17324c5e00521d0f50d8993cb37afa5a057104cb Mon Sep 17 00:00:00 2001 From: moo <160301629+moogoesmeow0@users.noreply.github.com> Date: Sun, 30 Aug 2026 12:46:01 -0700 Subject: [PATCH] tuned --- .../robot/subsystems/spindexer/Spindexer.java | 23 ++- .../subsystems/spindexer/SpindexerTuning.java | 146 ++++++++++-------- 2 files changed, 89 insertions(+), 80 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index 762552a..011379b 100644 --- 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 b638575..3c7ea95 100644 --- 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 -- 2.47.3