Tune constants some SysID stuff

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

@@ -11,12 +11,16 @@ public final class Constants {
 
   public static final int driverport = 0;
   public static final int codriverport = 1;
+  public static final int testport = 2;
 
   public static final int shooterPort = 31;
   public static final int indexerPort = 21;
-  public static final double shooterP = 0.5;
+  public static final double shooterP = 0.00035273;
   public static final double shooterI = 0;
   public static final double shooterD = 0;
+  public static final double shooterS = 0.090624;
+  public static final double shooterV = 0.0021896;
+  public static final double shooterA = 0.00070176;
 
   public static final int climberPort = 10;
   public static final double climberP = 0.35;
@@ -26,11 +30,14 @@ public final class Constants {
   public static final int intakePort = 32;
   public static final int intakeFrontBeambreak = 6;
   public static final int intakeBackBeambreak = 5;
-  public static final double intakeP = 0.01;
+  public static final double intakeP = 3.896e-05;
   public static final double intakeI = 0;
   public static final double intakeD = 0;
+  public static final double intakeS = 0.27754;
+  public static final double intakeV = 0.0020349;
+  public static final double intakeA = 0.00018939;
 
-  public static final double armP = 10;
+  public static final double armP = 37.5;
   public static final double armI = 0;
   public static final double armD = 0;
   public static final double armS = 0;

src/main/java/frc/robot/Robot.java

@@ -23,6 +23,7 @@
 import frc.robot.subsystems.Index;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Shooter;
+import frc.robot.subsystems.Sysid;
 import java.util.HashMap;
 
 public class Robot extends CommandRobot {
@@ -35,6 +36,7 @@ public class Robot extends CommandRobot {
   private final Climber m_climber = new Climber();
   private final Intake m_intake = new Intake();
   private final Index m_index = new Index();
+  private final Sysid m_sysid = new Sysid();
   private final Superstructure m_superstructure = new Superstructure(m_shooter, m_intake, m_index);
   private static final CommandSwerveDrivetrain m_drivetrain = TunerConstants.DriveTrain;
 
@@ -62,6 +64,20 @@ public Robot() {
     DriverStation.startDataLog(DataLogManager.getLog());
     SignalLogger.setPath(DataLogManager.getLogDir());
     SignalLogger.start();
+    // m_sysid.add(m_shooter::sysIdDynamic);
+    // m_sysid.add(m_shooter::sysIdQuasistatic);
+
+    m_sysid.add(m_arm::sysIdDynamic);
+    m_sysid.add(m_arm::sysIdQuasistatic);
+
+    // m_sysid.add(m_climber::sysIdDynamic);
+    // m_sysid.add(m_climber::sysIdQuasistatic);
+
+    // m_sysid.add(m_intake::sysIdDynamic);
+    // m_sysid.add(m_intake::sysIdQuasistatic);
+
+    // m_sysid.addAll(m_drivetrain.getSysIdCommands());
+
     m_drivetrain.registerTelemetry(m_logger::telemeterize);
 
     m_scheduler.registerSubsystem(m_arm);
@@ -88,7 +104,7 @@ public Robot() {
                         Utils.deadzone(-m_driver.getLeftX() * Constants.DrivebaseMaxSpeed * 0.5))
                     .withRotationalRate(
                         Utils.deadzone(
-                            -m_driver.getRightX() * Constants.DrivebaseMaxAngularRate * 0.5))));
+                            -m_driver.getRightX() * Constants.DrivebaseMaxAngularRate))));
     m_intake.setDefaultCommand(m_superstructure.intake());
     m_shooter.setDefaultCommand(m_shooter.stop());
     m_index.setDefaultCommand(m_index.stop());

src/main/java/frc/robot/subsystems/Arm.java

@@ -4,21 +4,34 @@
 
 package frc.robot.subsystems;
 
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.Volts;
+
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
 import edu.wpi.first.util.datalog.DataLog;
 import edu.wpi.first.util.datalog.DoubleLogEntry;
 import edu.wpi.first.util.datalog.StringLogEntry;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
 import java.util.function.DoubleSupplier;
 
