Skip to content

Arm logic #16

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 44 commits into from
Sep 18, 2025
Merged

Arm logic #16

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
44 commits
Select commit Hold shift + click to select a range
26ede8b
changed elevator so arm has access
ShmayaR Sep 14, 2025
b969772
added logic to stop arm from hitting anything
ShmayaR Sep 14, 2025
b93b24d
changed commands to work with logic
ShmayaR Sep 14, 2025
8bcbe19
Update ElevatorConstants.java
ShmayaR Sep 14, 2025
cbf1359
added load coral command which overrides arm logic
ShmayaR Sep 14, 2025
d3546dd
updated poses
ShmayaR Sep 14, 2025
4b60ce6
updated stuff
ShmayaR Sep 14, 2025
f327b94
fixed logic
ShmayaR Sep 15, 2025
bd98958
fixed logic issue and added d to arm pid
ShmayaR Sep 16, 2025
eb64ed0
fixed stuff
ShmayaR Sep 16, 2025
024fb62
made function more readable
ShmayaR Sep 18, 2025
d1c4d2d
Update Elevator.java
ShmayaR Sep 18, 2025
4dd87f5
rename
ShmayaR Sep 18, 2025
379867e
Update Arm.java
ShmayaR Sep 18, 2025
1b9c09f
removed magic number
ShmayaR Sep 18, 2025
4820ccc
moved into more variables
ShmayaR Sep 18, 2025
bbe7ac4
added javadoc
ShmayaR Sep 18, 2025
140137d
switched current limit type to stator
ShmayaR Sep 18, 2025
2b0037f
renamed
ShmayaR Sep 18, 2025
482bd8f
renamed stuff
ShmayaR Sep 18, 2025
1aeb7bb
extracted into multiple meathods
ShmayaR Sep 18, 2025
07b24f6
added command for checking if at an abgle
ShmayaR Sep 18, 2025
94c264b
Update Arm.java
ShmayaR Sep 18, 2025
936826d
Update Arm.java
ShmayaR Sep 18, 2025
9b9fc93
fixed arm logic
ShmayaR Sep 18, 2025
c2591b7
better pid
ShmayaR Sep 18, 2025
8159e68
Update Arm.java
ShmayaR Sep 18, 2025
dc3319b
added gravity offset to the getAngle method
ShmayaR Sep 18, 2025
d748b5e
Update src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
ShmayaR Sep 18, 2025
248ca5c
siwtched set target state commands from execute to functional so that…
ShmayaR Sep 18, 2025
3a8dc54
Merge branch 'arm-logic' of https://github.com/Programming-TRIGON/Rob…
ShmayaR Sep 18, 2025
c0db5e1
made load coral function part of setTargetState
ShmayaR Sep 18, 2025
43757f9
Update Arm.java
ShmayaR Sep 18, 2025
62fcd32
got rid of unnecesary functions
ShmayaR Sep 18, 2025
440dc79
put logic in setTargetAngle into different function
ShmayaR Sep 18, 2025
fae2066
Update Arm.java
ShmayaR Sep 18, 2025
33beb6d
separated setTargetPosition logic into differant logic
ShmayaR Sep 18, 2025
cf68be9
Update ArmConstants.java
ShmayaR Sep 18, 2025
d1c7edc
Update ArmConstants.java
ShmayaR Sep 18, 2025
5bf8194
Update Arm.java
ShmayaR Sep 18, 2025
bd19f72
Update Arm.java
ShmayaR Sep 18, 2025
4ea93e3
cleaned commands
ShmayaR Sep 18, 2025
df0a3f4
Update Elevator.java
ShmayaR Sep 18, 2025
fb1233d
changed names
ShmayaR Sep 18, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
68 changes: 43 additions & 25 deletions src/main/java/frc/trigon/robot/subsystems/arm/Arm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,12 @@

import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import edu.wpi.first.math.MathUtil;
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 frc.trigon.robot.RobotContainer;
import frc.trigon.robot.subsystems.MotorSubsystem;
import lib.hardware.phoenix6.cancoder.CANcoderEncoder;
import lib.hardware.phoenix6.cancoder.CANcoderSignal;
Expand Down Expand Up @@ -81,31 +83,29 @@ public void sysIDDrive(double targetVoltage) {
armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage));
}

