Programming-TRIGON · Nummun14 · Sep 18, 2025 · Sep 14, 2025 · Sep 14, 2025 · Sep 14, 2025
diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
@@ -6,7 +6,9 @@
 import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.subsystems.MotorSubsystem;
+import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
 import lib.hardware.phoenix6.cancoder.CANcoderEncoder;
 import lib.hardware.phoenix6.cancoder.CANcoderSignal;
 import lib.hardware.phoenix6.talonfx.TalonFXMotor;
@@ -81,6 +83,10 @@ public void sysIDDrive(double targetVoltage) {
         armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage));
     }
 
+    public boolean isArmAboveSafeZone() {
+        return getAngle().getDegrees() >= 90;
+    }
+
     public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) {
         if (isStateReversed)
             return this.targetState == targetState && atTargetAngle(isStateReversed);
@@ -93,7 +99,7 @@ public boolean atState(ArmConstants.ArmState targetState) {
 
     public boolean atTargetAngle(boolean isStateReversed) {
         if (isStateReversed) {
-            final double currentToTargetStateDifferenceDegrees = Math.abs(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle).minus(getAngle()).getDegrees());
+            final double currentToTargetStateDifferenceDegrees = Math.abs(subtractFrom360(targetState.targetAngle).minus(getAngle()).getDegrees());
             return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees();
         }
         return atTargetAngle();
@@ -104,8 +110,8 @@ public boolean atTargetAngle() {
         return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees();
     }
 
-    public boolean hasGamePiece() {
-        return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean();
+    public Rotation2d getAngle() {
+        return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION));
     }
 
     void setTargetState(ArmConstants.ArmState targetState) {
@@ -120,7 +126,7 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed)
 
         if (isStateReversed) {
             setTargetState(
-                    ArmConstants.FULL_ROTATION.minus(targetState.targetAngle)
+                    subtractFrom360(targetState.targetAngle)
                     , targetState.targetEndEffectorVoltage
             );
             return;
@@ -142,30 +148,33 @@ void prepareForState(ArmConstants.ArmState targetState) {
     void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) {
         this.isStateReversed = isStateReversed;
         this.targetState = targetState;
-        
+
         if (isStateReversed) {
-            setTargetAngle(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle));
+            setTargetAngle(subtractFrom360(targetState.targetAngle));
             return;
         }
         setTargetAngle(targetState.targetAngle);
     }
 
-    private Rotation2d getAngle() {
-        return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION));
-    }
-
     private void setTargetAngle(Rotation2d targetAngle) {
-        armMasterMotor.setControl(positionRequest.withPosition(targetAngle.getRotations()));
+        final double heightFromSafeZone = RobotContainer.ELEVATOR.getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
+        final double cosMinimumAngle = heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS;
+        final Rotation2d minimumSafeAngle = Rotation2d.fromRadians(RobotContainer.ELEVATOR.isElevatorAboveSafeZone() ? 0 : Math.acos(cosMinimumAngle));
+        armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minimumSafeAngle.getRotations())));
     }
 
     private void setTargetVoltage(double targetVoltage) {
         ArmConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage);
         endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage));
     }
 
