From: moo <moogoesmeow123@gmail.com>
Date: Tue, 27 Jan 2026 01:24:44 +0000 (-0800)
Subject: gtrjyu
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=03b126160d33fbef7176cf3ea0f863760ded3fab;p=FRC2026.git

gtrjyu
---

diff --git a/src/main/java/frc/robot/commands/gpm/DoSysidThings.java b/src/main/java/frc/robot/commands/gpm/DoSysidThings.java
new file mode 100644
index 0000000..7624265
--- /dev/null
+++ b/src/main/java/frc/robot/commands/gpm/DoSysidThings.java
@@ -0,0 +1,36 @@
+package frc.robot.commands.gpm;
+
+import edu.wpi.first.units.VoltageUnit;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import frc.robot.subsystems.turret.Turret;
+
+public class DoSysidThings extends SequentialCommandGroup {
+    Turret turret;
+
+    // TODO: CHECK HARDSTOPS
+    public DoSysidThings(Turret turret) {
+        this.turret = turret;
+        Config conf = new Config();
+        SysIdRoutine.Mechanism mech = new SysIdRoutine.Mechanism(turret::setVoltage, this::doLog, turret);
+        SysIdRoutine routine = new SysIdRoutine(conf, mech);
+
+        addCommands(
+            routine.quasistatic(SysIdRoutine.Direction.kForward),
+            routine.quasistatic(SysIdRoutine.Direction.kReverse),
+            routine.dynamic(SysIdRoutine.Direction.kForward),
+            routine.dynamic(SysIdRoutine.Direction.kReverse),
+        )
+    }
+
+    private void doLog(SysIdRoutineLog log) {
+        log.motor("turret")
+            .voltage(turret.getVolts())
+            .angularPosition(turret.getPosition())
+            .angularVelocity(turret.getVelocity())
+            .angularAcceleration(turret.getAccel());
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 914e8f2..94f331d 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -15,6 +15,11 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularAcceleration;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Velocity;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
@@ -111,6 +116,26 @@ public class Turret extends SubsystemBase implements TurretIO{
         SmartDashboard.putData("Turret Mech", mech);
     }
 
+    public void setVoltage(Voltage volts) {
+        motor.setVoltage(volts.magnitude());
+    }
+    
+    public Voltage getVolts() {
+        return motor.getMotorVoltage().getValue();
+    }
+
+    public Angle getPosition() {
+        return motor.getPosition().getValue();
+    }
+
+    public AngularVelocity getVelocity() {
+        return motor.getVelocity().getValue();
+    }
+
+    public AngularAcceleration getAccel() {
+        return motor.getAcceleration().getValue();
+    }
+    
     /* ---------------- Public API ---------------- */
 
     public void setFieldRelativeTarget(Rotation2d angle, double velocityRadPerSec) {