@@ -62,18 +75,42 @@ public enum Setpoint {
   public final Trigger m_atSetpoint = new Trigger(m_pid::atSetpoint);
   private double m_lastPosition = 0.2;
 
+  private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
+  private final MutableMeasure<Angle> m_angle = mutable(Rotations.of(0));
+  private final MutableMeasure<Velocity<Angle>> m_velocity = mutable(RotationsPerSecond.of(0));
+
+  private final SysIdRoutine m_sysIdRoutine =
+      new SysIdRoutine(
+          new SysIdRoutine.Config(Volts.of(0.5).per(Second.of(1)), Volts.of(1), Second.of(4)),
+          new SysIdRoutine.Mechanism(
+              (Measure<Voltage> volts) -> {
+                m_primaryMotor.setVoltage(volts.in(Volts));
+              },
+              log -> {
+                log.motor("arm")
+                    .voltage(
+                        m_appliedVoltage.mut_replace(
+                            m_primaryMotor.get() * RobotController.getBatteryVoltage(), Volts))
+                    .angularPosition(
+                        m_angle.mut_replace(m_encoder.getAbsolutePosition(), Rotations))
+                    .angularVelocity(
+                        m_velocity.mut_replace(
+                            (m_encoder.get() * 2 * Math.PI / 60), RotationsPerSecond));
+              },
+              this));
+
   /** Creates a new {@link Arm} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */
   public Arm() {
     m_followerMotor.follow(m_primaryMotor);
-    m_pid.setTolerance(0.1);
+    m_pid.setTolerance(0.05);
   }
 
   /** Achieves and maintains speed for the primary motor. */
   private Command achievePosition(double position) {
+    m_pid.setSetpoint(position);
     return run(
         () -> {
           m_lastPosition = position;
-          m_pid.setSetpoint(position);
           var pid_output = m_pid.calculate(getAngle());
           log_pid_output.append(pid_output);
           log_pid_setpoint.append(m_pid.getSetpoint());
@@ -101,7 +138,7 @@ public Command stop() {
    * @return a {@link Command} to get to the desired position.
    */
   public Command moveToPosition(double position) {
-    return achievePosition(position).until(m_pid::atSetpoint);
+    return defer(() -> achievePosition(position).until(m_pid::atSetpoint)).andThen(stop());
   }
 
   /**
@@ -166,4 +203,12 @@ public void periodic() {
   public double getAngle() {
     return m_encoder.getAbsolutePosition() * 2 * Math.PI;
   }
+
+  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.quasistatic(direction);
+  }
+
+  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.dynamic(direction);
+  }
 }

src/main/java/frc/robot/subsystems/Climber.java

@@ -4,15 +4,27 @@
 
 package frc.robot.subsystems;
 
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
 import edu.wpi.first.util.datalog.DoubleLogEntry;
 import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
 
 /** Our Crescendo climber Subsystem */
@@ -26,6 +38,28 @@ public class Climber extends SubsystemBase {
   private final DoubleLogEntry log_pid_output =
       new DoubleLogEntry(DataLogManager.getLog(), "/climber/pid/output");
 
+  private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
+  private final MutableMeasure<Angle> m_angle = mutable(Rotations.of(0));
+  private final MutableMeasure<Velocity<Angle>> m_velocity = mutable(RotationsPerSecond.of(0));
+
+  private final SysIdRoutine m_sysIdRoutine =
+      new SysIdRoutine(
+          new SysIdRoutine.Config(),
+          new SysIdRoutine.Mechanism(
+              (Measure<Voltage> volts) -> {
+                m_motor.setVoltage(volts.in(Volts));
+              },
+              log -> {
+                log.motor("climber")
+                    .voltage(
+                        m_appliedVoltage.mut_replace(
+                            m_motor.get() * RobotController.getBatteryVoltage(), Volts))
+                    .angularPosition(m_angle.mut_replace(m_encoder.getPosition(), Rotations))
+                    .angularVelocity(
+                        m_velocity.mut_replace(m_encoder.getVelocity(), RotationsPerSecond));
+              },
+              this));
+
   /** Creates a new {@link Climber} {@link edu.wpi.first.wpilibj2.command.Subsystem}. */
   public Climber() {}
 
@@ -43,4 +77,12 @@ public Command climb() {
           m_motor.setVoltage(output);
         });
   }
+
+  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.quasistatic(direction);
+  }
+
+  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.dynamic(direction);
+  }
 }

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -4,26 +4,40 @@
 
 package frc.robot.subsystems;
 
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import com.choreo.lib.Choreo;
 import com.choreo.lib.ChoreoControlFunction;
 import com.ctre.phoenix6.Orchestra;
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.configs.AudioConfigs;
+import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import java.util.ArrayList;
 import java.util.Arrays;
+import java.util.List;
+import java.util.function.Function;
 import java.util.function.Supplier;
 
 /**
@@ -53,6 +67,9 @@ public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsyst
   /* Keep track if we've ever applied the operator perspective before or not */
   private boolean hasAppliedOperatorPerspective;
 
+  private final SysIdRoutine[] m_driveSysIdRoutines = new SysIdRoutine[4];
+  private final SysIdRoutine[] m_steerSysIdRoutines = new SysIdRoutine[4];
+
   public CommandSwerveDrivetrain(
       SwerveDrivetrainConstants driveTrainConstants,
       double OdometryUpdateFrequency,
@@ -83,6 +100,33 @@ public CommandSwerveDrivetrain(
 
     m_swerveController =
         Choreo.choreoSwerveController(m_xController, m_yController, m_rotationController);
+
+    for (int i = 0; i < 4; i++) {
+      var module = getModule(i);
+      m_driveSysIdRoutines[i] = createSysIdRoutine(module.getDriveMotor());
+      m_steerSysIdRoutines[i] = createSysIdRoutine(module.getSteerMotor());
+    }
+  }
+
+  private SysIdRoutine createSysIdRoutine(TalonFX motor) {
+    MutableMeasure<Voltage> appliedVoltage = mutable(Volts.of(0));
+    MutableMeasure<Angle> angle = mutable(Rotations.of(0));
+    MutableMeasure<Velocity<Angle>> velocity = mutable(RotationsPerSecond.of(0));
+
+    return new SysIdRoutine(
+        new SysIdRoutine.Config(),
+        new SysIdRoutine.Mechanism(
+            (Measure<Voltage> volts) -> motor.setVoltage(volts.in(Volts)),
+            log ->
+                log.motor("swerveMotor" + motor.hashCode())
+                    .voltage(
+                        appliedVoltage.mut_replace(
+                            motor.get() * RobotController.getBatteryVoltage(), Volts))
+                    .angularPosition(angle.mut_replace(motor.getPosition().getValue(), Rotations))
+                    .angularVelocity(
+                        velocity.mut_replace(
+                            (motor.getVelocity().getValue() / 2048.0 * 10), RotationsPerSecond)),
+            this));
   }
 
   public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
@@ -203,4 +247,60 @@ public Command followTrajectory(String name, boolean isRed) {
         () -> isRed,
         this);
   }
+
+  public Command sysIdQuasistaticDrive(int moduleIndex, SysIdRoutine.Direction direction) {
+    return m_driveSysIdRoutines[moduleIndex].quasistatic(direction);
+  }
+
+  /**
+   * Creates a command for a dynamic drive SysId routine for the specified module.
+   *
+   * @param moduleIndex The index of the module.
+   * @param direction The direction of the SysId routine.
+   * @return A command for the dynamic drive SysId routine.
+   */
+  public Command sysIdDynamicDrive(int moduleIndex, SysIdRoutine.Direction direction) {
+    return m_driveSysIdRoutines[moduleIndex].dynamic(direction);
+  }
+
+  /**
+   * Creates a command for a quasistatic steer SysId routine for the specified module.
+   *
+   * @param moduleIndex The index of the module.
+   * @param direction The direction of the SysId routine.
+   * @return A command for the quasistatic steer SysId routine.
+   */
+  public Command sysIdQuasistaticSteer(int moduleIndex, SysIdRoutine.Direction direction) {
+    return m_steerSysIdRoutines[moduleIndex].quasistatic(direction);
+  }
+
+  /**
+   * Creates a command for a dynamic steer SysId routine for the specified module.
+   *
+   * @param moduleIndex The index of the module.
+   * @param direction The direction of the SysId routine.
+   * @return A command for the dynamic steer SysId routine.
+   */
+  public Command sysIdDynamicSteer(int moduleIndex, SysIdRoutine.Direction direction) {
+    return m_steerSysIdRoutines[moduleIndex].dynamic(direction);
+  }
+
+  /**
+   * Gets a list of SysId commands for all modules.
+   *
+   * @return A list of SysId commands.
+   */
+  public List<Function<SysIdRoutine.Direction, Command>> getSysIdCommands() {
+    List<Function<SysIdRoutine.Direction, Command>> sysidCommands = new ArrayList<>();
+
+    for (int i = 0; i < 4; i++) {
+      int moduleIndex = i;
+      sysidCommands.add(direction -> sysIdDynamicDrive(moduleIndex, direction));
+      sysidCommands.add(direction -> sysIdQuasistaticDrive(moduleIndex, direction));
+      sysidCommands.add(direction -> sysIdDynamicSteer(moduleIndex, direction));
+      sysidCommands.add(direction -> sysIdQuasistaticSteer(moduleIndex, direction));
+    }
+
+    return sysidCommands;
+  }
 }