public boolean isArmAboveSafeAngle() {
return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees();
}
Comment on lines +86 to +88
Copy link

@coderabbitai coderabbitai bot Sep 18, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue

Fix wrap-around in isArmAboveSafeAngle (Rotation2d.getDegrees is [-180,180)).

Angles >180° read as negative and will fail this check. Normalize before comparing.

-    return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees();
+    final double angleDeg = MathUtil.inputModulus(getAngle().getDegrees(), 0.0, 360.0);
+    return angleDeg >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees();
📝 Committable suggestion

‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.

Suggested change
public boolean isArmAboveSafeAngle() {
return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees();
}
public boolean isArmAboveSafeAngle() {
final double angleDeg = MathUtil.inputModulus(getAngle().getDegrees(), 0.0, 360.0);
return angleDeg >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees();
}
🤖 Prompt for AI Agents 
In src/main/java/frc/trigon/robot/subsystems/arm/Arm.java around lines 86 to 88,
isArmAboveSafeAngle uses Rotation2d.getDegrees() which returns values in
[-180,180) so angles >180° read negative and break the comparison; normalize the
measured angle (and the constant) to a consistent [0,360) range before comparing
(e.g., if deg < 0 add 360), then compare the normalized measured angle against
the normalized MAXIMUM_ARM_SAFE_ANGLE to determine if the arm is above the safe
angle.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

anytime the arm is above 90 degrees itll ignore the logic so it doesnt matter

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ShmayaR Thank you for the clarification about the system behavior above 90°.

I understand that your current system logic bypasses this safety check when the arm is above 90°, which would indeed make the wrap-around issue less critical in practice. However, I'd like to point out a couple of considerations:

  1. Latent bug risk: If the system behavior ever changes or if the arm physically operates in the 180°+ range, angles like 270° would read as -90° and incorrectly return false when they should return true.

  2. Code robustness: Even if the current logic prevents problematic scenarios, defensive programming would make the code more maintainable and less prone to future bugs.

Could you clarify what specifically happens when the arm reaches angles above 180°? Does the system have hard stops, or are there other safety mechanisms that prevent operation in that range?

The fix would be minimal and wouldn't impact performance, but I understand if it's not a priority given your current system constraints.

🐰💭

✏️ Learnings added 
Learnt from: ShmayaR
PR: Programming-TRIGON/RobotCodeOffSeason2025#16
File: src/main/java/frc/trigon/robot/subsystems/arm/Arm.java:86-88
Timestamp: 2025-09-18T22:46:49.248Z
Learning: In the arm subsystem for this robot, angles above 90 degrees bypass the isArmAboveSafeAngle() logic, making the Rotation2d wrap-around issue less critical in practice according to the team's current system design.

Note: Learnings are effective only in the context of similar code segments. To apply general coding guidelines, please configure review instructions. You can manage existing learnings in the app.


public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) {
if (isStateReversed)
return this.targetState == targetState && atTargetAngle(isStateReversed);
return this.targetState == targetState && atAngle(subtractFrom360Degrees(targetState.targetAngle));
return atState(targetState);
}

public boolean atState(ArmConstants.ArmState targetState) {
return this.targetState == targetState && atTargetAngle();
return this.targetState == targetState && atAngle(targetState.targetAngle);
}

public boolean atTargetAngle(boolean isStateReversed) {
public boolean atTargetAngle() {
if (isStateReversed) {
final double currentToTargetStateDifferenceDegrees = Math.abs(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle).minus(getAngle()).getDegrees());
return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees();
return atAngle(subtractFrom360Degrees(targetState.targetAngle));
}
return atTargetAngle();
}

public boolean atTargetAngle() {
final double currentToTargetStateDifferenceDegrees = Math.abs(targetState.targetAngle.minus(getAngle()).getDegrees());
return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees();
return atAngle(targetState.targetAngle);
}

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) {
Expand All @@ -120,7 +120,7 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed)

