Subsystems
======================================

.. grid:: 1

    .. grid-item-card::
        :link: https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robotbuilder/writing-code/robotbuilder-writing-subsystem-code.html

        Context from WPILib

Subsystems are programmatic representation of the physical robot mechanisms.
In short, if a robot has a `pivot` mechanism, such as Ember, it can be written in code as a `Subsystem`.

In short, a Subsystem gives us the following benefits

- Encapsulates the motors, sensors, and other electronic components in a single Object in code.

- Maintains its own **state** as an Object in code.

- Can contain methods that either return a `Command` to run, or `Trigger` to respond.

- Still has a periodic method that can be overriden and runs on the robot's clock.

Let's take an exercise to build a `PivotSubsystem`.
----------------------------------------------------
1. Start with creating a file called `PivotSubsystem.java`
2. Write the class and extend `SubsystemBase` and give it a angle to update.

    .. code-block:: java

        import edu.wpi.first.math.geometry.Rotation2d;
        import edu.wpi.first.wpilibj2.command.SubsystemBase;

        public class PivotSubsystem extends SubsystemBase {
            private Rotation2d angle;

            public PivotSubsystem() {
                // "Resting" angle when the robot turns on.
                this.angle = new Rotation2d().fromDegrees(40); 
            }
        }


3. Include motors you plan to use. We'll assume that you're using a Spark Max controller in this example.
   You can use the docs from REV Robotics to help give context to their API:

   https://codedocs.revrobotics.com/java/com/revrobotics/package-summary.html

    .. code-block:: java

        import com.revrobotics.CANSparkBase.IdleMode;
        import com.revrobotics.CANSparkLowLevel.MotorType;
        import com.revrobotics.CANSparkMax;
        import com.revrobotics.RelativeEncoder;

        /** in your class */
        private final CANSparkMax pivotMotor = new CANSparkMax(kPivotLeaderCanId, MotorType.kBrushless);
        private final RelativeEncoder encoder = pivotMotor.getEncoder();

        /** in your constructor */
        this.pivotMotor.restoreFactoryDefaults();
        this.pivotMotor.setIdleMode(IdleMode.kCoast);
        this.pivotMotor.setClosedLoopRampRate(1.0);
        this.pivotMotor.setInverted(true);
        this.pivotMotor.burnFlash();

        this.encoder.setPositionConversionFactor(2.0 * Math.PI / kPivotReduction);
        this.encoder.setPosition(0);

4. Write code in your periodic block to update the current angle and publish it to SmartDashboard. 
   Remember that you need to `override` this from `SubsystemBase`.

    .. code-block:: java

        /** in your PivotSubsystem class */

        @override
        public void periodic() {
            this.angle = new Rotation2d(this.encoder.getPosition());
            SmartDashboard.putNumber("PivotAngle", this.angle.getRadians());
        }

5. Finally, write an accessor to get the angle outside of the Subsystem (like in RobotContainer)
  
    .. code-block:: java

        /** in your PivotSubsystem class */
        public Rotation2d getRotation() {
            return this.angle;
        }

If you have followed these steps, you should have a Subsystem that can read a Spark Max controlled motor and read the position of the motor when it turns.

Assignment
-----------------------------
1. Provide the completed `PivotSubsystem` we have described so far.
2. Write the instance variables for voltage, temperature, and current on the motor and have them update periodically. 
   You can use the following code from "Ember" to help find a working implementation:

   https://github.com/frc4451/Ember2024/blob/main/src/main/java/frc/robot/subsystems/pivot/PivotIOSparkMax.java

3. In a Command based robot generated by WPILib, instantiate your PivotSubsystem and have it available for the next step.