Subsystem Wrapper

src/main/java/org/trigon/hardware/subsystems/arm/ArmConfiguration.java

@@ -0,0 +1,81 @@
+package org.trigon.hardware.subsystems.arm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.util.Color;
+
+public class ArmConfiguration {
+    public String name = "";
+
+    /**
+     * Length of the arm in meters.
+     */
+    public double lengthMeters = 1;
+
+    /**
+     * Maximum velocity of the motor.
+     */
+    public double maximumVelocity = 1;
+
+    /**
+     * Maximum acceleration of the motor.
+     */
+    public double maximumAcceleration = 1;
+
+    /**
+     * Maximum jerk of the motor.
+     */
+    public double maximumJerk = 1;
+
+    /**
+     * Ramp rate used in system identification.
+     * See <a href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/system-identification/introduction.html">
+     * WPILib System Identification Introduction</a>.
+     */
+    public double sysIDRampRate = 1;
+
+    /**
+     * Step voltage used in system identification.
+     * See <a href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/system-identification/introduction.html">
+     * WPILib System Identification Introduction</a>.
+     */
+    public double sysIDStepVoltage = 1;
+
+    public double gearRatio = 1;
+
+    /**
+     * Mass of the arm in kilograms.
+     */
+    public double massKilograms = 1;
+
+    /**
+     * Visual offset for display in Mechanism2D.
+     */
+    public double visualizationOffset = 1;
+
+    /**
+     * Maximum angle of the arm.
+     */
+    public Rotation2d maximumAngle = Rotation2d.kZero;
+
+    /**
+     * Minimum angle of the arm.
+     */
+    public Rotation2d minimumAngle = Rotation2d.kZero;
+
+    /**
+     * Acceptable angular tolerance for reaching the target.
+     */
+    public Rotation2d angleTolerance = Rotation2d.kZero;
+
+    public boolean focEnabled = true;
+
+    public boolean shouldSimulateGravity = true;
+
+    public Color mechanismColor = Color.kBlue;
+
+    /**
+     * The type and number of motors used in the simulation.
+     */
+    public DCMotor gearbox = DCMotor.getKrakenX60Foc(1);
+}

src/main/java/org/trigon/hardware/subsystems/arm/ArmSubsystem.java