if (isStateReversed) {
setTargetState(
ArmConstants.FULL_ROTATION.minus(targetState.targetAngle)
subtractFrom360Degrees(targetState.targetAngle)
, targetState.targetEndEffectorVoltage
);
return;
Expand All @@ -132,40 +132,58 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed)

void setTargetState(Rotation2d targetAngle, double targetVoltage) {
setTargetAngle(targetAngle);
setTargetVoltage(targetVoltage);
setEndEffectorTargetVoltage(targetVoltage);
}

void prepareForState(ArmConstants.ArmState targetState) {
prepareForState(targetState, false);
void setArmState(ArmConstants.ArmState targetState) {
setArmState(targetState, false);
}

void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) {
void setArmState(ArmConstants.ArmState targetState, boolean isStateReversed) {
this.isStateReversed = isStateReversed;
this.targetState = targetState;

if (isStateReversed) {
setTargetAngle(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle));
setTargetAngle(subtractFrom360Degrees(targetState.targetAngle));
return;
}
setTargetAngle(targetState.targetAngle);
}

private Rotation2d getAngle() {
return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION));
void setEndEffectorState(ArmConstants.ArmState targetState) {
setEndEffectorTargetVoltage(targetState.targetEndEffectorVoltage);
}

private void setTargetAngle(Rotation2d targetAngle) {
armMasterMotor.setControl(positionRequest.withPosition(targetAngle.getRotations()));
armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations())));
}

private Rotation2d calculateMinimumArmSafeAngle() {
final boolean isElevatorAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone();
final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromMinimumSafeZone();
final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1);
return isElevatorAboveSafeZone
? Rotation2d.fromRadians(0)
: Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle));
}

private void setTargetVoltage(double targetVoltage) {
private void setEndEffectorTargetVoltage(double targetVoltage) {
ArmConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage);
endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage));
}

private boolean atAngle(Rotation2d targetAngle) {
final double currentToTargetAngleDifferenceDegrees = Math.abs(targetAngle.minus(getAngle()).getDegrees());
return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees();
}

private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) {
return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - angleToSubtract.getDegrees());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This doesn't change anything, it just overcomplicates it for no reason. Just revert it to the way it was before. I don't care enough about this.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ezra does

}

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);
Expand Down
35 changes: 15 additions & 20 deletions src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,9 @@

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.StartEndCommand;
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
import frc.trigon.robot.RobotContainer;
import lib.commands.ExecuteEndCommand;
import lib.commands.GearRatioCalculationCommand;
import lib.commands.NetworkTablesCommand;

Expand Down Expand Up @@ -32,33 +33,27 @@ public static Command getGearRatioCalulationCommand() {
);
}

public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) {
return new StartEndCommand(
() -> RobotContainer.ARM.setTargetState(targetState, isStateReversed),
RobotContainer.ARM::stop,
RobotContainer.ARM
);
}

public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) {
return new StartEndCommand(
() -> RobotContainer.ARM.setTargetState(targetState),
RobotContainer.ARM::stop,
RobotContainer.ARM
);
return getSetTargetStateCommand(targetState, false);
}

public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) {
return new StartEndCommand(
() -> RobotContainer.ARM.prepareForState(targetState, isStateReversed),
RobotContainer.ARM::stop,
public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) {
return new FunctionalCommand(
() -> RobotContainer.ARM.setEndEffectorState(targetState),
() -> RobotContainer.ARM.setArmState(targetState, isStateReversed),
interrupted -> RobotContainer.ARM.stop(),
() -> false,
RobotContainer.ARM
);
}

public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState) {
return new StartEndCommand(
() -> RobotContainer.ARM.prepareForState(targetState),
return getPrepareForStateCommand(targetState, false);
}

public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) {
return new ExecuteEndCommand(
() -> RobotContainer.ARM.setArmState(targetState, isStateReversed),
RobotContainer.ARM::stop,
RobotContainer.ARM
);
Expand Down
41 changes: 23 additions & 18 deletions src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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)
);

Expand All @@ -110,17 +110,19 @@ 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)
);

