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 (file)
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 914e8f20b6035d66dcaf1735dd1948701bc4087d..94f331d72566f19c337484f2442816509b551e19 100644 (file)
--- 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) {