From 2ddecc2ec84917091b5588661453054c55b9a1ec Mon Sep 17 00:00:00 2001
From: iefomit <timofei.stem@gmail.com>
Date: Tue, 7 Apr 2026 11:16:00 -0700
Subject: [PATCH] added test command for foc

---
 .../drive_comm/SysIdPerModuleCommand.java     | 68 +++++++++++++++++++
 .../controls/PS5ControllerDriverConfig.java   |  4 ++
 2 files changed, 72 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/drive_comm/SysIdPerModuleCommand.java

diff --git a/src/main/java/frc/robot/commands/drive_comm/SysIdPerModuleCommand.java b/src/main/java/frc/robot/commands/drive_comm/SysIdPerModuleCommand.java
new file mode 100644
index 0000000..cc229f7
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive_comm/SysIdPerModuleCommand.java
@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.drive_comm;
+
+
+import com.ctre.phoenix6.SignalLogger;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import frc.robot.subsystems.drivetrain.Drivetrain;
+import frc.robot.util.SysId;
+
+public class SysIdPerModuleCommand extends SequentialCommandGroup {
+
+    private Config config = new Config();
+
+    public SysIdPerModuleCommand(Drivetrain drive) {
+        config = new Config(
+            Units.Volts.of(0.5).per(Units.Seconds),
+            Units.Volts.of(3),
+            Units.Seconds.of(5),
+            (x) -> SignalLogger.writeString("state", x.toString())
+        );
+
+        var modules = drive.getModules();
+
+        for (int i = 0; i < modules.length; i++) {
+            final int moduleIndex = i;
+
+            SysId sysId = new SysId(
+                "Module" + i,
+                (voltage) -> {
+                    for (int m = 0; m < modules.length; m++) {
+                        if (m == moduleIndex) {
+                            modules[m].setDriveVoltage(voltage);
+                        } else {
+                            modules[m].setDriveVoltage(Units.Volts.of(0));
+                        }
+                    }
+                    Rotation2d zeroAngle = Rotation2d.fromDegrees(0);
+                    Rotation2d[] angles = new Rotation2d[4];
+                    for (int a = 0; a < 4; a++) {
+                        angles[a] = zeroAngle;
+                    }
+                    drive.setAngleMotors(angles);
+                },
+                drive,
+                config
+            );
+
+            addCommands(
+                sysId.runQuasisStatic(Direction.kForward).withName("Module" + i + "_QS_Fwd"),
+                new WaitCommand(0.5),
+                sysId.runDynamic(Direction.kForward).withName("Module" + i + "_Dyn_Fwd"),
+                new WaitCommand(0.5),
+                sysId.runQuasisStatic(Direction.kReverse).withName("Module" + i + "_QS_Rev"),
+                new WaitCommand(0.5),
+                sysId.runDynamic(Direction.kReverse).withName("Module" + i + "_Dyn_Rev"),
+                new WaitCommand(1.0)
+            );
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 1f81078..3736e67 100644
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -12,6 +12,7 @@ import frc.robot.commands.gpm.IntakeMovementCommand;
 import frc.robot.commands.gpm.ReverseMotors;
 import frc.robot.commands.gpm.RunSpindexer;
 import frc.robot.commands.gpm.Superstructure;
+import frc.robot.commands.drive_comm.SysIdPerModuleCommand;
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.Climb.LinearClimb;
 import frc.robot.subsystems.Intake.Intake;
@@ -69,6 +70,9 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
             CommandScheduler.getInstance().cancelAll();
         }));
 
+        // sysId per-module characterization
+        controller.get(PS5Button.TRIANGLE).onTrue(new SysIdPerModuleCommand(getDrivetrain()));
+
         // Reverse motors
         if (intake != null && spindexer != null) {
             controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake));
-- 
2.39.5