+    public static Rotation2d subtractFrom360(Rotation2d angleToSubtract) {
+        return Rotation2d.fromDegrees(360 - angleToSubtract.getDegrees());
+    }
+
     private Pose3d calculateVisualizationPose() {
         final Transform3d armTransform = new Transform3d(
-                new Translation3d(),
+                new Translation3d(0, 0, RobotContainer.ELEVATOR.getPositionMeters()),
                 new Rotation3d(0, getAngle().getRadians(), 0)
         );
         return ArmConstants.ARM_VISUALIZATION_ORIGIN_POINT.transformBy(armTransform);

diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java
@@ -4,6 +4,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import frc.trigon.robot.RobotContainer;
+import lib.commands.ExecuteEndCommand;
 import lib.commands.GearRatioCalculationCommand;
 import lib.commands.NetworkTablesCommand;
 
@@ -33,31 +34,31 @@ public static Command getGearRatioCalulationCommand() {
     }
 
     public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) {
-        return new StartEndCommand(
+        return new ExecuteEndCommand(
                 () -> RobotContainer.ARM.setTargetState(targetState, isStateReversed),
                 RobotContainer.ARM::stop,
                 RobotContainer.ARM
         );
     }
 
     public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) {
-        return new StartEndCommand(
+        return new ExecuteEndCommand(
                 () -> RobotContainer.ARM.setTargetState(targetState),
                 RobotContainer.ARM::stop,
                 RobotContainer.ARM
         );
     }
 
     public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) {
-        return new StartEndCommand(
+        return new ExecuteEndCommand(
                 () -> RobotContainer.ARM.prepareForState(targetState, isStateReversed),
                 RobotContainer.ARM::stop,
                 RobotContainer.ARM
         );
     }
 
     public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState) {
-        return new StartEndCommand(
+        return new ExecuteEndCommand(
                 () -> RobotContainer.ARM.prepareForState(targetState),
                 RobotContainer.ARM::stop,
                 RobotContainer.ARM

diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java
@@ -50,26 +50,26 @@ public class ArmConstants {
     static final SimpleSensor DISTANCE_SENSOR = SimpleSensor.createDigitalSensor(DISTANCE_SENSOR_CHANNEL, DISTANCE_SENSOR_NAME);
 
     private static final double
-            ARM_GEAR_RATIO = 50,
+            ARM_GEAR_RATIO = 40,
             END_EFFECTOR_GEAR_RATIO = 17;
     private static final double ARM_MOTOR_CURRENT_LIMIT = 50;
     private static final double ANGLE_ENCODER_GRAVITY_OFFSET = 0;
     static final double POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 - Conversions.degreesToRotations(90) : 0 + Conversions.degreesToRotations(0) - ANGLE_ENCODER_GRAVITY_OFFSET;
     private static final boolean SHOULD_ARM_FOLLOWER_OPPOSE_MASTER = false;
     static final double
-            ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 0.6 : 0,
-            ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 1.5 : 0,
+            ARM_DEFAULT_MAXIMUM_VELOCITY = RobotHardwareStats.isSimulation() ? 2.4614 : 0,
+            ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 67.2344 : 0,
             ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10;
     static final boolean FOC_ENABLED = true;
 
+    public static final double ARM_LENGTH_METERS = 0.52;
     private static final int
             ARM_MOTOR_AMOUNT = 2,
             END_EFFECTOR_MOTOR_AMOUNT = 1;
     private static final DCMotor
             ARM_GEARBOX = DCMotor.getKrakenX60Foc(ARM_MOTOR_AMOUNT),
             END_EFFECTOR_GEARBOX = DCMotor.getKrakenX60Foc(END_EFFECTOR_MOTOR_AMOUNT);
     private static final double
-            ARM_LENGTH_METERS = 0.67,
             ARM_MASS_KILOGRAMS = 3.5,
             END_EFFECTOR_MOMENT_OF_INERTIA = 0.003,
             END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY = 12;
@@ -95,7 +95,7 @@ public class ArmConstants {
 
     static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config(
             Units.Volts.of(1.5).per(Units.Seconds),
-            Units.Volts.of(1.5),
+            Units.Volts.of(3),
             Units.Second.of(1000)
     );
 
@@ -110,7 +110,7 @@ public class ArmConstants {
     );
 
     static final Pose3d ARM_VISUALIZATION_ORIGIN_POINT = new Pose3d(
-            new Translation3d(0, 0.0954, 0.9517),
+            new Translation3d(0, -0.0954, 0.3573),
             new Rotation3d(0, 0, 0)
     );
 
@@ -120,7 +120,6 @@ public class ArmConstants {
             CommandScheduler.getInstance().getActiveButtonLoop(),
             DISTANCE_SENSOR::getBinaryValue
     ).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS);
-    static final Rotation2d FULL_ROTATION = Rotation2d.fromDegrees(360);
 
     static {
         configureArmMasterMotor();
@@ -143,13 +142,13 @@ private static void configureArmMasterMotor() {
         config.Feedback.FeedbackRemoteSensorID = ARM_MASTER_MOTOR.getID();
         config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder;
 
-        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 50 : 0;
+        config.Slot0.kP = RobotHardwareStats.isSimulation() ? 100 : 0;
         config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0;
-        config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0;
-        config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.0067258 : 0;
-        config.Slot0.kV = RobotHardwareStats.isSimulation() ? 6.2 : 0;
-        config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.063357 : 0;
-        config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.15048 : 0;
+        config.Slot0.kD = RobotHardwareStats.isSimulation() ? 1 : 0;
+        config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0;
+        config.Slot0.kV = RobotHardwareStats.isSimulation() ? 4.8752 : 0;
+        config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.17848 : 0;
+        config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.1117 : 0;
 
         config.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
         config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign;
@@ -179,7 +178,8 @@ private static void configureArmFollowerMotor() {
         config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
         config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
-        config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;
+        config.CurrentLimits.StatorCurrentLimitEnable = true;
+        config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;
 
         ARM_FOLLOWER_MOTOR.applyConfiguration(config);
 
@@ -196,7 +196,8 @@ private static void configureEndEffectorMotor() {
         config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
         config.Feedback.RotorToSensorRatio = END_EFFECTOR_GEAR_RATIO;
 
-        config.CurrentLimits.SupplyCurrentLimit = 80;
+        config.CurrentLimits.StatorCurrentLimitEnable = true;
+        config.CurrentLimits.StatorCurrentLimit = 80;
 
         END_EFFECTOR_MOTOR.applyConfiguration(config);
         END_EFFECTOR_MOTOR.setPhysicsSimulation(END_EFFECTOR_SIMULATION);
@@ -234,7 +235,7 @@ public enum ArmState {
         PREPARE_SCORE_L1(Rotation2d.fromDegrees(80), 0),
         SCORE_L1(Rotation2d.fromDegrees(75), 4),
         PREPARE_SCORE_L2(Rotation2d.fromDegrees(95), 0),
-        SCORE_L2(Rotation2d.fromDegrees(90), 4),
+        SCORE_L2(Rotation2d.fromDegrees(180), 4),
         PREPARE_SCORE_L3(Rotation2d.fromDegrees(95), 0),
         SCORE_L3(Rotation2d.fromDegrees(90), 4),
         PREPARE_SCORE_L4(Rotation2d.fromDegrees(95), 0),

diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
@@ -9,11 +9,13 @@
 import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.subsystems.MotorSubsystem;
-import org.littletonrobotics.junction.Logger;
+import frc.trigon.robot.subsystems.arm.ArmConstants;
 import lib.hardware.phoenix6.talonfx.TalonFXMotor;
 import lib.hardware.phoenix6.talonfx.TalonFXSignal;
 import lib.utilities.Conversions;
+import org.littletonrobotics.junction.Logger;
 
 public class Elevator extends MotorSubsystem {
     private final TalonFXMotor masterMotor = ElevatorConstants.MASTER_MOTOR;
@@ -23,7 +25,7 @@ public class Elevator extends MotorSubsystem {
             ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY,
             ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION,
             ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * 10
-            ).withEnableFOC(ElevatorConstants.FOC_ENABLED);
+    ).withEnableFOC(ElevatorConstants.FOC_ENABLED);
     private ElevatorConstants.ElevatorState targetState = ElevatorConstants.ElevatorState.REST;
 
     public Elevator() {
@@ -75,14 +77,31 @@ public void updatePeriodically() {
         Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters());
     }
 
+    public double getPositionMeters() {
+        return rotationsToMeters(getPositionRotations());
+    }
+
+    public boolean isElevatorAboveSafeZone() {
+        return getPositionMeters() >= ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS;
+    }
+
+    void prepareLoadCoral() {
+        this.targetState = ElevatorConstants.ElevatorState.LOAD_CORAL;
+        scalePositionRequestSpeed(targetState.speedScalar);
+        masterMotor.setControl(positionRequest.withPosition(metersToRotations(ElevatorConstants.ElevatorState.LOAD_CORAL.targetPositionMeters)));
+    }
+
     void setTargetState(ElevatorConstants.ElevatorState targetState) {
         this.targetState = targetState;
         scalePositionRequestSpeed(targetState.speedScalar);
         setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters));
     }
 
     void setTargetPositionRotations(double targetPositionRotations) {
-        masterMotor.setControl(positionRequest.withPosition(targetPositionRotations));
+        final double elevatorHeightFromArm = Math.cos(RobotContainer.ARM.getAngle().getRadians()) * ArmConstants.ARM_LENGTH_METERS;
+        final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeZone() ? 0 : elevatorHeightFromArm) + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
+        final double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters);
+        masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations)));
     }
 
     private Pose3d getFirstStageComponentPose() {
@@ -117,10 +136,6 @@ private void scalePositionRequestSpeed(double speedScalar) {
         positionRequest.Jerk = positionRequest.Acceleration * 10;
     }
 
-    private double getPositionMeters() {
-        return rotationsToMeters(getPositionRotations());
-    }
-
     private double rotationsToMeters(double positionsRotations) {
         return Conversions.rotationsToDistance(positionsRotations, ElevatorConstants.DRUM_DIAMETER_METERS);
     }

diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java
@@ -3,6 +3,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.StartEndCommand;
 import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.subsystems.arm.ArmConstants;
+import lib.commands.ExecuteEndCommand;
 import lib.commands.NetworkTablesCommand;
 
 import java.util.Set;
@@ -17,16 +19,24 @@ public static Command getDebbugingCommand() {
         );
     }
 
-    public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState targetState) {
+    public static Command getPrepareLoadCoral() {
         return new StartEndCommand(
+                RobotContainer.ELEVATOR::prepareLoadCoral,
+                RobotContainer.ELEVATOR::stop,
+                RobotContainer.ELEVATOR
+        ).onlyIf(() -> RobotContainer.ARM.atState(ArmConstants.ArmState.REST));
+    }
+
+    public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState targetState) {
+        return new ExecuteEndCommand(
                 () -> RobotContainer.ELEVATOR.setTargetState(targetState),
                 RobotContainer.ELEVATOR::stop,
                 RobotContainer.ELEVATOR
         );
     }
-    
+
     public static Command getSetTargetStateCommand(double targetPositionRotations) {
-        return new StartEndCommand(
+        return new ExecuteEndCommand(
                 () -> RobotContainer.ELEVATOR.setTargetPositionRotations(targetPositionRotations),
                 RobotContainer.ELEVATOR::stop,
                 RobotContainer.ELEVATOR