Subsystem Wrapper

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

@@ -0,0 +1,28 @@
+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 = "";
+    public double
+            lengthMeters = 1,
+            maximumVelocity = 1,
+            maximumAcceleration = 1,
+            maximumJerk = 1,
+            sysIDRampRate = 1,
+            sysIDStepVoltage = 1,
+            gearRatio = 1,
+            massKilograms = 1,
+            visualizationOffset = 1;
+    public Rotation2d
+            maximumAngle = Rotation2d.kZero,
+            minimumAngle = Rotation2d.kZero,
+            angleTolerance = Rotation2d.kZero;
+    public boolean
+            focEnabled = true,
+            shouldSimulateGravity = true;
+    public Color mechanismColor = Color.kBlue;
+    public DCMotor gearbox = DCMotor.getKrakenX60Foc(1);
+}

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

@@ -0,0 +1,160 @@
+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);
+    }
+
+    public interface ArmState {
+        Rotation2d getTargetAngle();
+
+        double getSpeedScalar();
+    }
+}

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

@@ -0,0 +1,25 @@
+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 = "";
+    public double
+            positionToleranceMeters = 1,
+            drumRadiusMeters = 1,
+            minimumHeight = 1,
+            maximumHeight = 1,
+            maximumVelocity = 1,
+            maximumAcceleration = 1,
+            maximumJerk = 1,
+            sysIDRampRate = 1,
+            sysIDStepVoltage = 1,
+            gearRatio = 1,
+            massKilograms = 1;
+    public boolean
+            focEnabled = true,
+            shouldSimulateGravity = true;
+    public Color mechanismColor = Color.kBlue;
+    public DCMotor gearbox = DCMotor.getKrakenX60Foc(1);
+}

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

@@ -0,0 +1,176 @@
+package org.trigon.hardware.subsystems.elevator;
+
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+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.phoenix6.talonfx.TalonFXMotor;
+import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal;
+import org.trigon.hardware.simulation.ElevatorSimulation;
+import org.trigon.utilities.Conversions;
+import org.trigon.utilities.mechanisms.ElevatorMechanism2d;
+
+public class ElevatorSubsystem {
+    private final TalonFXMotor motor;
+    private final Pose3d[] stagesOriginPoints;
+    private final String name;
+    private final double
+            positionToleranceMeters,
+            drumRadiusMeters,
+            maximumVelocity,
+            maximumAcceleration,
+            maximumJerk;
+    private final VoltageOut voltageRequest;
+    private final DynamicMotionMagicVoltage positionRequest;
+    private final ElevatorMechanism2d mechanism;
+    private final SysIdRoutine.Config sysIDConfig;
+    private ElevatorState targetState;
+
+    public ElevatorSubsystem(TalonFXMotor motor, ElevatorConfiguration config, Pose3d... stagesOriginPoints) {
+        this.motor = motor;
+        this.stagesOriginPoints = stagesOriginPoints;
+
+        name = config.name;
+        positionToleranceMeters = config.positionToleranceMeters;
+        drumRadiusMeters = config.drumRadiusMeters;
+        maximumVelocity = config.maximumVelocity;
+        maximumAcceleration = config.maximumAcceleration;
+        maximumJerk = config.maximumJerk;
+        mechanism = new ElevatorMechanism2d(name + "Mechanism", config.maximumHeight, config.minimumHeight, 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 ElevatorSimulation(
+                        config.gearbox,
+                        config.gearRatio,
+                        config.massKilograms,
+                        config.drumRadiusMeters,
+                        config.minimumHeight,
+                        config.maximumHeight,
+                        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)
+                .linearPosition(Units.Meters.of(getPositionRotations()))
+                .linearVelocity(Units.MetersPerSecond.of(motor.getSignal(TalonFXSignal.VELOCITY)))
+                .voltage(Units.Volts.of(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)));
+    }
+
+    public void updateMechanism() {
+        logComponentPoses();
+
+        mechanism.update(
+                getPositionRotations(),
+                motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)
+        );
+    }
+
+    public void updatePeriodically() {
+        motor.update();
+        Logger.recordOutput(name + "/CurrentPositionMeters", getPositionMeters());
+    }
+
+    public void sysIDDrive(double targetVoltage) {
+        motor.setControl(voltageRequest.withOutput(targetVoltage));
+    }
+
+    public boolean atState(ElevatorState targetState) {
+        return this.targetState == targetState && atTargetState();
+    }
+
+    public boolean atTargetState() {
+        if (targetState == null)
+            return false;
+        final double currentToTargetStateDifference = Math.abs(targetState.getTargetPositionMeters() - getPositionMeters());
+        return currentToTargetStateDifference < positionToleranceMeters;
+    }
+
+    public double getPositionRotations() {
+        return motor.getSignal(TalonFXSignal.POSITION);
+    }
+
+    public double getPositionMeters() {
+        return rotationsToMeters(getPositionRotations());
+    }
+
+    public void setTargetState(ElevatorState targetState) {
+        this.targetState = targetState;
+        setTargetState(targetState.getTargetPositionMeters(), targetState.getSpeedScalar());
+    }
+
+    public void setTargetState(double targetPositionMeters, double speedScalar) {
+        scalePositionRequestSpeed(speedScalar);
+        setTargetPositionRotations(metersToRotations(targetPositionMeters));
+    }
+
+    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 setTargetPositionRotations(double targetPositionRotations) {
+        setControl(positionRequest.withPosition(targetPositionRotations));
+    }
+
+    private void logComponentPoses() {
+        for (int i = 0; i < stagesOriginPoints.length; i++)
+            Logger.recordOutput("Poses/Components/" + name + "Pose" + i, calculateComponentPose(i));
+    }
+
+    private Pose3d calculateComponentPose(int targetStage) {
+        final Transform3d elevatorTransform = new Transform3d(
+                new Translation3d(0, 0, getPositionMeters() / (targetStage + 1)),
+                new Rotation3d()
+        );
+        return stagesOriginPoints[targetStage].transformBy(elevatorTransform);
+    }
+
+    private double rotationsToMeters(double positionRotations) {
+        return Conversions.rotationsToDistance(positionRotations, drumRadiusMeters * 2);
+    }
+
+    private double metersToRotations(double positionMeters) {
+        return Conversions.distanceToRotations(positionMeters, drumRadiusMeters * 2);
+    }
+
+    public interface ElevatorState {
+        double getTargetPositionMeters();
+
+        double getSpeedScalar();
+    }
+}