@@ -0,0 +1,165 @@
+package org.trigon.hardware.subsystems.arm;
+
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import org.littletonrobotics.junction.Logger;
+import org.trigon.hardware.RobotHardwareStats;
+import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor;
+import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal;
+import org.trigon.hardware.simulation.SingleJointedArmSimulation;
+import org.trigon.utilities.mechanisms.SingleJointedArmMechanism2d;
+
+public class ArmSubsystem {
+    private final TalonFXMotor motor;
+    private final Pose3d originPoint;
+    private final String name;
+    private final double
+            maximumVelocity,
+            maximumAcceleration,
+            maximumJerk,
+            visualizationOffsetFromGravityOffset;
+    private final Rotation2d angleTolerance;
+    private final VoltageOut voltageRequest;
+    private final DynamicMotionMagicVoltage positionRequest;
+    private final SingleJointedArmMechanism2d mechanism;
+    private final SysIdRoutine.Config sysIDConfig;
+    private ArmState targetState;
+
+    public ArmSubsystem(TalonFXMotor motor, ArmConfiguration config, Pose3d originPoint) {
+        this.motor = motor;
+        this.originPoint = originPoint;
+
+        name = config.name;
+        maximumVelocity = config.maximumVelocity;
+        maximumAcceleration = config.maximumAcceleration;
+        maximumJerk = config.maximumJerk;
+        angleTolerance = config.angleTolerance;
+        visualizationOffsetFromGravityOffset = config.visualizationOffset;
+        mechanism = new SingleJointedArmMechanism2d(name + "Mechanism", config.lengthMeters, config.mechanismColor);
+        voltageRequest = new VoltageOut(0).withEnableFOC(config.focEnabled);
+        positionRequest = new DynamicMotionMagicVoltage(0, maximumVelocity, maximumAcceleration, maximumJerk).withEnableFOC(config.focEnabled);
+        sysIDConfig = new SysIdRoutine.Config(
+                Units.Volts.of(config.sysIDRampRate).per(Units.Seconds),
+                Units.Volts.of(config.sysIDStepVoltage),
+                Units.Second.of(1000)
+        );
+        motor.setPhysicsSimulation(
+                new SingleJointedArmSimulation(
+                        config.gearbox,
+                        config.gearRatio,
+                        config.massKilograms,
+                        config.lengthMeters,
+                        config.minimumAngle,
+                        config.maximumAngle,
+                        config.shouldSimulateGravity
+                ));
+    }
+
+    public String getName() {
+        return name;
+    }
+
+    public SysIdRoutine.Config getSysIdConfig() {
+        return sysIDConfig;
+    }
+
+    public void setBrake(boolean brake) {
+        motor.setBrake(brake);
+    }
+
+    public void stop() {
+        motor.stopMotor();
+    }
+
+    public void updateLog(SysIdRoutineLog log) {
+        log.motor(name)
+                .angularPosition(Units.Rotations.of(motor.getSignal(TalonFXSignal.POSITION)))
+                .angularVelocity(Units.RotationsPerSecond.of(motor.getSignal(TalonFXSignal.VELOCITY)))
+                .voltage(Units.Volts.of(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)));
+    }
+
+    public void updateMechanism() {
+        logComponentPose();
+
+        mechanism.update(
+                getAngle(),
+                Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + (RobotHardwareStats.isSimulation() ? 0 : visualizationOffsetFromGravityOffset))
+        );
+    }
+
+    public void updatePeriodically() {
+        motor.update();
+        Logger.recordOutput(name + "/CurrentAngle", getAngle());
+    }
+
+    public void sysIDDrive(double targetVoltage) {
+        motor.setControl(voltageRequest.withOutput(targetVoltage));
+    }
+
+    public boolean atState(ArmState targetState) {
+        return this.targetState == targetState && atTargetState();
+    }
+
+    public boolean atTargetState() {
+        if (targetState == null)
+            return false;
+        final double currentToTargetStateDifferenceDegrees = Math.abs(targetState.getTargetAngle().minus(getAngle()).getDegrees());
+        return currentToTargetStateDifferenceDegrees < angleTolerance.getDegrees();
+    }
+
+    public Rotation2d getAngle() {
+        return Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.POSITION) + (RobotHardwareStats.isSimulation() ? 0 : visualizationOffsetFromGravityOffset));
+    }
+
+    public void setTargetState(ArmState targetState) {
+        this.targetState = targetState;
+        setTargetState(targetState.getTargetAngle(), targetState.getSpeedScalar());
+    }
+
+    public void setTargetState(Rotation2d targetAngle, double speedScalar) {
+        scalePositionRequestSpeed(speedScalar);
+        setTargetAngle(targetAngle);
+    }
+
+    public void setControl(ControlRequest request) {
+        motor.setControl(request);
+    }
+
+    private void scalePositionRequestSpeed(double speedScalar) {
+        positionRequest.Velocity = maximumVelocity * speedScalar;
+        positionRequest.Acceleration = maximumAcceleration * speedScalar;
+        positionRequest.Jerk = maximumJerk * speedScalar;
+    }
+
+    private void setTargetAngle(Rotation2d targetAngle) {
+        setControl(positionRequest.withPosition(targetAngle.getRotations() - (RobotHardwareStats.isSimulation() ? 0 : visualizationOffsetFromGravityOffset)));
+    }
+
+    private void logComponentPose() {
+        Logger.recordOutput("Poses/Components/" + name + "Pose", calculateComponentPose());
+    }
+
+    private Pose3d calculateComponentPose() {
+        final Transform3d armTransform = new Transform3d(
+                new Translation3d(),
+                new Rotation3d(0, getAngle().getRadians(), 0)
+        );
+        return originPoint.transformBy(armTransform);
+    }
+
+    /**
+     * An interface for an arm state.
+     * getTargetAngle represents the target angle of the state.
+     * getSpeedScalar represents the speed scalar of the state.
+     */
+    public interface ArmState {
+        Rotation2d getTargetAngle();
+
+        double getSpeedScalar();
+    }
+}

src/main/java/org/trigon/hardware/subsystems/elevator/ElevatorConfiguration.java

@@ -0,0 +1,73 @@
+package org.trigon.hardware.subsystems.elevator;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.util.Color;
+
+public class ElevatorConfiguration {
+    public String name = "";
+    /**
+     * Acceptable position error in meters.
+     */
+    public double positionToleranceMeters = 1;
+
+    /**
+     * Radius of the elevator drum in meters.
+     */
+    public double drumRadiusMeters = 1;
+
+    /**
+     * Minimum elevator height in meters.
+     */
+    public double minimumHeight = 1;
+
+    /**
+     * Maximum elevator height in meters.
+     */
+    public double maximumHeight = 1;
+
+    /**
+     * Maximum velocity of the elevator in meters per second.
+     */
+    public double maximumVelocity = 1;
+
+    /**
+     * Maximum acceleration of the elevator in meters per second squared.
+     */
+    public double maximumAcceleration = 1;
+
+    /**
+     * Maximum jerk of the elevator in meters per second cubed.
+     */
+    public double maximumJerk = 1;
+
+    /**
+     * Ramp rate used in system identification.
+     * See <a href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/system-identification/introduction.html">
+     * WPILib System Identification Introduction</a>.
+     */
+    public double sysIDRampRate = 1;
+
+    /**
+     * Step voltage used in system identification.
+     * See <a href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/system-identification/introduction.html">
+     * WPILib System Identification Introduction</a>.
+     */
+    public double sysIDStepVoltage = 1;
+
+    public double gearRatio = 1;
+    /**
+     * Mass of the Elevator in kg.
+     */
+    public double massKilograms = 1;
+
+    public boolean focEnabled = true;
+
+    public boolean shouldSimulateGravity = true;
+
+    public Color mechanismColor = Color.kBlue;
+
+    /**
+     * The type and number of motors used in the simulation.
+     */
+    public DCMotor gearbox = DCMotor.getKrakenX60Foc(1);
+}