/**
* The highest point of the arms angular zone where the safety logic applies.
*/
static final Rotation2d MAXIMUM_ARM_SAFE_ANGLE = Rotation2d.fromDegrees(90);
static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0);
private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2;
static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent(
CommandScheduler.getInstance().getActiveButtonLoop(),
DISTANCE_SENSOR::getBinaryValue
).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS);
static final Rotation2d FULL_ROTATION = Rotation2d.fromDegrees(360);

static {
configureArmMasterMotor();
Expand All @@ -143,13 +145,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() ? 38 : 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;
Expand All @@ -158,7 +160,8 @@ private static void configureArmMasterMotor() {
config.MotionMagic.MotionMagicAcceleration = ARM_DEFAULT_MAXIMUM_ACCELERATION;
config.MotionMagic.MotionMagicJerk = config.MotionMagic.MotionMagicAcceleration * 10;

config.CurrentLimits.SupplyCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;
config.CurrentLimits.StatorCurrentLimitEnable = true;
config.CurrentLimits.StatorCurrentLimit = ARM_MOTOR_CURRENT_LIMIT;

ARM_MASTER_MOTOR.applyConfiguration(config);
ARM_MASTER_MOTOR.setPhysicsSimulation(ARM_SIMULATION);
Expand All @@ -179,7 +182,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);

Expand All @@ -196,7 +200,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);
Expand Down Expand Up @@ -234,7 +239,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),
Expand Down
38 changes: 31 additions & 7 deletions src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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() {
Expand Down Expand Up @@ -75,14 +77,40 @@ 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;
}

public double getElevatorHeightFromMinimumSafeZone() {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

MinimumSafeZone is not a place that you can get the height from

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why not? its a constant

return getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
}

void setTargetState(ElevatorConstants.ElevatorState targetState) {
this.targetState = targetState;
scalePositionRequestSpeed(targetState.speedScalar);
if (targetState == ElevatorConstants.ElevatorState.LOAD_CORAL) {
masterMotor.setControl(positionRequest.withPosition(metersToRotations(targetState.targetPositionMeters)));
return;
}
setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters));
}
Comment on lines 92 to 100
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue

🧩 Analysis chain

One more guard: prevent future bypasses of the clamp

Search for any other direct masterMotor.setControl(positionRequest.withPosition(...)) writes in Elevator to ensure all paths route through setTargetPositionRotations().

Also applies to: 102-114

🏁 Script executed:

#!/bin/bash
rg -nP --type=java -C2 'setControl\s*\(\s*positionRequest\.withPosition\(' src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java

Length of output: 535

Prevent clamp bypass in LOAD_CORAL branch

src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java — the LOAD_CORAL branch in setTargetState directly calls masterMotor.setControl(positionRequest.withPosition(...)) (line 96), bypassing the clamp in setTargetPositionRotations (line 103). Replace that direct call with setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)) or apply the same Math.max clamp before calling masterMotor.setControl.

🤖 Prompt for AI Agents 
In src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java around lines
92 to 100, the LOAD_CORAL branch in setTargetState directly invokes
masterMotor.setControl(positionRequest.withPosition(...)) which bypasses the
clamping logic inside setTargetPositionRotations; replace the direct masterMotor
call with a call to
setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters))
so the same Math.max clamp is applied (or compute the rotations, apply the same
clamp used in setTargetPositionRotations, then call masterMotor.setControl with
the clamped value).


void setTargetPositionRotations(double targetPositionRotations) {
masterMotor.setControl(positionRequest.withPosition(targetPositionRotations));
masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations())));
}

private double calculateMinimumSafeElevatorHeightRotations() {
final double armCos = RobotContainer.ARM.getAngle().getRadians();
final double elevatorHeightFromArm = Math.cos(armCos) * ArmConstants.ARM_LENGTH_METERS;
final double minimumElevatorSafeZone = ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS;
final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeAngle()
? 0 : elevatorHeightFromArm)
+ minimumElevatorSafeZone;
return metersToRotations(minimumSafeHeightMeters);
}

private Pose3d getFirstStageComponentPose() {
Expand Down Expand Up @@ -117,10 +145,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);
}
Expand Down
Loading
Loading