Sysid doc

Docs/3_Specifics/3.6_SysID.md

@@ -0,0 +1,112 @@
+# SysId: System Identification
+What is SysId?
+
+SysId is a WPILib tool that characterizes a mechanism by applying controlled voltages and measuring its response. It uses this data to determine feedforward constants:
+- kS: Represents voltage needed to overcome static friction
+- kV: Represents voltage needed to maintain velocity
+- kA: Represents voltage needed to accelerate
+
+We use SysId to determine feedforward constants for our feedforward controller. SysId can also provide baseline PID values but we will often tune PID separately.
+
+### Creating SysId Routines
+
+Our example shooter has two mechanisms: a flywheel, which is a continuously rotating roller, and a hood, which is a limited-angle pivot.
+
+They need separate routines because the hood must stop before reaching its mechanical limits, while the flywheel can rotate freely.
+
+Flywheel: 
+
+```Java
+private SysIdRoutine flywheelSysid =
+    new SysIdRoutine(
+        new Config(
+            null, null, null,
+            (state) -> Logger.recordOutput(
+                "Shooter/Flywheel/SysID State",
+                state.toString())),
+        new Mechanism(
+            (voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)),
+            null,
+            this));
+
+```
+
+Hood:
+
+```Java
+private SysIdRoutine hoodSysid =
+    new SysIdRoutine(
+        new Config(
+            null,
+            Volts.of(5),
+            null,
+            (state) -> Logger.recordOutput(
+                "Shooter/Hood/SysID State",
+                state.toString())),
+        new Mechanism(
+            (voltage) -> hoodIO.setHoodVoltage(voltage.in(Volts)),
+            null,
+            this));
+```
+
+Config sets the test parameters and logs the current state. Mechanism tells SysId how to apply voltage through our IO layer.
+
+Then we will create commands to run these routines
+
+SysId uses two types of tests:
+- Quasistatic: Slowly ramps up voltage to measure the mechanism's response across different velocities.
+- Dynamic: Applies a step voltage to measure acceleration.
+
+Each routine runs forward and reverse tests of both types.
+
+Flywheel: 
+
+```Java
+@Override
+public Command runFlywheelSysid() {
+  return Commands.sequence(
+      flywheelSysid.quasistatic(Direction.kForward),
+      flywheelSysid.quasistatic(Direction.kReverse),
+      flywheelSysid.dynamic(Direction.kForward),
+      flywheelSysid.dynamic(Direction.kReverse));
+}
+```
+The flywheel does not need position-based stopping conditions because it can continuously rotate.
+
+Hood:
+
+```Java
+@Override
+public Command runHoodSysid() {
+  return Commands.sequence(
+      hoodSysid.quasistatic(Direction.kForward)
+          .until(() -> hoodInputs.hoodPositionRotations.getDegrees()
+              > HOOD_MAX_ANGLE.getDegrees() - 5),
+      hoodSysid.quasistatic(Direction.kReverse)
+          .until(() -> hoodInputs.hoodPositionRotations.getDegrees()
+              < HOOD_MIN_ANGLE.getDegrees() + 5),
+      hoodSysid.dynamic(Direction.kForward)
+          .until(() -> hoodInputs.hoodPositionRotations.getDegrees()
+              > HOOD_MAX_ANGLE.getDegrees() - 5),
+      hoodSysid.dynamic(Direction.kReverse)
+          .until(() -> hoodInputs.hoodPositionRotations.getDegrees()
+              < HOOD_MIN_ANGLE.getDegrees() + 5));
+}
+```
+The .until(...) conditions stop the hood 5° before its mechanical limits to prevent damage.
+
+Adding routines to the Auto Chooser to run them as autos:
+
+```Java
+autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid());
+autoChooser.addOption("Hood Sysid", shooter.runHoodSysid());
+```
+
+Analyze the Data by running the SysId routine to collect data. Then, load the log into the WPILib SysId tool and select the corresponding tests and values. Extract kS, kV, and kA values.
+Add the constants to the mechanism's feedforward controller.
+Check the diagnostic plots before using the results. Poor data can produce inaccurate constants.
+
+Further Reading
+
+- [WPILib SysId docs](https://docs.wpilib.org/en/stable/docs/software/advanced-controls/system-identification/index.html)
+- [Helpful Chief delphi thread](https://www.google.com/url?q=https://www.chiefdelphi.com/t/sysid-pid-and-feedfoward-tuning-for-elevator/482797&sa=D&source=docs&ust=1788129798942293&usg=AOvVaw1IUPkZIfUi8t0NkQ_1eo3A)