From 62014a50b24c2976ded1bf2c55bee3ea486cc90d Mon Sep 17 00:00:00 2001
From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com>
Date: Tue, 14 Oct 2025 23:04:52 +0300
Subject: [PATCH 01/17] =?UTF-8?q?Lots=20of=20code,=20but=20no=20tests=20?=
=?UTF-8?q?=F0=9F=91=8D=F0=9F=91=8D=F0=9F=91=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../java/frc/trigon/robot/RobotContainer.java | 2 +-
.../commandfactories/GeneralCommands.java | 2 +-
.../robot/subsystems/swerve/Swerve.java | 2 +-
.../hardware}/subsystems/MotorSubsystem.java | 2 +-
.../hardware/subsystems/arm/ArmSubsystem.java | 186 +++++++++++++++++
.../subsystems/arm/ArmSubsystemCommands.java | 56 +++++
.../arm/ArmSubsystemConfiguration.java | 81 ++++++++
.../elevator/ElevatorSubsystem.java | 194 ++++++++++++++++++
.../elevator/ElevatorSubsystemCommands.java | 43 ++++
.../ElevatorSubsystemConfiguration.java | 73 +++++++
.../flywheel/SimpleMotorSubsystem.java | 170 +++++++++++++++
.../SimpleMotorSubsystemCommands.java | 42 ++++
.../SimpleMotorSubsystemConfiguration.java | 57 +++++
13 files changed, 906 insertions(+), 4 deletions(-)
rename src/main/java/{frc/trigon/robot => trigon/hardware}/subsystems/MotorSubsystem.java (99%)
create mode 100644 src/main/java/trigon/hardware/subsystems/arm/ArmSubsystem.java
create mode 100644 src/main/java/trigon/hardware/subsystems/arm/ArmSubsystemCommands.java
create mode 100644 src/main/java/trigon/hardware/subsystems/arm/ArmSubsystemConfiguration.java
create mode 100644 src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystem.java
create mode 100644 src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystemCommands.java
create mode 100644 src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystemConfiguration.java
create mode 100644 src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystem.java
create mode 100644 src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystemCommands.java
create mode 100644 src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystemConfiguration.java
diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java
index 1a871d6e..14a8a528 100644
--- a/src/main/java/frc/trigon/robot/RobotContainer.java
+++ b/src/main/java/frc/trigon/robot/RobotContainer.java
@@ -18,9 +18,9 @@
import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
-import frc.trigon.robot.subsystems.MotorSubsystem;
import frc.trigon.robot.subsystems.swerve.Swerve;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import trigon.hardware.subsystems.MotorSubsystem;
import trigon.utilities.flippable.Flippable;
public class RobotContainer {
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java
index 5fbedf61..b11500ce 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java
@@ -3,8 +3,8 @@
import edu.wpi.first.wpilibj2.command.*;
import frc.trigon.robot.commands.CommandConstants;
import frc.trigon.robot.constants.OperatorConstants;
-import frc.trigon.robot.subsystems.MotorSubsystem;
import frc.trigon.robot.subsystems.swerve.SwerveCommands;
+import trigon.hardware.subsystems.MotorSubsystem;
import java.util.function.BooleanSupplier;
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
index be0af824..f36ddb24 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
@@ -15,7 +15,6 @@
import frc.trigon.robot.RobotContainer;
import frc.trigon.robot.constants.PathPlannerConstants;
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants;
-import frc.trigon.robot.subsystems.MotorSubsystem;
import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule;
import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants;
import org.littletonrobotics.junction.AutoLogOutput;
@@ -24,6 +23,7 @@
import trigon.hardware.phoenix6.Phoenix6SignalThread;
import trigon.hardware.phoenix6.pigeon2.Pigeon2Gyro;
import trigon.hardware.phoenix6.pigeon2.Pigeon2Signal;
+import trigon.hardware.subsystems.MotorSubsystem;
import trigon.utilities.flippable.Flippable;
import trigon.utilities.flippable.FlippablePose2d;
import trigon.utilities.flippable.FlippableRotation2d;
diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/trigon/hardware/subsystems/MotorSubsystem.java
similarity index 99%
rename from src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java
rename to src/main/java/trigon/hardware/subsystems/MotorSubsystem.java
index 75b736f9..ce839419 100644
--- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java
+++ b/src/main/java/trigon/hardware/subsystems/MotorSubsystem.java
@@ -1,4 +1,4 @@
-package frc.trigon.robot.subsystems;
+package trigon.hardware.subsystems;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.DriverStation;
diff --git a/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystem.java b/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystem.java
new file mode 100644
index 00000000..48c9e7f9
--- /dev/null
+++ b/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystem.java
@@ -0,0 +1,186 @@
+package 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 trigon.hardware.RobotHardwareStats;
+import trigon.hardware.phoenix6.talonfx.TalonFXMotor;
+import trigon.hardware.phoenix6.talonfx.TalonFXSignal;
+import trigon.hardware.simulation.SingleJointedArmSimulation;
+import trigon.hardware.subsystems.MotorSubsystem;
+import trigon.utilities.mechanisms.SingleJointedArmMechanism2d;
+
+public class ArmSubsystem extends MotorSubsystem {
+ 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, ArmSubsystemConfiguration 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),
+ null
+ );
+ motor.setPhysicsSimulation(
+ new SingleJointedArmSimulation(
+ config.gearbox,
+ config.gearRatio,
+ config.massKilograms,
+ config.lengthMeters,
+ config.minimumAngle,
+ config.maximumAngle,
+ config.shouldSimulateGravity
+ ));
+
+ setName(name);
+ }
+
+ @Override
+ public String getName() {
+ return name;
+ }
+
+ @Override
+ public SysIdRoutine.Config getSysIDConfig() {
+ return sysIDConfig;
+ }
+
+ @Override
+ public void setBrake(boolean brake) {
+ motor.setBrake(brake);
+ }
+
+ @Override
+ public void stop() {
+ motor.stopMotor();
+ }
+
+ @Override
+ 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)));
+ }
+
+ @Override
+ public void updateMechanism() {
+ logComponentPose();
+
+ mechanism.update(
+ getAngle(),
+ Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + (RobotHardwareStats.isSimulation() ? 0 : visualizationOffsetFromGravityOffset))
+ );
+ }
+
+ @Override
+ public void updatePeriodically() {
+ motor.update();
+ Logger.recordOutput(name + "/CurrentAngle", getAngle());
+ }
+
+ @Override
+ 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);
+ }
+
+ protected double getRotorPositionRotations() {
+ return motor.getSignal(TalonFXSignal.ROTOR_POSITION);
+ }
+
+ protected double getRawMotorPositionRotations() {
+ return motor.getSignal(TalonFXSignal.POSITION);
+ }
+
+ 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();
+
+ ArmState getPrepareState();
+
+ double getSpeedScalar();
+ }
+}
diff --git a/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystemCommands.java b/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystemCommands.java
new file mode 100644
index 00000000..a09a549e
--- /dev/null
+++ b/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystemCommands.java
@@ -0,0 +1,56 @@
+package trigon.hardware.subsystems.arm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import trigon.commands.ExecuteEndCommand;
+import trigon.commands.GearRatioCalculationCommand;
+import trigon.commands.NetworkTablesCommand;
+
+import java.util.Set;
+import java.util.function.DoubleConsumer;
+
+public class ArmSubsystemCommands {
+ public Command getDebuggingCommand(ArmSubsystem subsystem) {
+ return new NetworkTablesCommand(
+ (targetAngleDegrees, speedScalar) -> subsystem.setTargetState(Rotation2d.fromDegrees(targetAngleDegrees), speedScalar),
+ false,
+ Set.of(subsystem),
+ subsystem.getName() + "TargetAngleDegrees",
+ subsystem.getName() + "SpeedScalar"
+ );
+ }
+
+ public Command getGearRatioCalculationCommand(DoubleConsumer runVoltage, double backlashAccountabilityTimeSeconds, ArmSubsystem subsystem) {
+ return new GearRatioCalculationCommand(
+ subsystem::getRotorPositionRotations,
+ subsystem::getRawMotorPositionRotations,
+ runVoltage,
+ backlashAccountabilityTimeSeconds,
+ subsystem
+ );
+ }
+
+ public Command getSetTargetStateCommand(ArmSubsystem.ArmState targetState, ArmSubsystem subsystem) {
+ return new ExecuteEndCommand(
+ () -> subsystem.setTargetState(targetState),
+ subsystem::stop,
+ subsystem
+ );
+ }
+
+ public Command getSetTargetStateCommand(Rotation2d targetAngle, double speedScalar, ArmSubsystem subsystem) {
+ return new ExecuteEndCommand(
+ () -> subsystem.setTargetState(targetAngle, speedScalar),
+ subsystem::stop,
+ subsystem
+ );
+ }
+
+ public Command getPrepareTargetStateCommand(ArmSubsystem.ArmState targetState, ArmSubsystem subsystem) {
+ return new ExecuteEndCommand(
+ () -> subsystem.setTargetState(targetState.getPrepareState()),
+ subsystem::stop,
+ subsystem
+ );
+ }
+}
diff --git a/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystemConfiguration.java b/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystemConfiguration.java
new file mode 100644
index 00000000..8638ec8d
--- /dev/null
+++ b/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystemConfiguration.java
@@ -0,0 +1,81 @@
+package 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 ArmSubsystemConfiguration {
+ 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
+ * WPILib System Identification Introduction.
+ */
+ public double sysIDRampRate = 1;
+
+ /**
+ * Step voltage used in system identification.
+ * See
+ * WPILib System Identification Introduction.
+ */
+ 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);
+}
diff --git a/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystem.java
new file mode 100644
index 00000000..97463cde
--- /dev/null
+++ b/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystem.java
@@ -0,0 +1,194 @@
+package 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 trigon.hardware.phoenix6.talonfx.TalonFXMotor;
+import trigon.hardware.phoenix6.talonfx.TalonFXSignal;
+import trigon.hardware.simulation.ElevatorSimulation;
+import trigon.hardware.subsystems.MotorSubsystem;
+import trigon.utilities.Conversions;
+import trigon.utilities.mechanisms.ElevatorMechanism2d;
+
+public class ElevatorSubsystem extends MotorSubsystem {
+ 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, ElevatorSubsystemConfiguration 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),
+ null
+ );
+ motor.setPhysicsSimulation(
+ new ElevatorSimulation(
+ config.gearbox,
+ config.gearRatio,
+ config.massKilograms,
+ config.drumRadiusMeters,
+ config.minimumHeight,
+ config.maximumHeight,
+ config.shouldSimulateGravity
+ ));
+
+ setName(name);
+ }
+
+ @Override
+ public String getName() {
+ return name;
+ }
+
+ @Override
+ public SysIdRoutine.Config getSysIDConfig() {
+ return sysIDConfig;
+ }
+
+ @Override
+ public void setBrake(boolean brake) {
+ motor.setBrake(brake);
+ }
+
+ @Override
+ public void stop() {
+ motor.stopMotor();
+ }
+
+ @Override
+ 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)));
+ }
+
+ @Override
+ public void updateMechanism() {
+ logComponentPoses();
+
+ mechanism.update(
+ getPositionRotations(),
+ motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)
+ );
+ }
+
+ @Override
+ public void updatePeriodically() {
+ motor.update();
+ Logger.recordOutput(name + "/CurrentPositionMeters", getPositionMeters());
+ }
+
+ @Override
+ 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);
+ }
+
+ /**
+ * An interface for an elevator state.
+ * getTargetPositionMeters represents the target position of the state.
+ * getSpeedScalar represents the speed scalar of the state.
+ */
+ public interface ElevatorState {
+ double getTargetPositionMeters();
+
+ ElevatorState getPrepareState();
+
+ double getSpeedScalar();
+ }
+}
diff --git a/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystemCommands.java b/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystemCommands.java
new file mode 100644
index 00000000..17fe4985
--- /dev/null
+++ b/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystemCommands.java
@@ -0,0 +1,43 @@
+package trigon.hardware.subsystems.elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import trigon.commands.ExecuteEndCommand;
+import trigon.commands.NetworkTablesCommand;
+
+import java.util.Set;
+
+public class ElevatorSubsystemCommands {
+ public Command getDebuggingCommand(ElevatorSubsystem subsystem) {
+ return new NetworkTablesCommand(
+ subsystem::setTargetState,
+ false,
+ Set.of(subsystem),
+ subsystem.getName() + "TargetPositionMeters",
+ subsystem.getName() + "SpeedScalar"
+ );
+ }
+
+ public Command getSetTargetStateCommand(ElevatorSubsystem.ElevatorState targetState, ElevatorSubsystem subsystem) {
+ return new ExecuteEndCommand(
+ () -> subsystem.setTargetState(targetState),
+ subsystem::stop,
+ subsystem
+ );
+ }
+
+ public Command getSetTargetStateCommand(double targetPositionMeters, double speedScalar, ElevatorSubsystem subsystem) {
+ return new ExecuteEndCommand(
+ () -> subsystem.setTargetState(targetPositionMeters, speedScalar),
+ subsystem::stop,
+ subsystem
+ );
+ }
+
+ public Command getPrepareTargetStateCommand(ElevatorSubsystem.ElevatorState targetState, ElevatorSubsystem subsystem) {
+ return new ExecuteEndCommand(
+ () -> subsystem.setTargetState(targetState.getPrepareState()),
+ subsystem::stop,
+ subsystem
+ );
+ }
+}
diff --git a/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystemConfiguration.java b/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystemConfiguration.java
new file mode 100644
index 00000000..6542dc04
--- /dev/null
+++ b/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystemConfiguration.java
@@ -0,0 +1,73 @@
+package trigon.hardware.subsystems.elevator;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.util.Color;
+
+public class ElevatorSubsystemConfiguration {
+ 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
+ * WPILib System Identification Introduction.
+ */
+ public double sysIDRampRate = 1;
+
+ /**
+ * Step voltage used in system identification.
+ * See
+ * WPILib System Identification Introduction.
+ */
+ 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);
+}
diff --git a/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystem.java b/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystem.java
new file mode 100644
index 00000000..0d5fa1b9
--- /dev/null
+++ b/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystem.java
@@ -0,0 +1,170 @@
+package trigon.hardware.subsystems.flywheel;
+
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+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 trigon.hardware.phoenix6.talonfx.TalonFXMotor;
+import trigon.hardware.phoenix6.talonfx.TalonFXSignal;
+import trigon.hardware.simulation.SimpleMotorSimulation;
+import trigon.hardware.subsystems.MotorSubsystem;
+import trigon.utilities.mechanisms.SpeedMechanism2d;
+
+public class SimpleMotorSubsystem extends MotorSubsystem {
+ private final TalonFXMotor motor;
+ private final String name;
+ private final double
+ maximumVelocity,
+ maximumAcceleration,
+ maximumJerk;
+ private final VoltageOut voltageRequest;
+ private final VelocityVoltage velocityRequest;
+ private final SpeedMechanism2d mechanism;
+ private final double velocityTolerance;
+ private final SysIdRoutine.Config sysIDConfig;
+ private SimpleMotorState targetState;
+
+ private final boolean shouldUseVoltageControl;
+
+ public SimpleMotorSubsystem(TalonFXMotor motor, SimpleMotorSubsystemConfiguration config) {
+ this.motor = motor;
+ name = config.name;
+ maximumVelocity = config.maximumVelocity;
+ maximumAcceleration = config.maximumAcceleration;
+ maximumJerk = config.maximumJerk;
+ velocityTolerance = config.velocityTolerance;
+ mechanism = new SpeedMechanism2d(name + "Mechanism", config.maximumDisplayableVelocity);
+ voltageRequest = new VoltageOut(0).withEnableFOC(config.focEnabled);
+ velocityRequest = new VelocityVoltage(0).withEnableFOC(config.focEnabled);
+ shouldUseVoltageControl = config.shouldUseVoltageControl;
+ sysIDConfig = new SysIdRoutine.Config(
+ Units.Volts.of(config.sysIDRampRate).per(Units.Seconds),
+ Units.Volts.of(config.sysIDStepVoltage),
+ null
+ );
+ motor.setPhysicsSimulation(
+ new SimpleMotorSimulation(
+ config.gearbox,
+ config.gearRatio,
+ config.momentOfInertia
+ ));
+
+ setName(name);
+ }
+
+ @Override
+ public String getName() {
+ return name;
+ }
+
+ @Override
+ public SysIdRoutine.Config getSysIDConfig() {
+ return sysIDConfig;
+ }
+
+ @Override
+ public void setBrake(boolean brake) {
+ motor.setBrake(brake);
+ }
+
+ @Override
+ public void stop() {
+ motor.stopMotor();
+ }
+
+ @Override
+ public void updateLog(SysIdRoutineLog log) {
+ log.motor(name)
+ .angularPosition(Units.Degrees.of(motor.getSignal(TalonFXSignal.POSITION)))
+ .angularVelocity(Units.RotationsPerSecond.of(motor.getSignal(TalonFXSignal.VELOCITY)))
+ .voltage(Units.Volts.of(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)));
+ }
+
+ @Override
+ public void updateMechanism() {
+ if (shouldUseVoltageControl) {
+ mechanism.update(getVoltage());
+ return;
+ }
+ mechanism.update(
+ getVelocityRotationsPerSecond(),
+ targetState.getTargetUnit()
+ );
+ }
+
+ @Override
+ public void updatePeriodically() {
+ motor.update();
+ Logger.recordOutput(name + "/CurrentVelocityRotationsPerSecond", motor.getSignal(TalonFXSignal.VELOCITY));
+ Logger.recordOutput(name + "/CurrentVoltage", motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE));
+ }
+
+ @Override
+ public void sysIDDrive(double targetVoltage) {
+ setTargetVoltage(targetVoltage);
+ }
+
+ public boolean atState(SimpleMotorSubsystem.SimpleMotorState targetState) {
+ return this.targetState == targetState && atTargetState();
+ }
+
+ public boolean atTargetState() {
+ if (targetState == null)
+ return false;
+
+ if (shouldUseVoltageControl)
+ return true;
+ return Math.abs(getVelocityRotationsPerSecond() - targetState.getTargetUnit()) < velocityTolerance;
+ }
+
+ public double getVelocityRotationsPerSecond() {
+ return motor.getSignal(TalonFXSignal.VELOCITY);
+ }
+
+ public double getVoltage() {
+ return motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE);
+ }
+
+ public void setTargetState(SimpleMotorSubsystem.SimpleMotorState targetState) {
+ if (shouldUseVoltageControl) {
+ setTargetStateWithVoltage(targetState);
+ return;
+ }
+ setTargetStateWithVelocity(targetState);
+ }
+
+ public void setControl(ControlRequest request) {
+ motor.setControl(request);
+ }
+
+ public void setTargetVoltage(double targetVoltage) {
+ setControl(voltageRequest.withOutput(targetVoltage));
+ }
+
+ public void setTargetVelocity(double targetVelocityRotationsPerSecond) {
+ setControl(velocityRequest.withVelocity(targetVelocityRotationsPerSecond));
+ }
+
+ private void setTargetStateWithVoltage(SimpleMotorSubsystem.SimpleMotorState targetState) {
+ this.targetState = targetState;
+ setTargetVoltage(targetState.getTargetUnit());
+ }
+
+ private void setTargetStateWithVelocity(SimpleMotorSubsystem.SimpleMotorState targetState) {
+ this.targetState = targetState;
+ setTargetVelocity(targetState.getTargetUnit());
+ }
+
+ /**
+ * An interface for a Simple motor state.
+ * getTargetUnit represents the target Velocity or Voltage (depending on the control mode) of the state.
+ */
+ public interface SimpleMotorState {
+ double getTargetUnit();
+
+ SimpleMotorState getPrepareState();
+ }
+}
diff --git a/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystemCommands.java b/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystemCommands.java
new file mode 100644
index 00000000..b5b78036
--- /dev/null
+++ b/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystemCommands.java
@@ -0,0 +1,42 @@
+package trigon.hardware.subsystems.flywheel;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import trigon.commands.ExecuteEndCommand;
+import trigon.commands.NetworkTablesCommand;
+
+import java.util.Set;
+
+public class SimpleMotorSubsystemCommands {
+ public Command getDebuggingCommand(SimpleMotorSubsystem subsystem) {
+ return new NetworkTablesCommand(
+ subsystem::setTargetVelocity,
+ false,
+ Set.of(subsystem),
+ subsystem.getName() + "TargetVelocityRotationsPerSecond"
+ );
+ }
+
+ public Command getSetTargetStateCommand(SimpleMotorSubsystem.SimpleMotorState targetState, SimpleMotorSubsystem subsystem) {
+ return new ExecuteEndCommand(
+ () -> subsystem.setTargetState(targetState),
+ subsystem::stop,
+ subsystem
+ );
+ }
+
+ public Command getSetTargetStateCommand(double targetVelocityRotationsPerSecond, SimpleMotorSubsystem subsystem) {
+ return new ExecuteEndCommand(
+ () -> subsystem.setTargetVelocity(targetVelocityRotationsPerSecond),
+ subsystem::stop,
+ subsystem
+ );
+ }
+
+ public Command getPrepareTargetStateCommand(SimpleMotorSubsystem.SimpleMotorState targetState, SimpleMotorSubsystem subsystem) {
+ return new ExecuteEndCommand(
+ () -> subsystem.setTargetState(targetState.getPrepareState()),
+ subsystem::stop,
+ subsystem
+ );
+ }
+}
diff --git a/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystemConfiguration.java b/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystemConfiguration.java
new file mode 100644
index 00000000..04076448
--- /dev/null
+++ b/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystemConfiguration.java
@@ -0,0 +1,57 @@
+package trigon.hardware.subsystems.flywheel;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+
+/**
+ * The config for the SimpleMotor Subsystem.
+ */
+public class SimpleMotorSubsystemConfiguration {
+ public String name = "";
+ public double
+ gearRatio = 1,
+ /**
+ * The moment of inertia of the motor
+ */
+ momentOfInertia = 0.003,
+ /**
+ * Maximum velocity of the motor.
+ */
+ maximumVelocity = 1,
+ /**
+ * Maximum acceleration of the motor.
+ */
+ maximumAcceleration = 1,
+ /**
+ * Maximum jerk of the motor.
+ */
+ maximumJerk = 1,
+ /**
+ * Maximum velocity displayed by the mechanism 2D
+ */
+ maximumDisplayableVelocity = 1,
+ /**
+ * The acceptable tolerance between the target velocity and current velocity.
+ */
+ velocityTolerance = 1,
+ /**
+ * Ramp rate used in system identification.
+ * See
+ * WPILib System Identification Introduction.
+ */
+ sysIDRampRate = 1,
+ /**
+ * Step voltage used in system identification.
+ * See
+ * WPILib System Identification Introduction.
+ */
+ sysIDStepVoltage = 1;
+ public boolean focEnabled = true;
+ /**
+ * should the motor control mode be Voltage as opposed to Velocity.
+ */
+ public boolean shouldUseVoltageControl = false;
+ /**
+ * The type and amount of motors to be used in the simulation.
+ */
+ public DCMotor gearbox = DCMotor.getKrakenX60Foc(1);
+}
From a006a2e1348348c21d1af68e7e2b60a1ee8fc417 Mon Sep 17 00:00:00 2001
From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com>
Date: Tue, 14 Oct 2025 23:06:05 +0300
Subject: [PATCH 02/17] Things changed
---
.../hardware/subsystems/arm/ArmSubsystemCommands.java | 10 +++++-----
.../subsystems/elevator/ElevatorSubsystemCommands.java | 8 ++++----
.../flywheel/SimpleMotorSubsystemCommands.java | 8 ++++----
3 files changed, 13 insertions(+), 13 deletions(-)
diff --git a/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystemCommands.java b/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystemCommands.java
index a09a549e..052d5693 100644
--- a/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystemCommands.java
+++ b/src/main/java/trigon/hardware/subsystems/arm/ArmSubsystemCommands.java
@@ -10,7 +10,7 @@
import java.util.function.DoubleConsumer;
public class ArmSubsystemCommands {
- public Command getDebuggingCommand(ArmSubsystem subsystem) {
+ public static Command getDebuggingCommand(ArmSubsystem subsystem) {
return new NetworkTablesCommand(
(targetAngleDegrees, speedScalar) -> subsystem.setTargetState(Rotation2d.fromDegrees(targetAngleDegrees), speedScalar),
false,
@@ -20,7 +20,7 @@ public Command getDebuggingCommand(ArmSubsystem subsystem) {
);
}
- public Command getGearRatioCalculationCommand(DoubleConsumer runVoltage, double backlashAccountabilityTimeSeconds, ArmSubsystem subsystem) {
+ public static Command getGearRatioCalculationCommand(DoubleConsumer runVoltage, double backlashAccountabilityTimeSeconds, ArmSubsystem subsystem) {
return new GearRatioCalculationCommand(
subsystem::getRotorPositionRotations,
subsystem::getRawMotorPositionRotations,
@@ -30,7 +30,7 @@ public Command getGearRatioCalculationCommand(DoubleConsumer runVoltage, double
);
}
- public Command getSetTargetStateCommand(ArmSubsystem.ArmState targetState, ArmSubsystem subsystem) {
+ public static Command getSetTargetStateCommand(ArmSubsystem.ArmState targetState, ArmSubsystem subsystem) {
return new ExecuteEndCommand(
() -> subsystem.setTargetState(targetState),
subsystem::stop,
@@ -38,7 +38,7 @@ public Command getSetTargetStateCommand(ArmSubsystem.ArmState targetState, ArmSu
);
}
- public Command getSetTargetStateCommand(Rotation2d targetAngle, double speedScalar, ArmSubsystem subsystem) {
+ public static Command getSetTargetStateCommand(Rotation2d targetAngle, double speedScalar, ArmSubsystem subsystem) {
return new ExecuteEndCommand(
() -> subsystem.setTargetState(targetAngle, speedScalar),
subsystem::stop,
@@ -46,7 +46,7 @@ public Command getSetTargetStateCommand(Rotation2d targetAngle, double speedScal
);
}
- public Command getPrepareTargetStateCommand(ArmSubsystem.ArmState targetState, ArmSubsystem subsystem) {
+ public static Command getPrepareTargetStateCommand(ArmSubsystem.ArmState targetState, ArmSubsystem subsystem) {
return new ExecuteEndCommand(
() -> subsystem.setTargetState(targetState.getPrepareState()),
subsystem::stop,
diff --git a/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystemCommands.java b/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystemCommands.java
index 17fe4985..3f47b4bb 100644
--- a/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystemCommands.java
+++ b/src/main/java/trigon/hardware/subsystems/elevator/ElevatorSubsystemCommands.java
@@ -7,7 +7,7 @@
import java.util.Set;
public class ElevatorSubsystemCommands {
- public Command getDebuggingCommand(ElevatorSubsystem subsystem) {
+ public static Command getDebuggingCommand(ElevatorSubsystem subsystem) {
return new NetworkTablesCommand(
subsystem::setTargetState,
false,
@@ -17,7 +17,7 @@ public Command getDebuggingCommand(ElevatorSubsystem subsystem) {
);
}
- public Command getSetTargetStateCommand(ElevatorSubsystem.ElevatorState targetState, ElevatorSubsystem subsystem) {
+ public static Command getSetTargetStateCommand(ElevatorSubsystem.ElevatorState targetState, ElevatorSubsystem subsystem) {
return new ExecuteEndCommand(
() -> subsystem.setTargetState(targetState),
subsystem::stop,
@@ -25,7 +25,7 @@ public Command getSetTargetStateCommand(ElevatorSubsystem.ElevatorState targetSt
);
}
- public Command getSetTargetStateCommand(double targetPositionMeters, double speedScalar, ElevatorSubsystem subsystem) {
+ public static Command getSetTargetStateCommand(double targetPositionMeters, double speedScalar, ElevatorSubsystem subsystem) {
return new ExecuteEndCommand(
() -> subsystem.setTargetState(targetPositionMeters, speedScalar),
subsystem::stop,
@@ -33,7 +33,7 @@ public Command getSetTargetStateCommand(double targetPositionMeters, double spee
);
}
- public Command getPrepareTargetStateCommand(ElevatorSubsystem.ElevatorState targetState, ElevatorSubsystem subsystem) {
+ public static Command getPrepareTargetStateCommand(ElevatorSubsystem.ElevatorState targetState, ElevatorSubsystem subsystem) {
return new ExecuteEndCommand(
() -> subsystem.setTargetState(targetState.getPrepareState()),
subsystem::stop,
diff --git a/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystemCommands.java b/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystemCommands.java
index b5b78036..b9f2f2ce 100644
--- a/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystemCommands.java
+++ b/src/main/java/trigon/hardware/subsystems/flywheel/SimpleMotorSubsystemCommands.java
@@ -7,7 +7,7 @@
import java.util.Set;
public class SimpleMotorSubsystemCommands {
- public Command getDebuggingCommand(SimpleMotorSubsystem subsystem) {
+ public static Command getDebuggingCommand(SimpleMotorSubsystem subsystem) {
return new NetworkTablesCommand(
subsystem::setTargetVelocity,
false,
@@ -16,7 +16,7 @@ public Command getDebuggingCommand(SimpleMotorSubsystem subsystem) {
);
}
- public Command getSetTargetStateCommand(SimpleMotorSubsystem.SimpleMotorState targetState, SimpleMotorSubsystem subsystem) {
+ public static Command getSetTargetStateCommand(SimpleMotorSubsystem.SimpleMotorState targetState, SimpleMotorSubsystem subsystem) {
return new ExecuteEndCommand(
() -> subsystem.setTargetState(targetState),
subsystem::stop,
@@ -24,7 +24,7 @@ public Command getSetTargetStateCommand(SimpleMotorSubsystem.SimpleMotorState ta
);
}
- public Command getSetTargetStateCommand(double targetVelocityRotationsPerSecond, SimpleMotorSubsystem subsystem) {
+ public static Command getSetTargetStateCommand(double targetVelocityRotationsPerSecond, SimpleMotorSubsystem subsystem) {
return new ExecuteEndCommand(
() -> subsystem.setTargetVelocity(targetVelocityRotationsPerSecond),
subsystem::stop,
@@ -32,7 +32,7 @@ public Command getSetTargetStateCommand(double targetVelocityRotationsPerSecond,
);
}
- public Command getPrepareTargetStateCommand(SimpleMotorSubsystem.SimpleMotorState targetState, SimpleMotorSubsystem subsystem) {
+ public static Command getPrepareTargetStateCommand(SimpleMotorSubsystem.SimpleMotorState targetState, SimpleMotorSubsystem subsystem) {
return new ExecuteEndCommand(
() -> subsystem.setTargetState(targetState.getPrepareState()),
subsystem::stop,
From 7feb7aa6d6a7e46c15319fe260d1ab83cac61d66 Mon Sep 17 00:00:00 2001
From: ShmayaR
Date: Sun, 30 Nov 2025 17:22:07 +0200
Subject: [PATCH 03/17] Update PoseEstimator.java
---
.../robot/poseestimation/poseestimator/PoseEstimator.java | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java
index 1fb75fe0..74df2188 100644
--- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java
+++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java
@@ -135,7 +135,7 @@ public void updatePoseEstimatorOdometry(SwerveModulePosition[][] swerveWheelPosi
}
/**
- * Gets the estimated pose of the robot at the target timestamp.
+ * Samples the estimated pose of the robot at a specific timestamp.
*
* @param timestamp the target timestamp
* @return the robot's estimated pose at the timestamp
From fa22f14b57d4c66d93792484770155c64d658fb2 Mon Sep 17 00:00:00 2001
From: ShmayaR
Date: Sun, 30 Nov 2025 17:24:04 +0200
Subject: [PATCH 04/17] added a bunch of numbers
---
.../subsystems/swerve/SwerveConstants.java | 22 +++++++++----------
1 file changed, 11 insertions(+), 11 deletions(-)
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java
index 3269a171..065d8236 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java
@@ -28,15 +28,15 @@ public class SwerveConstants {
REAR_LEFT_ID = 3,
REAR_RIGHT_ID = 4;
private static final double
- FRONT_LEFT_STEER_ENCODER_OFFSET = 0,
- FRONT_RIGHT_STEER_ENCODER_OFFSET = 0,
- REAR_LEFT_STEER_ENCODER_OFFSET = 0,
- REAR_RIGHT_STEER_ENCODER_OFFSET = 0;
+ FRONT_LEFT_STEER_ENCODER_OFFSET = -0.044677734375,
+ FRONT_RIGHT_STEER_ENCODER_OFFSET = 0.3525390625,
+ REAR_LEFT_STEER_ENCODER_OFFSET = -0.30517578125,
+ REAR_RIGHT_STEER_ENCODER_OFFSET = -0.121826171875;
private static final double
- FRONT_LEFT_WHEEL_DIAMETER = 0.05 * 2,
- FRONT_RIGHT_WHEEL_DIAMETER = 0.05 * 2,
- REAR_LEFT_WHEEL_DIAMETER = 0.05 * 2,
- REAR_RIGHT_WHEEL_DIAMETER = 0.05 * 2;
+ FRONT_LEFT_WHEEL_DIAMETER = 0.1016,
+ FRONT_RIGHT_WHEEL_DIAMETER = 0.1016,
+ REAR_LEFT_WHEEL_DIAMETER = 0.1016,
+ REAR_RIGHT_WHEEL_DIAMETER = 0.1016;
static final SwerveModule[] SWERVE_MODULES = new SwerveModule[]{
new SwerveModule(FRONT_LEFT_ID, FRONT_LEFT_STEER_ENCODER_OFFSET, FRONT_LEFT_WHEEL_DIAMETER),
new SwerveModule(FRONT_RIGHT_ID, FRONT_RIGHT_STEER_ENCODER_OFFSET, FRONT_RIGHT_WHEEL_DIAMETER),
@@ -99,9 +99,9 @@ public class SwerveConstants {
private static void configureGyro() {
final Pigeon2Configuration config = new Pigeon2Configuration();
- config.MountPose.MountPoseYaw = 0;
- config.MountPose.MountPosePitch = 0;
- config.MountPose.MountPoseRoll = 0;
+ config.MountPose.MountPoseYaw = Units.degreesToRotations(-52.198792);
+ config.MountPose.MountPosePitch = Units.degreesToRadians(-0.087891);
+ config.MountPose.MountPoseRoll = Units.degreesToRadians(-0.659180);
GYRO.applyConfiguration(config);
GYRO.setSimulationYawVelocitySupplier(() -> RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond);
From 0daebb132e109d7300f8860ed1f898637e6fabb4 Mon Sep 17 00:00:00 2001
From: ShmayaR
Date: Sun, 30 Nov 2025 17:28:06 +0200
Subject: [PATCH 05/17] Revert "added a bunch of numbers"
This reverts commit fa22f14b57d4c66d93792484770155c64d658fb2.
---
.../subsystems/swerve/SwerveConstants.java | 22 +++++++++----------
1 file changed, 11 insertions(+), 11 deletions(-)
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java
index 065d8236..3269a171 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java
@@ -28,15 +28,15 @@ public class SwerveConstants {
REAR_LEFT_ID = 3,
REAR_RIGHT_ID = 4;
private static final double
- FRONT_LEFT_STEER_ENCODER_OFFSET = -0.044677734375,
- FRONT_RIGHT_STEER_ENCODER_OFFSET = 0.3525390625,
- REAR_LEFT_STEER_ENCODER_OFFSET = -0.30517578125,
- REAR_RIGHT_STEER_ENCODER_OFFSET = -0.121826171875;
+ FRONT_LEFT_STEER_ENCODER_OFFSET = 0,
+ FRONT_RIGHT_STEER_ENCODER_OFFSET = 0,
+ REAR_LEFT_STEER_ENCODER_OFFSET = 0,
+ REAR_RIGHT_STEER_ENCODER_OFFSET = 0;
private static final double
- FRONT_LEFT_WHEEL_DIAMETER = 0.1016,
- FRONT_RIGHT_WHEEL_DIAMETER = 0.1016,
- REAR_LEFT_WHEEL_DIAMETER = 0.1016,
- REAR_RIGHT_WHEEL_DIAMETER = 0.1016;
+ FRONT_LEFT_WHEEL_DIAMETER = 0.05 * 2,
+ FRONT_RIGHT_WHEEL_DIAMETER = 0.05 * 2,
+ REAR_LEFT_WHEEL_DIAMETER = 0.05 * 2,
+ REAR_RIGHT_WHEEL_DIAMETER = 0.05 * 2;
static final SwerveModule[] SWERVE_MODULES = new SwerveModule[]{
new SwerveModule(FRONT_LEFT_ID, FRONT_LEFT_STEER_ENCODER_OFFSET, FRONT_LEFT_WHEEL_DIAMETER),
new SwerveModule(FRONT_RIGHT_ID, FRONT_RIGHT_STEER_ENCODER_OFFSET, FRONT_RIGHT_WHEEL_DIAMETER),
@@ -99,9 +99,9 @@ public class SwerveConstants {
private static void configureGyro() {
final Pigeon2Configuration config = new Pigeon2Configuration();
- config.MountPose.MountPoseYaw = Units.degreesToRotations(-52.198792);
- config.MountPose.MountPosePitch = Units.degreesToRadians(-0.087891);
- config.MountPose.MountPoseRoll = Units.degreesToRadians(-0.659180);
+ config.MountPose.MountPoseYaw = 0;
+ config.MountPose.MountPosePitch = 0;
+ config.MountPose.MountPoseRoll = 0;
GYRO.applyConfiguration(config);
GYRO.setSimulationYawVelocitySupplier(() -> RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond);
From 8170a19a6471848fb6a620131601a40174a801f1 Mon Sep 17 00:00:00 2001
From: ShmayaR
Date: Sun, 30 Nov 2025 17:28:16 +0200
Subject: [PATCH 06/17] Revert "Update PoseEstimator.java"
This reverts commit 7feb7aa6d6a7e46c15319fe260d1ab83cac61d66.
---
.../robot/poseestimation/poseestimator/PoseEstimator.java | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java
index 74df2188..1fb75fe0 100644
--- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java
+++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java
@@ -135,7 +135,7 @@ public void updatePoseEstimatorOdometry(SwerveModulePosition[][] swerveWheelPosi
}
/**
- * Samples the estimated pose of the robot at a specific timestamp.
+ * Gets the estimated pose of the robot at the target timestamp.
*
* @param timestamp the target timestamp
* @return the robot's estimated pose at the timestamp
From dd80c617497358c43651876862d7785d6f396754 Mon Sep 17 00:00:00 2001
From: ShmayaR
Date: Sun, 30 Nov 2025 18:23:47 +0200
Subject: [PATCH 07/17] resolved conflicts
---
src/main/java/frc/trigon/robot/RobotContainer.java | 4 ++--
.../robot/commands/commandfactories/GeneralCommands.java | 2 +-
src/main/java/lib/subsystems/MotorSubsystem.java | 2 +-
src/main/java/lib/subsystems/arm/ArmSubsystem.java | 4 ++--
src/main/java/lib/subsystems/arm/ArmSubsystemCommands.java | 2 +-
.../java/lib/subsystems/arm/ArmSubsystemConfiguration.java | 2 +-
src/main/java/lib/subsystems/elevator/ElevatorSubsystem.java | 4 ++--
.../lib/subsystems/elevator/ElevatorSubsystemCommands.java | 2 +-
.../subsystems/elevator/ElevatorSubsystemConfiguration.java | 2 +-
.../java/lib/subsystems/flywheel/SimpleMotorSubsystem.java | 4 ++--
.../lib/subsystems/flywheel/SimpleMotorSubsystemCommands.java | 2 +-
.../flywheel/SimpleMotorSubsystemConfiguration.java | 2 +-
12 files changed, 16 insertions(+), 16 deletions(-)
diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java
index 5c6e9e31..bc3e7aa5 100644
--- a/src/main/java/frc/trigon/robot/RobotContainer.java
+++ b/src/main/java/frc/trigon/robot/RobotContainer.java
@@ -18,10 +18,10 @@
import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
-import frc.trigon.robot.subsystems.MotorSubsystem;
import frc.trigon.robot.subsystems.swerve.Swerve;
-import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import lib.subsystems.MotorSubsystem;
import lib.utilities.flippable.Flippable;
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
public class RobotContainer {
public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator();
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java
index b11500ce..be9aec58 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java
@@ -4,7 +4,7 @@
import frc.trigon.robot.commands.CommandConstants;
import frc.trigon.robot.constants.OperatorConstants;
import frc.trigon.robot.subsystems.swerve.SwerveCommands;
-import trigon.hardware.subsystems.MotorSubsystem;
+import lib.subsystems.MotorSubsystem;
import java.util.function.BooleanSupplier;
diff --git a/src/main/java/lib/subsystems/MotorSubsystem.java b/src/main/java/lib/subsystems/MotorSubsystem.java
index 2663fe46..2e6e2cfa 100644
--- a/src/main/java/lib/subsystems/MotorSubsystem.java
+++ b/src/main/java/lib/subsystems/MotorSubsystem.java
@@ -1,4 +1,4 @@
-package trigon.hardware.subsystems;
+package lib.subsystems;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.DriverStation;
diff --git a/src/main/java/lib/subsystems/arm/ArmSubsystem.java b/src/main/java/lib/subsystems/arm/ArmSubsystem.java
index 2129b659..3b6c5681 100644
--- a/src/main/java/lib/subsystems/arm/ArmSubsystem.java
+++ b/src/main/java/lib/subsystems/arm/ArmSubsystem.java
@@ -1,4 +1,4 @@
-package trigon.hardware.subsystems.arm;
+package lib.subsystems.arm;
import com.ctre.phoenix6.controls.ControlRequest;
import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage;
@@ -12,7 +12,7 @@
import lib.hardware.phoenix6.talonfx.TalonFXMotor;
import lib.hardware.phoenix6.talonfx.TalonFXSignal;
import lib.hardware.simulation.SingleJointedArmSimulation;
-import trigon.hardware.subsystems.MotorSubsystem;
+import lib.subsystems.MotorSubsystem;
import lib.utilities.mechanisms.SingleJointedArmMechanism2d;
public class ArmSubsystem extends MotorSubsystem {
diff --git a/src/main/java/lib/subsystems/arm/ArmSubsystemCommands.java b/src/main/java/lib/subsystems/arm/ArmSubsystemCommands.java
index 49ed4911..7d2ed27f 100644
--- a/src/main/java/lib/subsystems/arm/ArmSubsystemCommands.java
+++ b/src/main/java/lib/subsystems/arm/ArmSubsystemCommands.java
@@ -1,4 +1,4 @@
-package trigon.hardware.subsystems.arm;
+package lib.subsystems.arm;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
diff --git a/src/main/java/lib/subsystems/arm/ArmSubsystemConfiguration.java b/src/main/java/lib/subsystems/arm/ArmSubsystemConfiguration.java
index 8638ec8d..7d316336 100644
--- a/src/main/java/lib/subsystems/arm/ArmSubsystemConfiguration.java
+++ b/src/main/java/lib/subsystems/arm/ArmSubsystemConfiguration.java
@@ -1,4 +1,4 @@
-package trigon.hardware.subsystems.arm;
+package lib.subsystems.arm;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.system.plant.DCMotor;
diff --git a/src/main/java/lib/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/lib/subsystems/elevator/ElevatorSubsystem.java
index eb6f7e43..12ef904e 100644
--- a/src/main/java/lib/subsystems/elevator/ElevatorSubsystem.java
+++ b/src/main/java/lib/subsystems/elevator/ElevatorSubsystem.java
@@ -1,4 +1,4 @@
-package trigon.hardware.subsystems.elevator;
+package lib.subsystems.elevator;
import com.ctre.phoenix6.controls.ControlRequest;
import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage;
@@ -14,7 +14,7 @@
import lib.hardware.phoenix6.talonfx.TalonFXMotor;
import lib.hardware.phoenix6.talonfx.TalonFXSignal;
import lib.hardware.simulation.ElevatorSimulation;
-import trigon.hardware.subsystems.MotorSubsystem;
+import lib.subsystems.MotorSubsystem;
import lib.utilities.Conversions;
import lib.utilities.mechanisms.ElevatorMechanism2d;
diff --git a/src/main/java/lib/subsystems/elevator/ElevatorSubsystemCommands.java b/src/main/java/lib/subsystems/elevator/ElevatorSubsystemCommands.java
index 575fa147..de849504 100644
--- a/src/main/java/lib/subsystems/elevator/ElevatorSubsystemCommands.java
+++ b/src/main/java/lib/subsystems/elevator/ElevatorSubsystemCommands.java
@@ -1,4 +1,4 @@
-package trigon.hardware.subsystems.elevator;
+package lib.subsystems.elevator;
import edu.wpi.first.wpilibj2.command.Command;
import lib.commands.ExecuteEndCommand;
diff --git a/src/main/java/lib/subsystems/elevator/ElevatorSubsystemConfiguration.java b/src/main/java/lib/subsystems/elevator/ElevatorSubsystemConfiguration.java
index 6542dc04..53667246 100644
--- a/src/main/java/lib/subsystems/elevator/ElevatorSubsystemConfiguration.java
+++ b/src/main/java/lib/subsystems/elevator/ElevatorSubsystemConfiguration.java
@@ -1,4 +1,4 @@
-package trigon.hardware.subsystems.elevator;
+package lib.subsystems.elevator;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.util.Color;
diff --git a/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystem.java b/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystem.java
index 50c973ec..dbd93865 100644
--- a/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystem.java
+++ b/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystem.java
@@ -1,4 +1,4 @@
-package trigon.hardware.subsystems.flywheel;
+package lib.subsystems.flywheel;
import com.ctre.phoenix6.controls.ControlRequest;
import com.ctre.phoenix6.controls.VelocityVoltage;
@@ -10,7 +10,7 @@
import lib.hardware.phoenix6.talonfx.TalonFXMotor;
import lib.hardware.phoenix6.talonfx.TalonFXSignal;
import lib.hardware.simulation.SimpleMotorSimulation;
-import trigon.hardware.subsystems.MotorSubsystem;
+import lib.subsystems.MotorSubsystem;
import lib.utilities.mechanisms.SpeedMechanism2d;
public class SimpleMotorSubsystem extends MotorSubsystem {
diff --git a/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemCommands.java b/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemCommands.java
index fb89220e..d4bcb225 100644
--- a/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemCommands.java
+++ b/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemCommands.java
@@ -1,4 +1,4 @@
-package trigon.hardware.subsystems.flywheel;
+package lib.subsystems.flywheel;
import edu.wpi.first.wpilibj2.command.Command;
import lib.commands.ExecuteEndCommand;
diff --git a/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemConfiguration.java b/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemConfiguration.java
index 04076448..6328b99a 100644
--- a/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemConfiguration.java
+++ b/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemConfiguration.java
@@ -1,4 +1,4 @@
-package trigon.hardware.subsystems.flywheel;
+package lib.subsystems.flywheel;
import edu.wpi.first.math.system.plant.DCMotor;
From f4e552600f94910e9f0eaf27f023e535e2fe4353 Mon Sep 17 00:00:00 2001
From: ShmayaR
Date: Sun, 30 Nov 2025 18:36:50 +0200
Subject: [PATCH 08/17] resolved conflicts
---
src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
index 5fc7d07e..426adf80 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
@@ -13,7 +13,7 @@
import frc.trigon.robot.RobotContainer;
import frc.trigon.robot.constants.PathPlannerConstants;
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants;
-import frc.trigon.robot.subsystems.MotorSubsystem;
+import lib.subsystems.MotorSubsystem;
import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule;
import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants;
import lib.hardware.phoenix6.Phoenix6SignalThread;
From f5357a2f5101f07e2a56f9470d0362547d3aa567 Mon Sep 17 00:00:00 2001
From: ShmayaR
Date: Mon, 5 Jan 2026 15:11:57 +0200
Subject: [PATCH 09/17] Merge branch 'main' into subsystem-wrapper
---
.../java/frc/trigon/robot/RobotContainer.java | 2 +-
.../commandfactories/GeneralCommands.java | 2 +-
.../robot/subsystems/swerve/Swerve.java | 2 +-
.../java/lib/subsystems/MotorSubsystem.java | 171 ---------------
.../java/lib/subsystems/arm/ArmSubsystem.java | 186 -----------------
.../subsystems/arm/ArmSubsystemCommands.java | 56 -----
.../arm/ArmSubsystemConfiguration.java | 81 --------
.../elevator/ElevatorSubsystem.java | 194 ------------------
.../elevator/ElevatorSubsystemCommands.java | 43 ----
.../ElevatorSubsystemConfiguration.java | 73 -------
.../flywheel/SimpleMotorSubsystem.java | 170 ---------------
.../SimpleMotorSubsystemCommands.java | 42 ----
.../SimpleMotorSubsystemConfiguration.java | 57 -----
13 files changed, 3 insertions(+), 1076 deletions(-)
delete mode 100644 src/main/java/lib/subsystems/MotorSubsystem.java
delete mode 100644 src/main/java/lib/subsystems/arm/ArmSubsystem.java
delete mode 100644 src/main/java/lib/subsystems/arm/ArmSubsystemCommands.java
delete mode 100644 src/main/java/lib/subsystems/arm/ArmSubsystemConfiguration.java
delete mode 100644 src/main/java/lib/subsystems/elevator/ElevatorSubsystem.java
delete mode 100644 src/main/java/lib/subsystems/elevator/ElevatorSubsystemCommands.java
delete mode 100644 src/main/java/lib/subsystems/elevator/ElevatorSubsystemConfiguration.java
delete mode 100644 src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystem.java
delete mode 100644 src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemCommands.java
delete mode 100644 src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemConfiguration.java
diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java
index 4b1a55b1..c29e399d 100644
--- a/src/main/java/frc/trigon/robot/RobotContainer.java
+++ b/src/main/java/frc/trigon/robot/RobotContainer.java
@@ -20,7 +20,7 @@
import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
import frc.trigon.robot.subsystems.swerve.Swerve;
-import lib.subsystems.MotorSubsystem;
+import frc.trigon.lib.subsystems.MotorSubsystem;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
public class RobotContainer {
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java
index be9aec58..2c8d76f1 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java
@@ -4,7 +4,7 @@
import frc.trigon.robot.commands.CommandConstants;
import frc.trigon.robot.constants.OperatorConstants;
import frc.trigon.robot.subsystems.swerve.SwerveCommands;
-import lib.subsystems.MotorSubsystem;
+import frc.trigon.lib.subsystems.MotorSubsystem;
import java.util.function.BooleanSupplier;
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
index 3f8022ef..fefb875e 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
@@ -19,7 +19,7 @@
import frc.trigon.robot.RobotContainer;
import frc.trigon.robot.constants.AutonomousConstants;
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants;
-import lib.subsystems.MotorSubsystem;
+import frc.trigon.lib.subsystems.MotorSubsystem;
import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule;
import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants;
import org.littletonrobotics.junction.AutoLogOutput;
diff --git a/src/main/java/lib/subsystems/MotorSubsystem.java b/src/main/java/lib/subsystems/MotorSubsystem.java
deleted file mode 100644
index 55c0697f..00000000
--- a/src/main/java/lib/subsystems/MotorSubsystem.java
+++ /dev/null
@@ -1,171 +0,0 @@
-package lib.subsystems;
-
-import edu.wpi.first.units.Units;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
-import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean;
-import frc.trigon.lib.hardware.RobotHardwareStats;
-
-import java.util.ArrayList;
-import java.util.List;
-import java.util.concurrent.Executor;
-import java.util.concurrent.Executors;
-import java.util.function.Consumer;
-
-/**
- * A class that represents a subsystem that has motors (rather than something like LEDs).
- * This class will automatically stop all the motors when the robot is disabled, and set the motors to brake when the robot is enabled.
- * If a subsystem doesn't need to ever brake (i.e. shooter, flywheel, etc.), then it should override the {@link #setBrake(boolean)} method and do nothing.
- */
-public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.SubsystemBase {
- public static boolean IS_BRAKING = true;
- private static final List REGISTERED_SUBSYSTEMS = new ArrayList<>();
- private static final Trigger DISABLED_TRIGGER = new Trigger(DriverStation::isDisabled);
- private static final Executor BRAKE_MODE_EXECUTOR = Executors.newFixedThreadPool(8);
- private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", RobotHardwareStats.isSimulation());
-
- static {
- DISABLED_TRIGGER.onTrue(new InstantCommand(() -> forEach(MotorSubsystem::stop)).ignoringDisable(true));
- DISABLED_TRIGGER.onFalse(new InstantCommand(() -> {
- setAllSubsystemsBrakeAsync(true);
- IS_BRAKING = true;
- }).ignoringDisable(true));
- }
-
- private final SysIdRoutine sysIDRoutine = createSysIDRoutine();
-
- public MotorSubsystem() {
- REGISTERED_SUBSYSTEMS.add(this);
- }
-
- /**
- * Runs the given consumer on all the subsystem instances.
- *
- * @param toRun the consumer to run on each registered subsystem
- */
- public static void forEach(Consumer toRun) {
- REGISTERED_SUBSYSTEMS.forEach(toRun);
- }
-
- /**
- * Sets whether the all the subsystems should brake or coast their motors.
- * This command will run asynchronously, since the TalonFX's setting of brake/coast is blocking.
- *
- * @param brake whether the motors should brake or coast
- */
- public static void setAllSubsystemsBrakeAsync(boolean brake) {
- BRAKE_MODE_EXECUTOR.execute(() -> forEach((subsystem) -> subsystem.setBrake(brake)));
- }
-
- public static boolean isExtensiveLoggingEnabled() {
- return ENABLE_EXTENSIVE_LOGGING.get() || RobotHardwareStats.isReplay();
- }
-
- /**
- * Runs periodically, to update the subsystem, and update the mechanism of the subsystem (if there is one).
- * This only updates the mechanism if the robot is in replay mode or extensive logging is enabled.
- * This function cannot be overridden. Use {@linkplain MotorSubsystem#updatePeriodically} or {@linkplain MotorSubsystem#updateMechanism} (depending on the usage) instead.
- */
- @Override
- public final void periodic() {
- updatePeriodically();
- if (isExtensiveLoggingEnabled())
- updateMechanism();
- }
-
- /**
- * Creates a quasistatic (ramp up) command for characterizing the subsystem's mechanism.
- *
- * @param direction the direction in which to run the test
- * @return the command
- * @throws IllegalStateException if the {@link MotorSubsystem#getSysIDConfig()} function wasn't overridden or returns null
- */
- public final Command getQuasistaticCharacterizationCommand(SysIdRoutine.Direction direction) throws IllegalStateException {
- if (sysIDRoutine == null)
- throw new IllegalStateException("Subsystem " + getName() + " doesn't have a SysID routine!");
- return sysIDRoutine.quasistatic(direction);
- }
-
- /**
- * Creates a dynamic (constant "step up") command for characterizing the subsystem's mechanism.
- *
- * @param direction the direction in which to run the test
- * @return the command
- * @throws IllegalStateException if the {@link MotorSubsystem#getSysIDConfig()} function wasn't overridden or returns null
- */
- public final Command getDynamicCharacterizationCommand(SysIdRoutine.Direction direction) throws IllegalStateException {
- if (sysIDRoutine == null)
- throw new IllegalStateException("Subsystem " + getName() + " doesn't have a SysID routine!");
- return sysIDRoutine.dynamic(direction);
- }
-
- /**
- * Drives the motor with the given voltage for characterizing.
- *
- * @param targetDrivePower the target drive power, unitless. This can be amps, volts, etc. Depending on the characterization type
- */
- public void sysIDDrive(double targetDrivePower) {
- }
-
- /**
- * Updates the SysId log of the motor states for characterizing.
- *
- * @param log the log to update
- */
- public void updateLog(SysIdRoutineLog log) {
- }
-
- public SysIdRoutine.Config getSysIDConfig() {
- return null;
- }
-
- /**
- * Sets whether the subsystem's motors should brake or coast.
- * If a subsystem doesn't need to ever brake (i.e. shooter, flywheel, etc.), don't implement this method.
- *
- * @param brake whether the motors should brake or coast
- */
- public void setBrake(boolean brake) {
- }
-
- /**
- * Updates periodically. Anything that should be updated periodically but isn't related to the mechanism (or shouldn't always be logged, in order to save resources) should be put here.
- */
- public void updatePeriodically() {
- }
-
- /**
- * Updates the mechanism of the subsystem periodically if the robot is in replay mode, or if {@linkplain MotorSubsystem#ENABLE_EXTENSIVE_LOGGING) is true.
- * This doesn't always run in order to save resources.
- */
- public void updateMechanism() {
- }
-
- public void changeDefaultCommand(Command newDefaultCommand) {
- final Command currentDefaultCommand = getDefaultCommand();
- if (currentDefaultCommand != null)
- currentDefaultCommand.cancel();
- setDefaultCommand(newDefaultCommand);
- }
-
- public abstract void stop();
-
- private SysIdRoutine createSysIDRoutine() {
- if (getSysIDConfig() == null)
- return null;
-
- return new SysIdRoutine(
- getSysIDConfig(),
- new SysIdRoutine.Mechanism(
- (voltage) -> sysIDDrive(voltage.in(Units.Volts)),
- this::updateLog,
- this,
- getName()
- )
- );
- }
-}
\ No newline at end of file
diff --git a/src/main/java/lib/subsystems/arm/ArmSubsystem.java b/src/main/java/lib/subsystems/arm/ArmSubsystem.java
deleted file mode 100644
index 3b6c5681..00000000
--- a/src/main/java/lib/subsystems/arm/ArmSubsystem.java
+++ /dev/null
@@ -1,186 +0,0 @@
-package lib.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 lib.hardware.RobotHardwareStats;
-import lib.hardware.phoenix6.talonfx.TalonFXMotor;
-import lib.hardware.phoenix6.talonfx.TalonFXSignal;
-import lib.hardware.simulation.SingleJointedArmSimulation;
-import lib.subsystems.MotorSubsystem;
-import lib.utilities.mechanisms.SingleJointedArmMechanism2d;
-
-public class ArmSubsystem extends MotorSubsystem {
- 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, ArmSubsystemConfiguration 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),
- null
- );
- motor.setPhysicsSimulation(
- new SingleJointedArmSimulation(
- config.gearbox,
- config.gearRatio,
- config.massKilograms,
- config.lengthMeters,
- config.minimumAngle,
- config.maximumAngle,
- config.shouldSimulateGravity
- ));
-
- setName(name);
- }
-
- @Override
- public String getName() {
- return name;
- }
-
- @Override
- public SysIdRoutine.Config getSysIDConfig() {
- return sysIDConfig;
- }
-
- @Override
- public void setBrake(boolean brake) {
- motor.setBrake(brake);
- }
-
- @Override
- public void stop() {
- motor.stopMotor();
- }
-
- @Override
- 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)));
- }
-
- @Override
- public void updateMechanism() {
- logComponentPose();
-
- mechanism.update(
- getAngle(),
- Rotation2d.fromRotations(motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + (RobotHardwareStats.isSimulation() ? 0 : visualizationOffsetFromGravityOffset))
- );
- }
-
- @Override
- public void updatePeriodically() {
- motor.update();
- Logger.recordOutput(name + "/CurrentAngle", getAngle());
- }
-
- @Override
- 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);
- }
-
- protected double getRotorPositionRotations() {
- return motor.getSignal(TalonFXSignal.ROTOR_POSITION);
- }
-
- protected double getRawMotorPositionRotations() {
- return motor.getSignal(TalonFXSignal.POSITION);
- }
-
- 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();
-
- ArmState getPrepareState();
-
- double getSpeedScalar();
- }
-}
diff --git a/src/main/java/lib/subsystems/arm/ArmSubsystemCommands.java b/src/main/java/lib/subsystems/arm/ArmSubsystemCommands.java
deleted file mode 100644
index 7d2ed27f..00000000
--- a/src/main/java/lib/subsystems/arm/ArmSubsystemCommands.java
+++ /dev/null
@@ -1,56 +0,0 @@
-package lib.subsystems.arm;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj2.command.Command;
-import lib.commands.ExecuteEndCommand;
-import lib.commands.GearRatioCalculationCommand;
-import lib.commands.NetworkTablesCommand;
-
-import java.util.Set;
-import java.util.function.DoubleConsumer;
-
-public class ArmSubsystemCommands {
- public static Command getDebuggingCommand(ArmSubsystem subsystem) {
- return new NetworkTablesCommand(
- (targetAngleDegrees, speedScalar) -> subsystem.setTargetState(Rotation2d.fromDegrees(targetAngleDegrees), speedScalar),
- false,
- Set.of(subsystem),
- subsystem.getName() + "TargetAngleDegrees",
- subsystem.getName() + "SpeedScalar"
- );
- }
-
- public static Command getGearRatioCalculationCommand(DoubleConsumer runVoltage, double backlashAccountabilityTimeSeconds, ArmSubsystem subsystem) {
- return new GearRatioCalculationCommand(
- subsystem::getRotorPositionRotations,
- subsystem::getRawMotorPositionRotations,
- runVoltage,
- backlashAccountabilityTimeSeconds,
- subsystem
- );
- }
-
- public static Command getSetTargetStateCommand(ArmSubsystem.ArmState targetState, ArmSubsystem subsystem) {
- return new ExecuteEndCommand(
- () -> subsystem.setTargetState(targetState),
- subsystem::stop,
- subsystem
- );
- }
-
- public static Command getSetTargetStateCommand(Rotation2d targetAngle, double speedScalar, ArmSubsystem subsystem) {
- return new ExecuteEndCommand(
- () -> subsystem.setTargetState(targetAngle, speedScalar),
- subsystem::stop,
- subsystem
- );
- }
-
- public static Command getPrepareTargetStateCommand(ArmSubsystem.ArmState targetState, ArmSubsystem subsystem) {
- return new ExecuteEndCommand(
- () -> subsystem.setTargetState(targetState.getPrepareState()),
- subsystem::stop,
- subsystem
- );
- }
-}
diff --git a/src/main/java/lib/subsystems/arm/ArmSubsystemConfiguration.java b/src/main/java/lib/subsystems/arm/ArmSubsystemConfiguration.java
deleted file mode 100644
index 7d316336..00000000
--- a/src/main/java/lib/subsystems/arm/ArmSubsystemConfiguration.java
+++ /dev/null
@@ -1,81 +0,0 @@
-package lib.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 ArmSubsystemConfiguration {
- 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
- * WPILib System Identification Introduction.
- */
- public double sysIDRampRate = 1;
-
- /**
- * Step voltage used in system identification.
- * See
- * WPILib System Identification Introduction.
- */
- 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);
-}
diff --git a/src/main/java/lib/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/lib/subsystems/elevator/ElevatorSubsystem.java
deleted file mode 100644
index 12ef904e..00000000
--- a/src/main/java/lib/subsystems/elevator/ElevatorSubsystem.java
+++ /dev/null
@@ -1,194 +0,0 @@
-package lib.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 lib.hardware.phoenix6.talonfx.TalonFXMotor;
-import lib.hardware.phoenix6.talonfx.TalonFXSignal;
-import lib.hardware.simulation.ElevatorSimulation;
-import lib.subsystems.MotorSubsystem;
-import lib.utilities.Conversions;
-import lib.utilities.mechanisms.ElevatorMechanism2d;
-
-public class ElevatorSubsystem extends MotorSubsystem {
- 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, ElevatorSubsystemConfiguration 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),
- null
- );
- motor.setPhysicsSimulation(
- new ElevatorSimulation(
- config.gearbox,
- config.gearRatio,
- config.massKilograms,
- config.drumRadiusMeters,
- config.minimumHeight,
- config.maximumHeight,
- config.shouldSimulateGravity
- ));
-
- setName(name);
- }
-
- @Override
- public String getName() {
- return name;
- }
-
- @Override
- public SysIdRoutine.Config getSysIDConfig() {
- return sysIDConfig;
- }
-
- @Override
- public void setBrake(boolean brake) {
- motor.setBrake(brake);
- }
-
- @Override
- public void stop() {
- motor.stopMotor();
- }
-
- @Override
- 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)));
- }
-
- @Override
- public void updateMechanism() {
- logComponentPoses();
-
- mechanism.update(
- getPositionRotations(),
- motor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)
- );
- }
-
- @Override
- public void updatePeriodically() {
- motor.update();
- Logger.recordOutput(name + "/CurrentPositionMeters", getPositionMeters());
- }
-
- @Override
- 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);
- }
-
- /**
- * An interface for an elevator state.
- * getTargetPositionMeters represents the target position of the state.
- * getSpeedScalar represents the speed scalar of the state.
- */
- public interface ElevatorState {
- double getTargetPositionMeters();
-
- ElevatorState getPrepareState();
-
- double getSpeedScalar();
- }
-}
diff --git a/src/main/java/lib/subsystems/elevator/ElevatorSubsystemCommands.java b/src/main/java/lib/subsystems/elevator/ElevatorSubsystemCommands.java
deleted file mode 100644
index de849504..00000000
--- a/src/main/java/lib/subsystems/elevator/ElevatorSubsystemCommands.java
+++ /dev/null
@@ -1,43 +0,0 @@
-package lib.subsystems.elevator;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import lib.commands.ExecuteEndCommand;
-import lib.commands.NetworkTablesCommand;
-
-import java.util.Set;
-
-public class ElevatorSubsystemCommands {
- public static Command getDebuggingCommand(ElevatorSubsystem subsystem) {
- return new NetworkTablesCommand(
- subsystem::setTargetState,
- false,
- Set.of(subsystem),
- subsystem.getName() + "TargetPositionMeters",
- subsystem.getName() + "SpeedScalar"
- );
- }
-
- public static Command getSetTargetStateCommand(ElevatorSubsystem.ElevatorState targetState, ElevatorSubsystem subsystem) {
- return new ExecuteEndCommand(
- () -> subsystem.setTargetState(targetState),
- subsystem::stop,
- subsystem
- );
- }
-
- public static Command getSetTargetStateCommand(double targetPositionMeters, double speedScalar, ElevatorSubsystem subsystem) {
- return new ExecuteEndCommand(
- () -> subsystem.setTargetState(targetPositionMeters, speedScalar),
- subsystem::stop,
- subsystem
- );
- }
-
- public static Command getPrepareTargetStateCommand(ElevatorSubsystem.ElevatorState targetState, ElevatorSubsystem subsystem) {
- return new ExecuteEndCommand(
- () -> subsystem.setTargetState(targetState.getPrepareState()),
- subsystem::stop,
- subsystem
- );
- }
-}
diff --git a/src/main/java/lib/subsystems/elevator/ElevatorSubsystemConfiguration.java b/src/main/java/lib/subsystems/elevator/ElevatorSubsystemConfiguration.java
deleted file mode 100644
index 53667246..00000000
--- a/src/main/java/lib/subsystems/elevator/ElevatorSubsystemConfiguration.java
+++ /dev/null
@@ -1,73 +0,0 @@
-package lib.subsystems.elevator;
-
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.util.Color;
-
-public class ElevatorSubsystemConfiguration {
- 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
- * WPILib System Identification Introduction.
- */
- public double sysIDRampRate = 1;
-
- /**
- * Step voltage used in system identification.
- * See
- * WPILib System Identification Introduction.
- */
- 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);
-}
diff --git a/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystem.java b/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystem.java
deleted file mode 100644
index dbd93865..00000000
--- a/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystem.java
+++ /dev/null
@@ -1,170 +0,0 @@
-package lib.subsystems.flywheel;
-
-import com.ctre.phoenix6.controls.ControlRequest;
-import com.ctre.phoenix6.controls.VelocityVoltage;
-import com.ctre.phoenix6.controls.VoltageOut;
-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 lib.hardware.phoenix6.talonfx.TalonFXMotor;
-import lib.hardware.phoenix6.talonfx.TalonFXSignal;
-import lib.hardware.simulation.SimpleMotorSimulation;
-import lib.subsystems.MotorSubsystem;
-import lib.utilities.mechanisms.SpeedMechanism2d;
-
-public class SimpleMotorSubsystem extends MotorSubsystem {
- private final TalonFXMotor motor;
- private final String name;
- private final double
- maximumVelocity,
- maximumAcceleration,
- maximumJerk;
- private final VoltageOut voltageRequest;
- private final VelocityVoltage velocityRequest;
- private final SpeedMechanism2d mechanism;
- private final double velocityTolerance;
- private final SysIdRoutine.Config sysIDConfig;
- private SimpleMotorState targetState;
-
- private final boolean shouldUseVoltageControl;
-
- public SimpleMotorSubsystem(TalonFXMotor motor, SimpleMotorSubsystemConfiguration config) {
- this.motor = motor;
- name = config.name;
- maximumVelocity = config.maximumVelocity;
- maximumAcceleration = config.maximumAcceleration;
- maximumJerk = config.maximumJerk;
- velocityTolerance = config.velocityTolerance;
- mechanism = new SpeedMechanism2d(name + "Mechanism", config.maximumDisplayableVelocity);
- voltageRequest = new VoltageOut(0).withEnableFOC(config.focEnabled);
- velocityRequest = new VelocityVoltage(0).withEnableFOC(config.focEnabled);
- shouldUseVoltageControl = config.shouldUseVoltageControl;
- sysIDConfig = new SysIdRoutine.Config(
- Units.Volts.of(config.sysIDRampRate).per(Units.Seconds),
- Units.Volts.of(config.sysIDStepVoltage),
- null
- );
- motor.setPhysicsSimulation(
- new SimpleMotorSimulation(
- config.gearbox,
- config.gearRatio,
- config.momentOfInertia
- ));
-
- setName(name);
- }
-
- @Override
- public String getName() {
- return name;
- }
-
- @Override
- public SysIdRoutine.Config getSysIDConfig() {
- return sysIDConfig;
- }
-
- @Override
- public void setBrake(boolean brake) {
- motor.setBrake(brake);
- }
-
- @Override
- public void stop() {
- motor.stopMotor();
- }
-
- @Override
- public void updateLog(SysIdRoutineLog log) {
- log.motor(name)
- .angularPosition(Units.Degrees.of(motor.getSignal(TalonFXSignal.POSITION)))
- .angularVelocity(Units.RotationsPerSecond.of(motor.getSignal(TalonFXSignal.VELOCITY)))
- .voltage(Units.Volts.of(motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)));
- }
-
- @Override
- public void updateMechanism() {
- if (shouldUseVoltageControl) {
- mechanism.update(getVoltage());
- return;
- }
- mechanism.update(
- getVelocityRotationsPerSecond(),
- targetState.getTargetUnit()
- );
- }
-
- @Override
- public void updatePeriodically() {
- motor.update();
- Logger.recordOutput(name + "/CurrentVelocityRotationsPerSecond", motor.getSignal(TalonFXSignal.VELOCITY));
- Logger.recordOutput(name + "/CurrentVoltage", motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE));
- }
-
- @Override
- public void sysIDDrive(double targetVoltage) {
- setTargetVoltage(targetVoltage);
- }
-
- public boolean atState(SimpleMotorSubsystem.SimpleMotorState targetState) {
- return this.targetState == targetState && atTargetState();
- }
-
- public boolean atTargetState() {
- if (targetState == null)
- return false;
-
- if (shouldUseVoltageControl)
- return true;
- return Math.abs(getVelocityRotationsPerSecond() - targetState.getTargetUnit()) < velocityTolerance;
- }
-
- public double getVelocityRotationsPerSecond() {
- return motor.getSignal(TalonFXSignal.VELOCITY);
- }
-
- public double getVoltage() {
- return motor.getSignal(TalonFXSignal.MOTOR_VOLTAGE);
- }
-
- public void setTargetState(SimpleMotorSubsystem.SimpleMotorState targetState) {
- if (shouldUseVoltageControl) {
- setTargetStateWithVoltage(targetState);
- return;
- }
- setTargetStateWithVelocity(targetState);
- }
-
- public void setControl(ControlRequest request) {
- motor.setControl(request);
- }
-
- public void setTargetVoltage(double targetVoltage) {
- setControl(voltageRequest.withOutput(targetVoltage));
- }
-
- public void setTargetVelocity(double targetVelocityRotationsPerSecond) {
- setControl(velocityRequest.withVelocity(targetVelocityRotationsPerSecond));
- }
-
- private void setTargetStateWithVoltage(SimpleMotorSubsystem.SimpleMotorState targetState) {
- this.targetState = targetState;
- setTargetVoltage(targetState.getTargetUnit());
- }
-
- private void setTargetStateWithVelocity(SimpleMotorSubsystem.SimpleMotorState targetState) {
- this.targetState = targetState;
- setTargetVelocity(targetState.getTargetUnit());
- }
-
- /**
- * An interface for a Simple motor state.
- * getTargetUnit represents the target Velocity or Voltage (depending on the control mode) of the state.
- */
- public interface SimpleMotorState {
- double getTargetUnit();
-
- SimpleMotorState getPrepareState();
- }
-}
diff --git a/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemCommands.java b/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemCommands.java
deleted file mode 100644
index d4bcb225..00000000
--- a/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemCommands.java
+++ /dev/null
@@ -1,42 +0,0 @@
-package lib.subsystems.flywheel;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import lib.commands.ExecuteEndCommand;
-import lib.commands.NetworkTablesCommand;
-
-import java.util.Set;
-
-public class SimpleMotorSubsystemCommands {
- public static Command getDebuggingCommand(SimpleMotorSubsystem subsystem) {
- return new NetworkTablesCommand(
- subsystem::setTargetVelocity,
- false,
- Set.of(subsystem),
- subsystem.getName() + "TargetVelocityRotationsPerSecond"
- );
- }
-
- public static Command getSetTargetStateCommand(SimpleMotorSubsystem.SimpleMotorState targetState, SimpleMotorSubsystem subsystem) {
- return new ExecuteEndCommand(
- () -> subsystem.setTargetState(targetState),
- subsystem::stop,
- subsystem
- );
- }
-
- public static Command getSetTargetStateCommand(double targetVelocityRotationsPerSecond, SimpleMotorSubsystem subsystem) {
- return new ExecuteEndCommand(
- () -> subsystem.setTargetVelocity(targetVelocityRotationsPerSecond),
- subsystem::stop,
- subsystem
- );
- }
-
- public static Command getPrepareTargetStateCommand(SimpleMotorSubsystem.SimpleMotorState targetState, SimpleMotorSubsystem subsystem) {
- return new ExecuteEndCommand(
- () -> subsystem.setTargetState(targetState.getPrepareState()),
- subsystem::stop,
- subsystem
- );
- }
-}
diff --git a/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemConfiguration.java b/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemConfiguration.java
deleted file mode 100644
index 6328b99a..00000000
--- a/src/main/java/lib/subsystems/flywheel/SimpleMotorSubsystemConfiguration.java
+++ /dev/null
@@ -1,57 +0,0 @@
-package lib.subsystems.flywheel;
-
-import edu.wpi.first.math.system.plant.DCMotor;
-
-/**
- * The config for the SimpleMotor Subsystem.
- */
-public class SimpleMotorSubsystemConfiguration {
- public String name = "";
- public double
- gearRatio = 1,
- /**
- * The moment of inertia of the motor
- */
- momentOfInertia = 0.003,
- /**
- * Maximum velocity of the motor.
- */
- maximumVelocity = 1,
- /**
- * Maximum acceleration of the motor.
- */
- maximumAcceleration = 1,
- /**
- * Maximum jerk of the motor.
- */
- maximumJerk = 1,
- /**
- * Maximum velocity displayed by the mechanism 2D
- */
- maximumDisplayableVelocity = 1,
- /**
- * The acceptable tolerance between the target velocity and current velocity.
- */
- velocityTolerance = 1,
- /**
- * Ramp rate used in system identification.
- * See
- * WPILib System Identification Introduction.
- */
- sysIDRampRate = 1,
- /**
- * Step voltage used in system identification.
- * See
- * WPILib System Identification Introduction.
- */
- sysIDStepVoltage = 1;
- public boolean focEnabled = true;
- /**
- * should the motor control mode be Voltage as opposed to Velocity.
- */
- public boolean shouldUseVoltageControl = false;
- /**
- * The type and amount of motors to be used in the simulation.
- */
- public DCMotor gearbox = DCMotor.getKrakenX60Foc(1);
-}
From 2d96e0b7bb5da3fbea89487405038e88c4444616 Mon Sep 17 00:00:00 2001
From: ShmayaR
Date: Mon, 5 Jan 2026 15:23:42 +0200
Subject: [PATCH 10/17] set submodule to subsystem wrapper branch
---
.gitmodules | 1 +
src/main/java/frc/trigon/lib | 2 +-
2 files changed, 2 insertions(+), 1 deletion(-)
diff --git a/.gitmodules b/.gitmodules
index 93a08b0c..2d5fdfb8 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,4 @@
[submodule "src/main/java/frc/trigon/lib"]
path = src/main/java/frc/trigon/lib
url = https://github.com/Programming-TRIGON/TRIGONLib
+ branch = subsystem-wrapper
diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib
index 553e7916..850f098d 160000
--- a/src/main/java/frc/trigon/lib
+++ b/src/main/java/frc/trigon/lib
@@ -1 +1 @@
-Subproject commit 553e7916e7607dde051530e8903879fb590e573c
+Subproject commit 850f098d229d40aae99ceb6d5c0763252d6929aa
From a1684edbd522de213b40d1a3b4d6d1e71ed9af54 Mon Sep 17 00:00:00 2001
From: ShmayaR
Date: Thu, 8 Jan 2026 12:20:49 +0200
Subject: [PATCH 11/17] Update lib
---
src/main/java/frc/trigon/lib | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib
index 850f098d..90e8d8af 160000
--- a/src/main/java/frc/trigon/lib
+++ b/src/main/java/frc/trigon/lib
@@ -1 +1 @@
-Subproject commit 850f098d229d40aae99ceb6d5c0763252d6929aa
+Subproject commit 90e8d8af28793e2763a71b2f4a1fcd1949295b2c
From 1328b40b9e31df3f61a2f103fda23f0d25d87025 Mon Sep 17 00:00:00 2001
From: ShmayaR
Date: Thu, 8 Jan 2026 14:07:48 +0200
Subject: [PATCH 12/17] Update lib
---
src/main/java/frc/trigon/lib | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib
index 90e8d8af..6b9824d0 160000
--- a/src/main/java/frc/trigon/lib
+++ b/src/main/java/frc/trigon/lib
@@ -1 +1 @@
-Subproject commit 90e8d8af28793e2763a71b2f4a1fcd1949295b2c
+Subproject commit 6b9824d047b81a262d6d0bf83412fa779bd61624
From 7d1000cd227011b83b100b13819f27fc277a5c19 Mon Sep 17 00:00:00 2001
From: ShmayaR
Date: Mon, 12 Jan 2026 23:50:27 +0200
Subject: [PATCH 13/17] subsystem examples
---
.../java/frc/trigon/robot/RobotContainer.java | 22 ++-
.../frc/trigon/robot/subsystems/arm/Arm.java | 12 ++
.../robot/subsystems/arm/ArmConstants.java | 171 ++++++++++++++++++
.../robot/subsystems/elevator/Elevator.java | 12 ++
.../elevator/ElevatorConstants.java | 147 +++++++++++++++
.../robot/subsystems/flyWheel/FlyWheel.java | 9 +
.../flyWheel/FlyWheelConstants.java | 126 +++++++++++++
src/main/python/keyboard_to_nt.py | 38 ++--
8 files changed, 522 insertions(+), 15 deletions(-)
create mode 100644 src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
create mode 100644 src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java
create mode 100644 src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
create mode 100644 src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java
create mode 100644 src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheel.java
create mode 100644 src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheelConstants.java
diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java
index c29e399d..e4681c1e 100644
--- a/src/main/java/frc/trigon/robot/RobotContainer.java
+++ b/src/main/java/frc/trigon/robot/RobotContainer.java
@@ -9,6 +9,13 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.trigon.lib.subsystems.MotorSubsystem;
+import frc.trigon.lib.subsystems.arm.ArmSubsystem;
+import frc.trigon.lib.subsystems.arm.ArmSubsystemCommands;
+import frc.trigon.lib.subsystems.elevator.ElevatorSubsystem;
+import frc.trigon.lib.subsystems.elevator.ElevatorSubsystemCommands;
+import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystem;
+import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystemCommands;
import frc.trigon.lib.utilities.flippable.Flippable;
import frc.trigon.robot.commands.CommandConstants;
import frc.trigon.robot.commands.commandfactories.GeneralCommands;
@@ -19,8 +26,13 @@
import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
+import frc.trigon.robot.subsystems.arm.Arm;
+import frc.trigon.robot.subsystems.arm.ArmConstants;
+import frc.trigon.robot.subsystems.elevator.Elevator;
+import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
+import frc.trigon.robot.subsystems.flyWheel.FlyWheel;
+import frc.trigon.robot.subsystems.flyWheel.FlyWheelConstants;
import frc.trigon.robot.subsystems.swerve.Swerve;
-import frc.trigon.lib.subsystems.MotorSubsystem;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
public class RobotContainer {
@@ -32,6 +44,9 @@ public class RobotContainer {
CameraConstants.OBJECT_DETECTION_CAMERA
);
public static final Swerve SWERVE = new Swerve();
+ public static final ArmSubsystem ARM = new Arm();
+ public static final ElevatorSubsystem ELEVATOR = new Elevator();
+ public static final SimpleMotorSubsystem FLY_WHEEL = new FlyWheel();
private LoggedDashboardChooser autoChooser;
public RobotContainer() {
@@ -54,12 +69,17 @@ private void configureBindings() {
private void bindDefaultCommands() {
SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand());
+ ARM.setDefaultCommand(ArmSubsystemCommands.getSetTargetStateCommand(ArmConstants.ArmState.REST, ARM));
+ ELEVATOR.setDefaultCommand(ElevatorSubsystemCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST,ELEVATOR));
+ FLY_WHEEL.setDefaultCommand(SimpleMotorSubsystemCommands.getSetTargetStateCommand(FlyWheelConstants.FlyWheelState.REST, FLY_WHEEL));
}
private void bindControllerCommands() {
OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND);
OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND);
OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand());
+ OperatorConstants.OPERATOR_CONTROLLER.a().whileTrue(ElevatorSubsystemCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.HALF, ELEVATOR));
+ OperatorConstants.OPERATOR_CONTROLLER.s().whileTrue(ElevatorSubsystemCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.FULL, ELEVATOR));
}
private void configureSysIDBindings(MotorSubsystem subsystem) {
diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
new file mode 100644
index 00000000..d9a3f009
--- /dev/null
+++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
@@ -0,0 +1,12 @@
+package frc.trigon.robot.subsystems.arm;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor;
+import frc.trigon.lib.subsystems.arm.ArmSubsystem;
+import frc.trigon.lib.subsystems.arm.ArmSubsystemConfiguration;
+
+public class Arm extends ArmSubsystem {
+ public Arm() {
+ super(ArmConstants.MASTER_MOTOR, ArmConstants.ARM_CONFIG, new Pose3d());
+ }
+}
diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java
new file mode 100644
index 00000000..90f46b5e
--- /dev/null
+++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java
@@ -0,0 +1,171 @@
+package frc.trigon.robot.subsystems.arm;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.signals.*;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderEncoder;
+import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderSignal;
+import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor;
+import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal;
+import frc.trigon.lib.subsystems.arm.ArmSubsystem;
+import frc.trigon.lib.subsystems.arm.ArmSubsystemConfiguration;
+
+public class ArmConstants {
+ private static final int
+ MASTER_MOTOR_ID = 11,
+ FOLLOWER_MOTOR_ID = 12,
+ ENCODER_ID = 11;
+ private static final String
+ MASTER_MOTOR_NAME = "ArmMasterMotor",
+ FOLLOWER_MOTOR_NAME = "ArmFollowerMotor",
+ ENCODER_NAME = "ArmEncoder";
+ static final TalonFXMotor
+ MASTER_MOTOR = new TalonFXMotor(MASTER_MOTOR_ID, MASTER_MOTOR_NAME),
+ FOLLOWER_MOTOR = new TalonFXMotor(FOLLOWER_MOTOR_ID, FOLLOWER_MOTOR_NAME);
+ private static final CANcoderEncoder ENCODER = new CANcoderEncoder(ENCODER_ID, ENCODER_NAME);
+
+ private static final String NAME = "Arm";
+ private final static double
+ GEAR_RATIO = 40,
+ MAXIMUM_VELOCITY = 20,
+ MAXIMUM_ACCELERATION = 7,
+ MAXIMUM_JERK = 1,
+ ARM_LENGTH_METERS = 1,
+ ARM_MASS_KILOGRAMS = 10,
+ SYS_ID_RAMP_RATE = 1,
+ SYS_ID_STEP_VOLTAGE = 5,
+ VISUALIZATION_OFFSET = 0;
+ private final static Rotation2d
+ MINIMUM_ANGLE = Rotation2d.fromDegrees(0),
+ MAXIMUM_ANGLE = Rotation2d.fromDegrees(360),
+ ANGLE_TOLERANCE = Rotation2d.fromDegrees(3);
+ private final static boolean FOC_ENABLED = true;
+ private final static boolean SHOULD_SIMULATE_GRAVITY = true;
+ private final static DCMotor GEAR_BOX = DCMotor.getKrakenX60Foc(2);
+
+ static ArmSubsystemConfiguration ARM_CONFIG = new ArmSubsystemConfiguration();
+
+ static {
+ configureSubsystem();
+ configureMasterMotor();
+ configureFollowerMotor();
+ configureAngleEncoder();
+ }
+
+ private static void configureSubsystem() {
+ ARM_CONFIG.name = NAME;
+ ARM_CONFIG.gearRatio = GEAR_RATIO;
+ ARM_CONFIG.maximumVelocity = MAXIMUM_VELOCITY;
+ ARM_CONFIG.maximumAcceleration = MAXIMUM_ACCELERATION;
+ ARM_CONFIG.maximumJerk = MAXIMUM_JERK;
+ ARM_CONFIG.lengthMeters = ARM_LENGTH_METERS;
+ ARM_CONFIG.massKilograms = ARM_MASS_KILOGRAMS;
+ ARM_CONFIG.minimumAngle = MINIMUM_ANGLE;
+ ARM_CONFIG.maximumAngle = MAXIMUM_ANGLE;
+ ARM_CONFIG.angleTolerance = ANGLE_TOLERANCE;
+ ARM_CONFIG.shouldSimulateGravity = SHOULD_SIMULATE_GRAVITY;
+ ARM_CONFIG.sysIDRampRate = SYS_ID_RAMP_RATE;
+ ARM_CONFIG.sysIDStepVoltage = SYS_ID_STEP_VOLTAGE;
+ ARM_CONFIG.focEnabled = FOC_ENABLED;
+ ARM_CONFIG.gearbox = GEAR_BOX;
+ ARM_CONFIG.visualizationOffset = VISUALIZATION_OFFSET;
+ }
+
+ private static void configureMasterMotor() {
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.Audio.BeepOnConfig = false;
+ config.Audio.BeepOnBoot = false;
+
+ config.Feedback.SensorToMechanismRatio = GEAR_RATIO;
+ config.Feedback.FeedbackRemoteSensorID = ENCODER.getID();
+ config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
+
+ config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+ config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+ config.Slot0.kP = 10;
+ config.Slot0.kI = 0;
+ config.Slot0.kD = 0;
+ config.Slot0.kS = 0.01529;
+ config.Slot0.kG = 0.78412;
+ config.Slot0.kA = 0;
+ config.Slot0.kV = 4.9117;
+
+ config.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
+ config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign;
+
+ config.MotionMagic.MotionMagicCruiseVelocity = MAXIMUM_VELOCITY;
+ config.MotionMagic.MotionMagicAcceleration = MAXIMUM_ACCELERATION;
+ config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10;
+
+ MASTER_MOTOR.applyConfiguration(config);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100);
+ }
+
+ private static void configureFollowerMotor() {
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.Audio.BeepOnConfig = false;
+ config.Audio.BeepOnBoot = false;
+
+ config.Feedback.SensorToMechanismRatio = GEAR_RATIO;
+ config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+ config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+ FOLLOWER_MOTOR.applyConfiguration(config);
+
+ final Follower FollowerRequest = new Follower(MASTER_MOTOR_ID, false);
+ FOLLOWER_MOTOR.setControl(FollowerRequest);
+ }
+
+ private static void configureAngleEncoder() {
+ final CANcoderConfiguration config = new CANcoderConfiguration();
+
+ config.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive;
+ config.MagnetSensor.MagnetOffset = VISUALIZATION_OFFSET;
+ config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1;
+
+ ENCODER.applyConfiguration(config);
+ ENCODER.setSimulationInputsFromTalonFX(MASTER_MOTOR);
+
+ ENCODER.registerSignal(CANcoderSignal.POSITION, 100);
+ ENCODER.registerSignal(CANcoderSignal.VELOCITY, 100);
+ }
+
+ public enum ArmState implements ArmSubsystem.ArmState {
+ REST(Rotation2d.fromDegrees(0), null, 1),
+ NINETY(Rotation2d.fromDegrees(90), null, 1),
+ ONE_EIGHTY(Rotation2d.fromDegrees(180), null, 1);
+
+ private final Rotation2d targetAngle;
+ private final ArmState prepareState;
+ private final double speedScalar;
+
+ ArmState(Rotation2d targetAngle, ArmState prepareState, double speedScalar) {
+ this.targetAngle = targetAngle;
+ this.prepareState = prepareState;
+ this.speedScalar = speedScalar;
+ }
+
+ @Override
+ public Rotation2d getTargetAngle() {
+ return targetAngle;
+ }
+
+ @Override
+ public ArmSubsystem.ArmState getPrepareState() {
+ return prepareState;
+ }
+
+ @Override
+ public double getSpeedScalar() {
+ return speedScalar;
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
new file mode 100644
index 00000000..10d9f96d
--- /dev/null
+++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
@@ -0,0 +1,12 @@
+package frc.trigon.robot.subsystems.elevator;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor;
+import frc.trigon.lib.subsystems.elevator.ElevatorSubsystem;
+import frc.trigon.lib.subsystems.elevator.ElevatorSubsystemConfiguration;
+
+public class Elevator extends ElevatorSubsystem {
+ public Elevator() {
+ super(ElevatorConstants.MASTER_MOTOR, ElevatorConstants.ELEVATOR_CONFIG, new Pose3d());
+ }
+}
diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java
new file mode 100644
index 00000000..8b603e92
--- /dev/null
+++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java
@@ -0,0 +1,147 @@
+package frc.trigon.robot.subsystems.elevator;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
+import edu.wpi.first.math.system.plant.DCMotor;
+import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor;
+import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal;
+import frc.trigon.lib.subsystems.elevator.ElevatorSubsystem;
+import frc.trigon.lib.subsystems.elevator.ElevatorSubsystemConfiguration;
+
+public class ElevatorConstants {
+ private static final int
+ MASTER_MOTOR_ID = 13,
+ FOLLOWER_MOTOR_ID = 14;
+ private static final String
+ MASTER_MOTOR_NAME = "ElevatorMasterMotor",
+ FOLLOWER_MOTOR_NAME = "ElevatorFollowerMotor";
+ static final TalonFXMotor
+ MASTER_MOTOR = new TalonFXMotor(MASTER_MOTOR_ID, MASTER_MOTOR_NAME),
+ FOLLOWER_MOTOR = new TalonFXMotor(FOLLOWER_MOTOR_ID, FOLLOWER_MOTOR_NAME);
+
+ private static final String NAME = "Elevator";
+ private final static double
+ GEAR_RATIO = 4,
+ MAXIMUM_VELOCITY = 20,
+ MAXIMUM_ACCELERATION = 7,
+ MAXIMUM_JERK = 70,
+ ELEVATOR_MASS_KILOGRAMS = 5,
+ SYS_ID_RAMP_RATE = 0.01,
+ SYS_ID_STEP_VOLTAGE = 1,
+ MINIMUM_HEIGHT_METERS = 0.1,
+ MAXIMUM_HEIGHT_METERS = 1.5,
+ HEIGHT_TOLERANCE_METERS = 0.1,
+ DRUM_RADIUS_METERS = 0.05;
+ private final static boolean FOC_ENABLED = true;
+ private final static boolean SHOULD_SIMULATE_GRAVITY = true;
+ private final static DCMotor GEAR_BOX = DCMotor.getKrakenX60Foc(2);
+
+ static ElevatorSubsystemConfiguration ELEVATOR_CONFIG = new ElevatorSubsystemConfiguration();
+
+ static {
+ configureSubsystem();
+ configureMasterMotor();
+ configureFollowerMotor();
+ }
+
+ private static void configureSubsystem() {
+ ELEVATOR_CONFIG.name = NAME;
+ ELEVATOR_CONFIG.gearRatio = GEAR_RATIO;
+ ELEVATOR_CONFIG.maximumVelocity = MAXIMUM_VELOCITY;
+ ELEVATOR_CONFIG.maximumAcceleration = MAXIMUM_ACCELERATION;
+ ELEVATOR_CONFIG.maximumJerk = MAXIMUM_JERK;
+ ELEVATOR_CONFIG.massKilograms = ELEVATOR_MASS_KILOGRAMS;
+ ELEVATOR_CONFIG.minimumHeight = MINIMUM_HEIGHT_METERS;
+ ELEVATOR_CONFIG.maximumHeight = MAXIMUM_HEIGHT_METERS;
+ ELEVATOR_CONFIG.positionToleranceMeters = HEIGHT_TOLERANCE_METERS;
+ ELEVATOR_CONFIG.shouldSimulateGravity = SHOULD_SIMULATE_GRAVITY;
+ ELEVATOR_CONFIG.sysIDRampRate = SYS_ID_RAMP_RATE;
+ ELEVATOR_CONFIG.sysIDStepVoltage = SYS_ID_STEP_VOLTAGE;
+ ELEVATOR_CONFIG.focEnabled = FOC_ENABLED;
+ ELEVATOR_CONFIG.gearbox = GEAR_BOX;
+ ELEVATOR_CONFIG.drumRadiusMeters = DRUM_RADIUS_METERS;
+ }
+
+ private static void configureMasterMotor() {
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.Audio.BeepOnConfig = false;
+ config.Audio.BeepOnBoot = false;
+
+ config.Feedback.SensorToMechanismRatio = GEAR_RATIO;
+
+ config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+ config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+ config.Slot0.kP = 5;
+ config.Slot0.kI = 0;
+ config.Slot0.kD = 0;
+ config.Slot0.kS = 0.027174;
+ config.Slot0.kG = 3;
+ config.Slot0.kA = 0;
+ config.Slot0.kV = 5;
+
+ config.Slot0.GravityType = GravityTypeValue.Elevator_Static;
+ config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign;
+
+ config.MotionMagic.MotionMagicCruiseVelocity = MAXIMUM_VELOCITY;
+ config.MotionMagic.MotionMagicAcceleration = MAXIMUM_ACCELERATION;
+ config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10;
+
+ MASTER_MOTOR.applyConfiguration(config);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100);
+ }
+
+ private static void configureFollowerMotor() {
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.Audio.BeepOnConfig = false;
+ config.Audio.BeepOnBoot = false;
+
+ config.Feedback.SensorToMechanismRatio = GEAR_RATIO;
+ config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+ config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+ FOLLOWER_MOTOR.applyConfiguration(config);
+
+ final Follower FollowerRequest = new Follower(MASTER_MOTOR_ID, false);
+ FOLLOWER_MOTOR.setControl(FollowerRequest);
+ }
+
+ public enum ElevatorState implements ElevatorSubsystem.ElevatorState {
+ REST(0.2, null, 1),
+ HALF(0.7, null, 1),
+ FULL(1.5, null, 1);
+
+ private final double targetPosition;
+ private final ElevatorState prepareState;
+ private final double speedScalar;
+
+ ElevatorState(double targetPosition, ElevatorState prepareState, double speedScalar) {
+ this.targetPosition = targetPosition;
+ this.prepareState = prepareState;
+ this.speedScalar = speedScalar;
+ }
+
+ @Override
+ public double getTargetPositionMeters() {
+ return 0;
+ }
+
+ @Override
+ public ElevatorSubsystem.ElevatorState getPrepareState() {
+ return prepareState;
+ }
+
+ @Override
+ public double getSpeedScalar() {
+ return speedScalar;
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheel.java b/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheel.java
new file mode 100644
index 00000000..82805553
--- /dev/null
+++ b/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheel.java
@@ -0,0 +1,9 @@
+package frc.trigon.robot.subsystems.flyWheel;
+
+import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystem;
+
+public class FlyWheel extends SimpleMotorSubsystem {
+ public FlyWheel() {
+ super(FlyWheelConstants.MASTER_MOTOR, FlyWheelConstants.FLY_WHEEL_CONFIG);
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheelConstants.java b/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheelConstants.java
new file mode 100644
index 00000000..04ee86b5
--- /dev/null
+++ b/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheelConstants.java
@@ -0,0 +1,126 @@
+package frc.trigon.robot.subsystems.flyWheel;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.system.plant.DCMotor;
+import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor;
+import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal;
+import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystem;
+import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystemConfiguration;
+
+public class FlyWheelConstants {
+ private static final int
+ MASTER_MOTOR_ID = 9,
+ FOLLOWER_MOTOR_ID = 10;
+ private static final String
+ MASTER_MOTOR_NAME = "FlyWheelMasterMotor",
+ FOLLOWER_MOTOR_NAME = "FlyWheelFollowerMotor";
+ static final TalonFXMotor
+ MASTER_MOTOR = new TalonFXMotor(MASTER_MOTOR_ID, MASTER_MOTOR_NAME),
+ FOLLOWER_MOTOR = new TalonFXMotor(FOLLOWER_MOTOR_ID, FOLLOWER_MOTOR_NAME);
+
+ private static final String NAME = "FlyWheel";
+ private final static double
+ GEAR_RATIO = 3,
+ MOMENT_OF_INERTIA = 0.003,
+ MAXIMUM_VELOCITY = 20,
+ MAXIMUM_ACCELERATION = 7,
+ MAXIMUM_JERK = 1,
+ MAXIMUM_DISPLAYABLE_VELOCITY = 1,
+ VELOCITY_TO_TOLERANCE = 1,
+ SYS_ID_RAMP_RATE = 3,
+ SYS_ID_STEP_VOLTAGE = 1;
+ private final static boolean FOC_ENABLED = true;
+ private final static boolean SHOULD_USE_VOLTAGE_CONTROL = false;
+ private final static DCMotor GEAR_BOX = DCMotor.getKrakenX60Foc(2);
+
+ static SimpleMotorSubsystemConfiguration FLY_WHEEL_CONFIG = new SimpleMotorSubsystemConfiguration();
+
+ static {
+ configureSubsystem();
+ configureMasterMotor();
+ configureFollowerMotor();
+ }
+
+ private static void configureSubsystem() {
+ FLY_WHEEL_CONFIG.name = NAME;
+ FLY_WHEEL_CONFIG.gearRatio = GEAR_RATIO;
+ FLY_WHEEL_CONFIG.momentOfInertia = MOMENT_OF_INERTIA;
+ FLY_WHEEL_CONFIG.maximumVelocity = MAXIMUM_VELOCITY;
+ FLY_WHEEL_CONFIG.maximumAcceleration = MAXIMUM_ACCELERATION;
+ FLY_WHEEL_CONFIG.maximumJerk = MAXIMUM_JERK;
+ FLY_WHEEL_CONFIG.maximumDisplayableVelocity = MAXIMUM_DISPLAYABLE_VELOCITY;
+ FLY_WHEEL_CONFIG.velocityTolerance = VELOCITY_TO_TOLERANCE;
+ FLY_WHEEL_CONFIG.sysIDRampRate = SYS_ID_RAMP_RATE;
+ FLY_WHEEL_CONFIG.sysIDStepVoltage = SYS_ID_STEP_VOLTAGE;
+ FLY_WHEEL_CONFIG.focEnabled = FOC_ENABLED;
+ FLY_WHEEL_CONFIG.shouldUseVoltageControl = SHOULD_USE_VOLTAGE_CONTROL;
+ FLY_WHEEL_CONFIG.gearbox = GEAR_BOX;
+ }
+
+ private static void configureMasterMotor() {
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.Audio.BeepOnConfig = false;
+ config.Audio.BeepOnBoot = false;
+
+ config.Feedback.SensorToMechanismRatio = GEAR_RATIO;
+ config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+ config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+ config.Slot0.kP = 0;
+ config.Slot0.kI = 0;
+ config.Slot0.kD = 0;
+ config.Slot0.kS = 0.0069588;
+ config.Slot0.kG = 0;
+ config.Slot0.kA = 0;
+ config.Slot0.kV = 0.37016;
+
+ MASTER_MOTOR.applyConfiguration(config);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE, 100);
+ MASTER_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100);
+ }
+
+ private static void configureFollowerMotor() {
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.Audio.BeepOnConfig = false;
+ config.Audio.BeepOnBoot = false;
+
+ config.Feedback.SensorToMechanismRatio = GEAR_RATIO;
+ config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+ config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+ FOLLOWER_MOTOR.applyConfiguration(config);
+
+ final Follower FollowerRequest = new Follower(MASTER_MOTOR_ID, false);
+ FOLLOWER_MOTOR.setControl(FollowerRequest);
+ }
+
+ public enum FlyWheelState implements SimpleMotorSubsystem.SimpleMotorState {
+ REST(0, null),
+ FORWARD(6, null),
+ BACKWARDS(-6, null);
+
+ private final double targetVelocity;
+ private final FlyWheelState prepareState;
+
+ FlyWheelState(double targetVelocity, FlyWheelState prepareState) {
+ this.targetVelocity = targetVelocity;
+ this.prepareState = prepareState;
+ }
+
+ @Override
+ public double getTargetUnit() {
+ return targetVelocity;
+ }
+
+ @Override
+ public SimpleMotorSubsystem.SimpleMotorState getPrepareState() {
+ return prepareState;
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/main/python/keyboard_to_nt.py b/src/main/python/keyboard_to_nt.py
index f605d1a5..f77ddccc 100644
--- a/src/main/python/keyboard_to_nt.py
+++ b/src/main/python/keyboard_to_nt.py
@@ -1,16 +1,8 @@
import ntcore
-import keyboard
import time
+from pynput import keyboard
def main():
- def on_action(event: keyboard.KeyboardEvent):
- if event.name == "/":
- return
- if event.is_keypad:
- table.putBoolean("numpad"+event.name, event.event_type == keyboard.KEY_DOWN)
- else:
- table.putBoolean(event.name.lower(), event.event_type == keyboard.KEY_DOWN)
-
ntcoreinst = ntcore.NetworkTableInstance.getDefault()
print("Setting up NetworkTables client")
@@ -18,7 +10,6 @@ def on_action(event: keyboard.KeyboardEvent):
ntcoreinst.setServer("127.0.0.1")
ntcoreinst.startDSClient()
- # Wait for connection
print("Waiting for connection to NetworkTables server...")
while not ntcoreinst.isConnected():
time.sleep(0.1)
@@ -26,8 +17,27 @@ def on_action(event: keyboard.KeyboardEvent):
print("Connected!")
table = ntcoreinst.getTable("SmartDashboard/keyboard")
- keyboard.hook(lambda e: on_action(e))
- keyboard.wait()
+ def on_press(key):
+ handle_key(key, True)
+
+ def on_release(key):
+ handle_key(key, False)
+
+ def handle_key(key, is_down):
+ try:
+ # Character keys
+ if key.char == "/":
+ return
+ table.putBoolean(key.char.lower(), is_down)
+ except AttributeError:
+ # Special keys (shift, space, arrows, etc.)
+ table.putBoolean(str(key).replace("Key.", ""), is_down)
+
+ with keyboard.Listener(
+ on_press=on_press,
+ on_release=on_release
+ ) as listener:
+ listener.join()
-if __name__ == '__main__':
- main()
\ No newline at end of file
+if __name__ == "__main__":
+ main()
From babc6163b84b3593593a21a056805f1834d8636e Mon Sep 17 00:00:00 2001
From: noamgreen12
Date: Tue, 13 Jan 2026 01:48:46 +0200
Subject: [PATCH 14/17] Update keyboard_to_nt.py
---
src/main/python/keyboard_to_nt.py | 38 ++++++++++++-------------------
1 file changed, 14 insertions(+), 24 deletions(-)
diff --git a/src/main/python/keyboard_to_nt.py b/src/main/python/keyboard_to_nt.py
index f77ddccc..f605d1a5 100644
--- a/src/main/python/keyboard_to_nt.py
+++ b/src/main/python/keyboard_to_nt.py
@@ -1,8 +1,16 @@
import ntcore
+import keyboard
import time
-from pynput import keyboard
def main():
+ def on_action(event: keyboard.KeyboardEvent):
+ if event.name == "/":
+ return
+ if event.is_keypad:
+ table.putBoolean("numpad"+event.name, event.event_type == keyboard.KEY_DOWN)
+ else:
+ table.putBoolean(event.name.lower(), event.event_type == keyboard.KEY_DOWN)
+
ntcoreinst = ntcore.NetworkTableInstance.getDefault()
print("Setting up NetworkTables client")
@@ -10,6 +18,7 @@ def main():
ntcoreinst.setServer("127.0.0.1")
ntcoreinst.startDSClient()
+ # Wait for connection
print("Waiting for connection to NetworkTables server...")
while not ntcoreinst.isConnected():
time.sleep(0.1)
@@ -17,27 +26,8 @@ def main():
print("Connected!")
table = ntcoreinst.getTable("SmartDashboard/keyboard")
- def on_press(key):
- handle_key(key, True)
-
- def on_release(key):
- handle_key(key, False)
-
- def handle_key(key, is_down):
- try:
- # Character keys
- if key.char == "/":
- return
- table.putBoolean(key.char.lower(), is_down)
- except AttributeError:
- # Special keys (shift, space, arrows, etc.)
- table.putBoolean(str(key).replace("Key.", ""), is_down)
-
- with keyboard.Listener(
- on_press=on_press,
- on_release=on_release
- ) as listener:
- listener.join()
+ keyboard.hook(lambda e: on_action(e))
+ keyboard.wait()
-if __name__ == "__main__":
- main()
+if __name__ == '__main__':
+ main()
\ No newline at end of file
From 96fc178186862e3d4777df13bf8435ec2c40a8c2 Mon Sep 17 00:00:00 2001
From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com>
Date: Tue, 13 Jan 2026 14:28:57 +0200
Subject: [PATCH 15/17] Update lib
---
src/main/java/frc/trigon/lib | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib
index 6b9824d0..56832d0d 160000
--- a/src/main/java/frc/trigon/lib
+++ b/src/main/java/frc/trigon/lib
@@ -1 +1 @@
-Subproject commit 6b9824d047b81a262d6d0bf83412fa779bd61624
+Subproject commit 56832d0d0f90f81245603a4f1c3f9ff98b0f1c3e
From 8984bf359295b2d05e5f01800a95a342ef12992a Mon Sep 17 00:00:00 2001
From: noamgreen12
Date: Tue, 13 Jan 2026 18:21:02 +0200
Subject: [PATCH 16/17] prototype settings
---
.../java/frc/trigon/robot/RobotContainer.java | 29 ++++++-------------
.../robot/constants/OperatorConstants.java | 7 +++--
.../flyWheel/FlyWheelConstants.java | 19 ++++++------
.../robot/subsystems/swerve/Swerve.java | 2 +-
.../subsystems/swerve/SwerveConstants.java | 17 ++++++-----
.../swerve/swervemodule/SwerveModule.java | 6 ++--
.../swervemodule/SwerveModuleConstants.java | 8 ++---
7 files changed, 39 insertions(+), 49 deletions(-)
diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java
index e4681c1e..31115bc5 100644
--- a/src/main/java/frc/trigon/robot/RobotContainer.java
+++ b/src/main/java/frc/trigon/robot/RobotContainer.java
@@ -10,15 +10,9 @@
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.trigon.lib.subsystems.MotorSubsystem;
-import frc.trigon.lib.subsystems.arm.ArmSubsystem;
-import frc.trigon.lib.subsystems.arm.ArmSubsystemCommands;
-import frc.trigon.lib.subsystems.elevator.ElevatorSubsystem;
-import frc.trigon.lib.subsystems.elevator.ElevatorSubsystemCommands;
import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystem;
import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystemCommands;
import frc.trigon.lib.utilities.flippable.Flippable;
-import frc.trigon.robot.commands.CommandConstants;
-import frc.trigon.robot.commands.commandfactories.GeneralCommands;
import frc.trigon.robot.constants.AutonomousConstants;
import frc.trigon.robot.constants.CameraConstants;
import frc.trigon.robot.constants.LEDConstants;
@@ -26,10 +20,6 @@
import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
-import frc.trigon.robot.subsystems.arm.Arm;
-import frc.trigon.robot.subsystems.arm.ArmConstants;
-import frc.trigon.robot.subsystems.elevator.Elevator;
-import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
import frc.trigon.robot.subsystems.flyWheel.FlyWheel;
import frc.trigon.robot.subsystems.flyWheel.FlyWheelConstants;
import frc.trigon.robot.subsystems.swerve.Swerve;
@@ -44,8 +34,8 @@ public class RobotContainer {
CameraConstants.OBJECT_DETECTION_CAMERA
);
public static final Swerve SWERVE = new Swerve();
- public static final ArmSubsystem ARM = new Arm();
- public static final ElevatorSubsystem ELEVATOR = new Elevator();
+ // public static final ArmSubsystem ARM = new Arm();
+ // public static final ElevatorSubsystem ELEVATOR = new Elevator();
public static final SimpleMotorSubsystem FLY_WHEEL = new FlyWheel();
private LoggedDashboardChooser autoChooser;
@@ -68,18 +58,17 @@ private void configureBindings() {
}
private void bindDefaultCommands() {
- SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand());
- ARM.setDefaultCommand(ArmSubsystemCommands.getSetTargetStateCommand(ArmConstants.ArmState.REST, ARM));
- ELEVATOR.setDefaultCommand(ElevatorSubsystemCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST,ELEVATOR));
+ // SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand());
+ // ARM.setDefaultCommand(ArmSubsystemCommands.getSetTargetStateCommand(ArmConstants.ArmState.REST, ARM));
+ // ELEVATOR.setDefaultCommand(ElevatorSubsystemCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST,ELEVATOR));
FLY_WHEEL.setDefaultCommand(SimpleMotorSubsystemCommands.getSetTargetStateCommand(FlyWheelConstants.FlyWheelState.REST, FLY_WHEEL));
}
private void bindControllerCommands() {
- OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND);
- OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND);
- OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand());
- OperatorConstants.OPERATOR_CONTROLLER.a().whileTrue(ElevatorSubsystemCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.HALF, ELEVATOR));
- OperatorConstants.OPERATOR_CONTROLLER.s().whileTrue(ElevatorSubsystemCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.FULL, ELEVATOR));
+// OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND);
+// OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND);
+// OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand());
+ OperatorConstants.INTAKE_TRIGGER.whileTrue(SimpleMotorSubsystemCommands.getSetTargetStateCommand(FlyWheelConstants.FlyWheelState.INTAKE, FLY_WHEEL));
}
private void configureSysIDBindings(MotorSubsystem subsystem) {
diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java
index 93323cc0..a3163955 100644
--- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java
+++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java
@@ -2,13 +2,13 @@
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand;
import frc.trigon.lib.hardware.misc.KeyboardController;
import frc.trigon.lib.hardware.misc.XboxController;
+import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand;
public class OperatorConstants {
public static final double DRIVER_CONTROLLER_DEADBAND = 0.07;
- private static final int DRIVER_CONTROLLER_PORT = 0;
+ private static final int DRIVER_CONTROLLER_PORT = 1;
private static final int
DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT = 1,
DRIVER_CONTROLLER_LEFT_STICK_EXPONENT = 2;
@@ -33,5 +33,6 @@ public class OperatorConstants {
FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(),
BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(),
FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(),
- BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down();
+ BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(),
+ INTAKE_TRIGGER = DRIVER_CONTROLLER.leftTrigger();
}
\ No newline at end of file
diff --git a/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheelConstants.java b/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheelConstants.java
index 04ee86b5..93684d50 100644
--- a/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheelConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/flyWheel/FlyWheelConstants.java
@@ -23,7 +23,7 @@ public class FlyWheelConstants {
private static final String NAME = "FlyWheel";
private final static double
- GEAR_RATIO = 3,
+ GEAR_RATIO = 1,
MOMENT_OF_INERTIA = 0.003,
MAXIMUM_VELOCITY = 20,
MAXIMUM_ACCELERATION = 7,
@@ -33,7 +33,7 @@ public class FlyWheelConstants {
SYS_ID_RAMP_RATE = 3,
SYS_ID_STEP_VOLTAGE = 1;
private final static boolean FOC_ENABLED = true;
- private final static boolean SHOULD_USE_VOLTAGE_CONTROL = false;
+ private final static boolean SHOULD_USE_VOLTAGE_CONTROL = true;
private final static DCMotor GEAR_BOX = DCMotor.getKrakenX60Foc(2);
static SimpleMotorSubsystemConfiguration FLY_WHEEL_CONFIG = new SimpleMotorSubsystemConfiguration();
@@ -72,10 +72,10 @@ private static void configureMasterMotor() {
config.Slot0.kP = 0;
config.Slot0.kI = 0;
config.Slot0.kD = 0;
- config.Slot0.kS = 0.0069588;
+ config.Slot0.kS = 0;
config.Slot0.kG = 0;
config.Slot0.kA = 0;
- config.Slot0.kV = 0.37016;
+ config.Slot0.kV = 0;
MASTER_MOTOR.applyConfiguration(config);
MASTER_MOTOR.registerSignal(TalonFXSignal.POSITION, 100);
@@ -102,20 +102,19 @@ private static void configureFollowerMotor() {
public enum FlyWheelState implements SimpleMotorSubsystem.SimpleMotorState {
REST(0, null),
- FORWARD(6, null),
- BACKWARDS(-6, null);
+ INTAKE(6, null);
- private final double targetVelocity;
+ private final double targetVoltage;
private final FlyWheelState prepareState;
- FlyWheelState(double targetVelocity, FlyWheelState prepareState) {
- this.targetVelocity = targetVelocity;
+ FlyWheelState(double targetVoltage, FlyWheelState prepareState) {
+ this.targetVoltage = targetVoltage;
this.prepareState = prepareState;
}
@Override
public double getTargetUnit() {
- return targetVelocity;
+ return targetVoltage;
}
@Override
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
index fefb875e..eba8fc29 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java
@@ -13,13 +13,13 @@
import frc.trigon.lib.hardware.phoenix6.Phoenix6SignalThread;
import frc.trigon.lib.hardware.phoenix6.pigeon2.Pigeon2Gyro;
import frc.trigon.lib.hardware.phoenix6.pigeon2.Pigeon2Signal;
+import frc.trigon.lib.subsystems.MotorSubsystem;
import frc.trigon.lib.utilities.flippable.Flippable;
import frc.trigon.lib.utilities.flippable.FlippablePose2d;
import frc.trigon.lib.utilities.flippable.FlippableRotation2d;
import frc.trigon.robot.RobotContainer;
import frc.trigon.robot.constants.AutonomousConstants;
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants;
-import frc.trigon.lib.subsystems.MotorSubsystem;
import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule;
import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants;
import org.littletonrobotics.junction.AutoLogOutput;
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java
index 2f45d2b1..3e580e32 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java
@@ -4,6 +4,7 @@
import com.pathplanner.lib.config.PIDConstants;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
@@ -19,7 +20,7 @@
public class SwerveConstants {
private static final int GYRO_ID = 0;
private static final String GYRO_NAME = "SwerveGyro";
- static final Pigeon2Gyro GYRO = new Pigeon2Gyro(GYRO_ID, GYRO_NAME, RobotConstants.CANIVORE_NAME);
+ static final Pigeon2Gyro GYRO = new Pigeon2Gyro(GYRO_ID, GYRO_NAME);
public static final int
FRONT_LEFT_ID = 1,
@@ -27,10 +28,10 @@ public class SwerveConstants {
REAR_LEFT_ID = 3,
REAR_RIGHT_ID = 4;
private static final double
- FRONT_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = 0,
- FRONT_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS = 0,
- REAR_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = 0,
- REAR_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS = 0;
+ FRONT_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = -0.441,
+ FRONT_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS = -0.285,
+ REAR_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = 0.493,
+ REAR_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS = -0.038;
private static final double//TODO:Calibrate
FRONT_LEFT_WHEEL_DIAMETER = 0.05 * 2,
FRONT_RIGHT_WHEEL_DIAMETER = 0.05 * 2,
@@ -102,9 +103,9 @@ public class SwerveConstants {
private static void configureGyro() {
final Pigeon2Configuration config = new Pigeon2Configuration();
//TODO:Calibrate
- config.MountPose.MountPoseYaw = 0;
- config.MountPose.MountPosePitch = 0;
- config.MountPose.MountPoseRoll = 0;
+ config.MountPose.MountPoseYaw = Rotation2d.fromDegrees(-11.623535).getRotations();
+ config.MountPose.MountPosePitch = Rotation2d.fromDegrees(-0.395508).getRotations();
+ config.MountPose.MountPoseRoll = Rotation2d.fromDegrees(-0.043945).getRotations();
GYRO.applyConfiguration(config);
GYRO.setSimulationYawVelocitySupplier(() -> Units.radiansToDegrees(RobotContainer.SWERVE.getRotationalVelocityRadiansPerSecond()));
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java
index e6d83a7d..8915d0bb 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java
@@ -40,9 +40,9 @@ public class SwerveModule {
* @param wheelDiameter the diameter of the wheel
*/
public SwerveModule(int moduleID, double offsetRotations, double wheelDiameter) {
- driveMotor = new TalonFXMotor(moduleID, "Module" + moduleID + "Drive", RobotConstants.CANIVORE_NAME);
- steerMotor = new TalonFXMotor(moduleID + 4, "Module" + moduleID + "Steer", RobotConstants.CANIVORE_NAME);
- steerEncoder = new CANcoderEncoder(moduleID + 4, "Module" + moduleID + "SteerEncoder", RobotConstants.CANIVORE_NAME);
+ driveMotor = new TalonFXMotor(moduleID, "Module" + moduleID + "Drive");
+ steerMotor = new TalonFXMotor(moduleID + 4, "Module" + moduleID + "Steer");
+ steerEncoder = new CANcoderEncoder(moduleID + 4, "Module" + moduleID + "SteerEncoder");
this.wheelDiameter = wheelDiameter;
configureHardware(offsetRotations);
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
index e2f76fc4..1cfd9a90 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
@@ -15,8 +15,8 @@
public class SwerveModuleConstants {
private static final double
- DRIVE_MOTOR_GEAR_RATIO = 7.13,
- REAR_STEER_MOTOR_GEAR_RATIO = 12.8;
+ DRIVE_MOTOR_GEAR_RATIO = 6.03,
+ REAR_STEER_MOTOR_GEAR_RATIO = 26.09;
static final boolean ENABLE_FOC = true;
private static final double
@@ -96,7 +96,7 @@ static TalonFXConfiguration generateSteerMotorConfiguration(int feedbackRemoteSe
config.Audio.BeepOnBoot = false;
config.Audio.BeepOnConfig = true;
- config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+ config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
config.CurrentLimits.StatorCurrentLimit = RobotHardwareStats.isSimulation() ? 200 : 50;
@@ -106,7 +106,7 @@ static TalonFXConfiguration generateSteerMotorConfiguration(int feedbackRemoteSe
config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
config.Feedback.FeedbackRemoteSensorID = feedbackRemoteSensorID;
- config.Slot0.kP = RobotHardwareStats.isSimulation() ? 120 : 40;
+ config.Slot0.kP = RobotHardwareStats.isSimulation() ? 120 : 30;
config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0;
config.ClosedLoopGeneral.ContinuousWrap = true;
From d39b117e19558c533d197980ce2fbde1dd5223a6 Mon Sep 17 00:00:00 2001
From: noamgreen12
Date: Tue, 13 Jan 2026 19:22:08 +0200
Subject: [PATCH 17/17] prototype
---
src/main/java/frc/trigon/lib | 2 +-
src/main/java/frc/trigon/robot/RobotContainer.java | 3 ++-
.../commands/commandfactories/GeneralCommands.java | 2 +-
.../robot/subsystems/swerve/SwerveConstants.java | 2 +-
.../swerve/swervemodule/SwerveModule.java | 14 ++++++--------
.../swerve/swervemodule/SwerveModuleConstants.java | 6 +++---
6 files changed, 14 insertions(+), 15 deletions(-)
diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib
index 56832d0d..6b9824d0 160000
--- a/src/main/java/frc/trigon/lib
+++ b/src/main/java/frc/trigon/lib
@@ -1 +1 @@
-Subproject commit 56832d0d0f90f81245603a4f1c3f9ff98b0f1c3e
+Subproject commit 6b9824d047b81a262d6d0bf83412fa779bd61624
diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java
index 31115bc5..e57e392e 100644
--- a/src/main/java/frc/trigon/robot/RobotContainer.java
+++ b/src/main/java/frc/trigon/robot/RobotContainer.java
@@ -13,6 +13,7 @@
import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystem;
import frc.trigon.lib.subsystems.flywheel.SimpleMotorSubsystemCommands;
import frc.trigon.lib.utilities.flippable.Flippable;
+import frc.trigon.robot.commands.commandfactories.GeneralCommands;
import frc.trigon.robot.constants.AutonomousConstants;
import frc.trigon.robot.constants.CameraConstants;
import frc.trigon.robot.constants.LEDConstants;
@@ -58,7 +59,7 @@ private void configureBindings() {
}
private void bindDefaultCommands() {
- // SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand());
+ SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand());
// ARM.setDefaultCommand(ArmSubsystemCommands.getSetTargetStateCommand(ArmConstants.ArmState.REST, ARM));
// ELEVATOR.setDefaultCommand(ElevatorSubsystemCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST,ELEVATOR));
FLY_WHEEL.setDefaultCommand(SimpleMotorSubsystemCommands.getSetTargetStateCommand(FlyWheelConstants.FlyWheelState.REST, FLY_WHEEL));
diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java
index 2c8d76f1..10e0d465 100644
--- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java
+++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java
@@ -14,7 +14,7 @@
*/
public class GeneralCommands {
public static Command getFieldRelativeDriveCommand() {
- return SwerveCommands.getClosedLoopFieldRelativeDriveCommand(
+ return SwerveCommands.getOpenLoopFieldRelativeDriveCommand(
() -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()),
() -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()),
() -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getRightX())
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java
index 828ee5af..bae85e8c 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java
@@ -30,7 +30,7 @@ public class SwerveConstants {
private static final double
FRONT_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = -0.441,
FRONT_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS = -0.285,
- REAR_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = 0.493,
+ REAR_LEFT_STEER_ENCODER_OFFSET_ROTATIONS = 0.430,
REAR_RIGHT_STEER_ENCODER_OFFSET_ROTATIONS = -0.038;
private static final double//TODO:Calibrate
FRONT_LEFT_WHEEL_DIAMETER = 0.05 * 2,
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java
index 8915d0bb..910e7934 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModule.java
@@ -8,14 +8,13 @@
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.units.Units;
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
-import frc.trigon.robot.constants.RobotConstants;
-import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants;
-import frc.trigon.robot.subsystems.swerve.SwerveConstants;
import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderEncoder;
import frc.trigon.lib.hardware.phoenix6.cancoder.CANcoderSignal;
import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXMotor;
import frc.trigon.lib.hardware.phoenix6.talonfx.TalonFXSignal;
import frc.trigon.lib.utilities.Conversions;
+import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants;
+import frc.trigon.robot.subsystems.swerve.SwerveConstants;
public class SwerveModule {
private final TalonFXMotor
@@ -49,9 +48,7 @@ public SwerveModule(int moduleID, double offsetRotations, double wheelDiameter)
}
public void setTargetState(SwerveModuleState targetState) {
- if (willOptimize(targetState)) {
- targetState.optimize(getCurrentSteerAngle());
- }
+ targetState.optimize(getCurrentSteerAngle());
this.targetState = targetState;
setTargetSteerAngle(targetState.angle);
@@ -132,9 +129,10 @@ public SwerveModulePosition getOdometryPosition(int odometryUpdateIndex) {
);
}
- private boolean willOptimize(SwerveModuleState state) {
+ private boolean
+ willOptimize(SwerveModuleState state) {
final Rotation2d angularDelta = state.angle.minus(getCurrentSteerAngle());
- return Math.abs(angularDelta.getRadians()) > Math.PI / 2;
+ return true;
}
/**
diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
index 88401b0d..d996529d 100644
--- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
+++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java
@@ -66,7 +66,7 @@ static TalonFXConfiguration generateDriveMotorConfiguration() {
config.Audio.BeepOnConfig = false;
config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
- config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+ config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
config.Feedback.SensorToMechanismRatio = DRIVE_MOTOR_GEAR_RATIO;
final double driveMotorSlipCurrent = AutonomousConstants.ROBOT_CONFIG.moduleConfig.driveCurrentLimit;
@@ -96,7 +96,7 @@ static TalonFXConfiguration generateSteerMotorConfiguration(int feedbackRemoteSe
config.Audio.BeepOnBoot = false;
config.Audio.BeepOnConfig = true;
- config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+ config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
config.CurrentLimits.StatorCurrentLimit = RobotHardwareStats.isSimulation() ? 200 : 50;
@@ -106,7 +106,7 @@ static TalonFXConfiguration generateSteerMotorConfiguration(int feedbackRemoteSe
config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
config.Feedback.FeedbackRemoteSensorID = feedbackRemoteSensorID;
- config.Slot0.kP = RobotHardwareStats.isSimulation() ? 120 : 30;
+ config.Slot0.kP = RobotHardwareStats.isSimulation() ? 120 : 40;
config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0;
config.ClosedLoopGeneral.ContinuousWrap = true;