From 26ede8b211998dc886de2b9e04e55772f6cb01f3 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 14 Sep 2025 21:43:42 +0300 Subject: [PATCH 01/43] changed elevator so arm has access --- .../java/frc/trigon/robot/subsystems/arm/Arm.java | 9 +++------ .../trigon/robot/subsystems/elevator/Elevator.java | 12 ++++++------ 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 1b14eb1a..48b2fed9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -6,6 +6,7 @@ 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; @@ -104,10 +105,6 @@ public boolean atTargetAngle() { return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); } - public boolean hasGamePiece() { - return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); - } - void setTargetState(ArmConstants.ArmState targetState) { this.isStateReversed = false; this.targetState = targetState; @@ -142,7 +139,7 @@ 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)); return; @@ -165,7 +162,7 @@ private void setTargetVoltage(double targetVoltage) { 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/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 7063a1d5..c18ea213 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -10,10 +10,10 @@ import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.subsystems.MotorSubsystem; -import org.littletonrobotics.junction.Logger; 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 +23,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,6 +75,10 @@ public void updatePeriodically() { Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters()); } + public double getPositionMeters() { + return rotationsToMeters(getPositionRotations()); + } + void setTargetState(ElevatorConstants.ElevatorState targetState) { this.targetState = targetState; scalePositionRequestSpeed(targetState.speedScalar); @@ -117,10 +121,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); } From b96977236e69bd406f2fb9056cd0d09b3737ffec Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 14 Sep 2025 22:05:31 +0300 Subject: [PATCH 02/43] added logic to stop arm from hitting anything --- .../frc/trigon/robot/subsystems/arm/Arm.java | 12 +++++----- .../robot/subsystems/arm/ArmConstants.java | 2 +- .../robot/subsystems/elevator/Elevator.java | 6 ++++- .../elevator/ElevatorConstants.java | 22 +++++++++++-------- 4 files changed, 26 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 48b2fed9..4e7e6704 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -8,6 +8,7 @@ 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; @@ -105,6 +106,10 @@ public boolean atTargetAngle() { return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); } + public Rotation2d getAngle() { + return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); + } + void setTargetState(ArmConstants.ArmState targetState) { this.isStateReversed = false; this.targetState = targetState; @@ -147,12 +152,9 @@ void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) setTargetAngle(targetState.targetAngle); } - private Rotation2d getAngle() { - return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); - } - private void setTargetAngle(Rotation2d targetAngle) { - armMasterMotor.setControl(positionRequest.withPosition(targetAngle.getRotations())); + Rotation2d minimumSafeAngle = Rotation2d.fromDegrees(Math.acos((RobotContainer.ELEVATOR.getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS) / ArmConstants.ARM_LENGTH_METERS)); + armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minimumSafeAngle.getRotations()))); } private void setTargetVoltage(double targetVoltage) { diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 0bdb08ef..46951e19 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -62,6 +62,7 @@ public class ArmConstants { ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10; static final boolean FOC_ENABLED = true; + public static final double ARM_LENGTH_METERS = 0.67; private static final int ARM_MOTOR_AMOUNT = 2, END_EFFECTOR_MOTOR_AMOUNT = 1; @@ -69,7 +70,6 @@ public class ArmConstants { 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; diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index c18ea213..3ef07c5d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -9,7 +9,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.arm.ArmConstants; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; import lib.utilities.Conversions; @@ -86,7 +88,9 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) { } void setTargetPositionRotations(double targetPositionRotations) { - masterMotor.setControl(positionRequest.withPosition(targetPositionRotations)); + double minimumSafeHeightMeters = Math.cos(RobotContainer.ARM.getAngle().getDegrees()) * ArmConstants.ARM_LENGTH_METERS + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters); + masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations))); } private Pose3d getFirstStageComponentPose() { diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index b56f6d03..6cca2017 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -2,7 +2,10 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.signals.*; +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.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; @@ -71,7 +74,7 @@ public class ElevatorConstants { new Rotation3d(0, 0, 0) ); static final Pose3d ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, -0.17 ,0.0814), + new Translation3d(0, -0.17, 0.0814), new Rotation3d(0, 0, 0) ); static final ElevatorMechanism2d MECHANISM = new ElevatorMechanism2d( @@ -88,6 +91,7 @@ public class ElevatorConstants { REVERSE_LIMIT_SENSOR::getBinaryValue ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); + public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.1; private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; static { @@ -106,13 +110,13 @@ private static void configureMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5:0; - config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4:0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165:0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766:0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239:0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202:0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 0; + config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766 : 0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239 : 0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202 : 0; config.Slot0.GravityType = GravityTypeValue.Elevator_Static; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; From b93b24d79eecda836dd257a98a2f3268d33aaaba Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 14 Sep 2025 22:08:16 +0300 Subject: [PATCH 03/43] changed commands to work with logic --- .../frc/trigon/robot/subsystems/arm/ArmCommands.java | 9 +++++---- .../robot/subsystems/elevator/ElevatorCommands.java | 5 +++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index c7bcd58c..94d3f8e6 100644 --- 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,7 +34,7 @@ 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 @@ -41,7 +42,7 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState } public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { - return new StartEndCommand( + return new ExecuteEndCommand( () -> RobotContainer.ARM.setTargetState(targetState), RobotContainer.ARM::stop, RobotContainer.ARM @@ -49,7 +50,7 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState } 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 @@ -57,7 +58,7 @@ public static Command getPrepareForStateCommand(ArmConstants.ArmState targetStat } 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/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index aba71757..8efd74c4 100644 --- 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,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.NetworkTablesCommand; import java.util.Set; @@ -18,7 +19,7 @@ public static Command getDebbugingCommand() { } public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState targetState) { - return new StartEndCommand( + return new ExecuteEndCommand( () -> RobotContainer.ELEVATOR.setTargetState(targetState), RobotContainer.ELEVATOR::stop, RobotContainer.ELEVATOR @@ -26,7 +27,7 @@ public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState t } public static Command getSetTargetStateCommand(double targetPositionRotations) { - return new StartEndCommand( + return new ExecuteEndCommand( () -> RobotContainer.ELEVATOR.setTargetPositionRotations(targetPositionRotations), RobotContainer.ELEVATOR::stop, RobotContainer.ELEVATOR From 8bcbe192683ab6179d1c8fa2e4dd0d34e99b0982 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 14 Sep 2025 22:17:17 +0300 Subject: [PATCH 04/43] Update ElevatorConstants.java --- .../frc/trigon/robot/subsystems/elevator/ElevatorConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 6cca2017..0310fa3f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -91,7 +91,7 @@ public class ElevatorConstants { REVERSE_LIMIT_SENSOR::getBinaryValue ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); - public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.1; + public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.3262; private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; static { From cbf1359b86b6617d5b113413942bdcf32fef612c Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 14 Sep 2025 22:30:59 +0300 Subject: [PATCH 05/43] added load coral command which overrides arm logic --- .../trigon/robot/subsystems/elevator/Elevator.java | 6 ++++++ .../robot/subsystems/elevator/ElevatorCommands.java | 11 ++++++++++- .../robot/subsystems/elevator/ElevatorConstants.java | 1 + 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 3ef07c5d..73b6735c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -81,6 +81,12 @@ public double getPositionMeters() { return rotationsToMeters(getPositionRotations()); } + void loadCoralFromElevator() { + this.targetState = ElevatorConstants.ElevatorState.LOAD_CORAL; + scalePositionRequestSpeed(targetState.speedScalar); + masterMotor.setControl(positionRequest.withPosition(ElevatorConstants.ElevatorState.LOAD_CORAL.targetPositionMeters)); + } + void setTargetState(ElevatorConstants.ElevatorState targetState) { this.targetState = targetState; scalePositionRequestSpeed(targetState.speedScalar); diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index 8efd74c4..cdb60be5 100644 --- 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,7 @@ 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; @@ -18,6 +19,14 @@ public static Command getDebbugingCommand() { ); } + public static Command getLoadCoralFromElevatorCommand() { + return new StartEndCommand( + RobotContainer.ELEVATOR::loadCoralFromElevator, + 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), @@ -25,7 +34,7 @@ public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState t RobotContainer.ELEVATOR ); } - + public static Command getSetTargetStateCommand(double targetPositionRotations) { return new ExecuteEndCommand( () -> RobotContainer.ELEVATOR.setTargetPositionRotations(targetPositionRotations), diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 0310fa3f..b78043b0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -163,6 +163,7 @@ private static void configureReverseLimitSensor() { public enum ElevatorState { REST(0.603, 0.7), + LOAD_CORAL(0.5519, 0.7), SCORE_L1(0.603, 1), SCORE_L2(0.603, 1), SCORE_L3(1.003, 1), From d3546ddef08e56a34a75f374fdc637e04e041639 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 14 Sep 2025 22:32:34 +0300 Subject: [PATCH 06/43] updated poses --- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 46951e19..c7574be8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -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) ); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 560d9c9f..eb47449c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -93,7 +93,7 @@ public class IntakeConstants { ); static final Pose3d INTAKE_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, 0.29449, 0.32349), + new Translation3d(0.3234, 0, 0.2944), new Rotation3d(0, MINIMUM_ANGLE.getRadians(), 0) ); private static final double MAXIMUM_DISPLAYABLE_VELOCITY = 12; From 4b60ce663ac65375d42a1eef3c91051da0c7f7b5 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 14 Sep 2025 23:28:39 +0300 Subject: [PATCH 07/43] updated stuff --- .../trigon/robot/subsystems/arm/ArmConstants.java | 14 ++++++++------ .../subsystems/elevator/ElevatorConstants.java | 6 +++--- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index c7574be8..5aa5c6d9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -50,19 +50,19 @@ 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() ? 1.93 : 0, + ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 189 : 0, ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10; static final boolean FOC_ENABLED = true; - public static final double ARM_LENGTH_METERS = 0.67; + public static final double ARM_LENGTH_METERS = 0.52; private static final int ARM_MOTOR_AMOUNT = 2, END_EFFECTOR_MOTOR_AMOUNT = 1; @@ -179,7 +179,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 +197,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); diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index b78043b0..aa5819ad 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -91,7 +91,7 @@ public class ElevatorConstants { REVERSE_LIMIT_SENSOR::getBinaryValue ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); - public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.3262; + public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.05; private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; static { @@ -164,8 +164,8 @@ private static void configureReverseLimitSensor() { public enum ElevatorState { REST(0.603, 0.7), LOAD_CORAL(0.5519, 0.7), - SCORE_L1(0.603, 1), - SCORE_L2(0.603, 1), + SCORE_L1(0.203, 1), + SCORE_L2(0.203, 1), SCORE_L3(1.003, 1), SCORE_L4(1.382, 1), PREPARE_L1(0.603, 1), From f327b94ac2e508756f90d9df1948abd491348280 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 15 Sep 2025 21:14:39 +0300 Subject: [PATCH 08/43] fixed logic --- .../frc/trigon/robot/subsystems/arm/Arm.java | 18 ++++++++++++++---- .../robot/subsystems/arm/ArmConstants.java | 19 +++++++++---------- .../robot/subsystems/elevator/Elevator.java | 6 +++++- .../elevator/ElevatorConstants.java | 2 ++ 4 files changed, 30 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 4e7e6704..a98abfea 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -83,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); @@ -95,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(); @@ -122,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; @@ -146,14 +150,16 @@ void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) this.targetState = targetState; if (isStateReversed) { - setTargetAngle(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle)); + subtractFrom360(targetState.targetAngle); return; } setTargetAngle(targetState.targetAngle); } private void setTargetAngle(Rotation2d targetAngle) { - Rotation2d minimumSafeAngle = Rotation2d.fromDegrees(Math.acos((RobotContainer.ELEVATOR.getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS) / ArmConstants.ARM_LENGTH_METERS)); + 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()))); } @@ -162,6 +168,10 @@ private void setTargetVoltage(double 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(0, 0, RobotContainer.ELEVATOR.getPositionMeters()), diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 5aa5c6d9..c001d223 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -57,8 +57,8 @@ public class ArmConstants { 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() ? 1.93 : 0, - ARM_DEFAULT_MAXIMUM_ACCELERATION = RobotHardwareStats.isSimulation() ? 189 : 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; @@ -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) ); @@ -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.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; @@ -236,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 index 73b6735c..299d919c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -81,6 +81,10 @@ public double getPositionMeters() { return rotationsToMeters(getPositionRotations()); } + public boolean isElevatorAboveSafeZone() { + return getPositionMeters() >= ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS; + } + void loadCoralFromElevator() { this.targetState = ElevatorConstants.ElevatorState.LOAD_CORAL; scalePositionRequestSpeed(targetState.speedScalar); @@ -94,7 +98,7 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) { } void setTargetPositionRotations(double targetPositionRotations) { - double minimumSafeHeightMeters = Math.cos(RobotContainer.ARM.getAngle().getDegrees()) * ArmConstants.ARM_LENGTH_METERS + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + double minimumSafeHeightMeters = RobotContainer.ARM.isArmAboveSafeZone() ? 0 : Math.cos(RobotContainer.ARM.getAngle().getDegrees()) * ArmConstants.ARM_LENGTH_METERS + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters); masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations))); } diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index aa5819ad..fe5f60cc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.subsystems.arm.ArmConstants; import lib.hardware.RobotHardwareStats; import lib.hardware.misc.simplesensor.SimpleSensor; import lib.hardware.phoenix6.talonfx.TalonFXMotor; @@ -92,6 +93,7 @@ public class ElevatorConstants { ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.05; + public static final double MAXIMUM_ELEVATOR_SAFE_ZONE_METERS = MINIMUM_ELEVATOR_SAFE_ZONE_METERS + ArmConstants.ARM_LENGTH_METERS; private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; static { From bd9895898c9556d607249cdd25c08526043c2595 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 16 Sep 2025 10:12:57 +0300 Subject: [PATCH 09/43] fixed logic issue and added d to arm pid --- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index c001d223..6ec68b16 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -144,7 +144,7 @@ private static void configureArmMasterMotor() { config.Slot0.kP = RobotHardwareStats.isSimulation() ? 100 : 0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 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; diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 299d919c..15069498 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -98,7 +98,7 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) { } void setTargetPositionRotations(double targetPositionRotations) { - double minimumSafeHeightMeters = RobotContainer.ARM.isArmAboveSafeZone() ? 0 : Math.cos(RobotContainer.ARM.getAngle().getDegrees()) * ArmConstants.ARM_LENGTH_METERS + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + double minimumSafeHeightMeters = RobotContainer.ARM.isArmAboveSafeZone() ? 0 : Math.cos(RobotContainer.ARM.getAngle().getRadians()) * ArmConstants.ARM_LENGTH_METERS + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters); masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations))); } From eb64ed037b24353084438a2adbc75fa817107fa1 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 16 Sep 2025 10:29:03 +0300 Subject: [PATCH 10/43] fixed stuff --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 4 ++-- .../trigon/robot/subsystems/elevator/ElevatorCommands.java | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index a98abfea..d503acd8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -150,7 +150,7 @@ void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) this.targetState = targetState; if (isStateReversed) { - subtractFrom360(targetState.targetAngle); + setTargetAngle(subtractFrom360(targetState.targetAngle)); return; } setTargetAngle(targetState.targetAngle); diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 15069498..ff50baa2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -85,10 +85,10 @@ public boolean isElevatorAboveSafeZone() { return getPositionMeters() >= ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS; } - void loadCoralFromElevator() { + void prepareLoadCoral() { this.targetState = ElevatorConstants.ElevatorState.LOAD_CORAL; scalePositionRequestSpeed(targetState.speedScalar); - masterMotor.setControl(positionRequest.withPosition(ElevatorConstants.ElevatorState.LOAD_CORAL.targetPositionMeters)); + masterMotor.setControl(positionRequest.withPosition(metersToRotations(ElevatorConstants.ElevatorState.LOAD_CORAL.targetPositionMeters))); } void setTargetState(ElevatorConstants.ElevatorState targetState) { diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index cdb60be5..88096bdd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -19,9 +19,9 @@ public static Command getDebbugingCommand() { ); } - public static Command getLoadCoralFromElevatorCommand() { + public static Command getPrepareLoadCoral() { return new StartEndCommand( - RobotContainer.ELEVATOR::loadCoralFromElevator, + RobotContainer.ELEVATOR::prepareLoadCoral, RobotContainer.ELEVATOR::stop, RobotContainer.ELEVATOR ).onlyIf(() -> RobotContainer.ARM.atState(ArmConstants.ArmState.REST)); From 024fb62ccfc3de83619a8c9580b6f398aecb1e5b Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 11:23:13 +0300 Subject: [PATCH 11/43] made function more readable --- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index ff50baa2..76181e02 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -98,8 +98,9 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) { } void setTargetPositionRotations(double targetPositionRotations) { - double minimumSafeHeightMeters = RobotContainer.ARM.isArmAboveSafeZone() ? 0 : Math.cos(RobotContainer.ARM.getAngle().getRadians()) * ArmConstants.ARM_LENGTH_METERS + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; - double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters); + final double elevatorHeightFrontArm = Math.cos(RobotContainer.ARM.getAngle().getRadians()) * ArmConstants.ARM_LENGTH_METERS; + final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeZone() ? 0 : elevatorHeightFrontArm) + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + final double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters); masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations))); } From d1c4d2dbba8594d60cb1e572b8d578238e8dc4ed Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 11:23:42 +0300 Subject: [PATCH 12/43] Update Elevator.java --- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 76181e02..aff262db 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -98,8 +98,8 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) { } void setTargetPositionRotations(double targetPositionRotations) { - final double elevatorHeightFrontArm = Math.cos(RobotContainer.ARM.getAngle().getRadians()) * ArmConstants.ARM_LENGTH_METERS; - final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeZone() ? 0 : elevatorHeightFrontArm) + ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + 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))); } From 4dd87f58275f61ab8e97175cd7fda320030971c1 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 16:13:17 +0300 Subject: [PATCH 13/43] rename --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index d503acd8..e51675f4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -157,8 +157,8 @@ void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) } private void setTargetAngle(Rotation2d targetAngle) { - final double heightFromSafeZone = RobotContainer.ELEVATOR.getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; - final double cosMinimumAngle = heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS; + final double distanceFromSafeZone = RobotContainer.ELEVATOR.getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + final double cosMinimumAngle = distanceFromSafeZone / 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()))); } From 379867eeff669a933b5bfbc7f03ddb89f3b57fd8 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 16:25:33 +0300 Subject: [PATCH 14/43] Update Arm.java --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index e51675f4..5af1d4f2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -99,7 +99,7 @@ public boolean atState(ArmConstants.ArmState targetState) { public boolean atTargetAngle(boolean isStateReversed) { if (isStateReversed) { - final double currentToTargetStateDifferenceDegrees = Math.abs(subtractFrom360(targetState.targetAngle).minus(getAngle()).getDegrees()); + final double currentToTargetStateDifferenceDegrees = Math.abs(subtractFrom360Degrees(targetState.targetAngle).minus(getAngle()).getDegrees()); return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); } return atTargetAngle(); @@ -126,7 +126,7 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) if (isStateReversed) { setTargetState( - subtractFrom360(targetState.targetAngle) + subtractFrom360Degrees(targetState.targetAngle) , targetState.targetEndEffectorVoltage ); return; @@ -150,7 +150,7 @@ void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) this.targetState = targetState; if (isStateReversed) { - setTargetAngle(subtractFrom360(targetState.targetAngle)); + setTargetAngle(subtractFrom360Degrees(targetState.targetAngle)); return; } setTargetAngle(targetState.targetAngle); @@ -168,7 +168,7 @@ private void setTargetVoltage(double targetVoltage) { endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage)); } - public static Rotation2d subtractFrom360(Rotation2d angleToSubtract) { + public static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { return Rotation2d.fromDegrees(360 - angleToSubtract.getDegrees()); } From 1b9c09f97687c56672cf5619e658479f88b1bfd3 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 16:30:49 +0300 Subject: [PATCH 15/43] removed magic number --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 5af1d4f2..f269c340 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -84,7 +84,7 @@ public void sysIDDrive(double targetVoltage) { } public boolean isArmAboveSafeZone() { - return getAngle().getDegrees() >= 90; + return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ZONE.getDegrees(); } public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) { diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 6ec68b16..5f79a1e4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -114,6 +114,7 @@ public class ArmConstants { new Rotation3d(0, 0, 0) ); + static final Rotation2d MAXIMUM_ARM_SAFE_ZONE = 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( From 4820ccc95706a1b82d4801037c6711bb65581c79 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 16:45:19 +0300 Subject: [PATCH 16/43] moved into more variables --- .../java/frc/trigon/robot/subsystems/arm/Arm.java | 14 +++++++++----- .../trigon/robot/subsystems/elevator/Elevator.java | 4 ++++ 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index f269c340..ba3c0ab3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -2,13 +2,13 @@ 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 frc.trigon.robot.subsystems.elevator.ElevatorConstants; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; import lib.hardware.phoenix6.cancoder.CANcoderSignal; import lib.hardware.phoenix6.talonfx.TalonFXMotor; @@ -157,10 +157,14 @@ void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) } private void setTargetAngle(Rotation2d targetAngle) { - final double distanceFromSafeZone = RobotContainer.ELEVATOR.getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; - final double cosMinimumAngle = distanceFromSafeZone / 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()))); + final boolean isAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); + final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromSafeZone(); + final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1); + final Rotation2d minSafeAngle = isAboveSafeZone + ? Rotation2d.fromRadians(0) + : Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle)); + armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minSafeAngle.getRotations()))); + } private void setTargetVoltage(double targetVoltage) { diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index aff262db..f284ef99 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -85,6 +85,10 @@ public boolean isElevatorAboveSafeZone() { return getPositionMeters() >= ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS; } + public double getElevatorHeightFromSafeZone() { + return getPositionMeters() - ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS; + } + void prepareLoadCoral() { this.targetState = ElevatorConstants.ElevatorState.LOAD_CORAL; scalePositionRequestSpeed(targetState.speedScalar); From bbe7ac40db79c659a089d86c9f2b1c1c0735e8cf Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 16:49:25 +0300 Subject: [PATCH 17/43] added javadoc --- .../java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 4 +++- .../robot/subsystems/elevator/ElevatorConstants.java | 7 +++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 5f79a1e4..d6140e5a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -113,7 +113,9 @@ public class ArmConstants { 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_ZONE = Rotation2d.fromDegrees(90); static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0); private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index fe5f60cc..05758a80 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -92,7 +92,14 @@ public class ElevatorConstants { REVERSE_LIMIT_SENSOR::getBinaryValue ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); + /** + * The lowest point in the Elevators zone where the safety logic applies. + */ public static final double MINIMUM_ELEVATOR_SAFE_ZONE_METERS = 0.05; + + /** + * The highest point in the Elevators zone where the safety logic applies. + */ public static final double MAXIMUM_ELEVATOR_SAFE_ZONE_METERS = MINIMUM_ELEVATOR_SAFE_ZONE_METERS + ArmConstants.ARM_LENGTH_METERS; private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; From 140137d614cf22b57fe9a6bf0330acd54d955ffa Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 17:05:41 +0300 Subject: [PATCH 18/43] switched current limit type to stator --- .../java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index d6140e5a..fb591657 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -160,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); From 2b0037f38b12d091879cfef345fa026cda16bf2c Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 17:09:04 +0300 Subject: [PATCH 19/43] renamed --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index ba3c0ab3..f42d48ee 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -158,7 +158,7 @@ void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) private void setTargetAngle(Rotation2d targetAngle) { final boolean isAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); - final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromSafeZone(); + final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromMinimumSafeZone(); final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1); final Rotation2d minSafeAngle = isAboveSafeZone ? Rotation2d.fromRadians(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 index f284ef99..cbcc3f5e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -85,8 +85,8 @@ public boolean isElevatorAboveSafeZone() { return getPositionMeters() >= ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS; } - public double getElevatorHeightFromSafeZone() { - return getPositionMeters() - ElevatorConstants.MAXIMUM_ELEVATOR_SAFE_ZONE_METERS; + public double getElevatorHeightFromMinimumSafeZone() { + return getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; } void prepareLoadCoral() { From 482bd8f1086befc880950ee5589c7c9a6d9e14b7 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 17:11:47 +0300 Subject: [PATCH 20/43] renamed stuff --- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 2 +- .../trigon/robot/subsystems/elevator/ElevatorCommands.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index cbcc3f5e..f62e24be 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -89,7 +89,7 @@ public double getElevatorHeightFromMinimumSafeZone() { return getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; } - void prepareLoadCoral() { + void prepareForLoadCoral() { this.targetState = ElevatorConstants.ElevatorState.LOAD_CORAL; scalePositionRequestSpeed(targetState.speedScalar); masterMotor.setControl(positionRequest.withPosition(metersToRotations(ElevatorConstants.ElevatorState.LOAD_CORAL.targetPositionMeters))); diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index 88096bdd..d2271a44 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -19,9 +19,9 @@ public static Command getDebbugingCommand() { ); } - public static Command getPrepareLoadCoral() { + public static Command getPrepareForLoadCoralCommand() { return new StartEndCommand( - RobotContainer.ELEVATOR::prepareLoadCoral, + RobotContainer.ELEVATOR::prepareForLoadCoral, RobotContainer.ELEVATOR::stop, RobotContainer.ELEVATOR ).onlyIf(() -> RobotContainer.ARM.atState(ArmConstants.ArmState.REST)); From 1aeb7bb1cc5cc3a792c1c1a17287c701ff45d1c2 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 17:29:52 +0300 Subject: [PATCH 21/43] extracted into multiple meathods --- .../frc/trigon/robot/subsystems/elevator/Elevator.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index f62e24be..ac09db78 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -102,8 +102,12 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) { } void setTargetPositionRotations(double 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 armCos = RobotContainer.ARM.getAngle().getRadians() * ArmConstants.ARM_LENGTH_METERS; + final double elevatorHeightFromArm = Math.cos(armCos); + final double minimumSafeElevatorHeight = ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeZone() + ? 0 : elevatorHeightFromArm) + + minimumSafeElevatorHeight; final double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters); masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations))); } From 07b24f62dfc5ed4f3c2d1654f9efbcf531eea8f3 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 18:17:56 +0300 Subject: [PATCH 22/43] added command for checking if at an abgle --- .../frc/trigon/robot/subsystems/arm/Arm.java | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index f42d48ee..a99cbb69 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -29,6 +29,7 @@ public class Arm extends MotorSubsystem { ).withEnableFOC(ArmConstants.FOC_ENABLED); public boolean isStateReversed = false; private ArmConstants.ArmState targetState = ArmConstants.ArmState.REST; + private Rotation2d calculatedMinimumArmAngle; public Arm() { setName("Arm"); @@ -99,15 +100,13 @@ public boolean atState(ArmConstants.ArmState targetState) { public boolean atTargetAngle(boolean isStateReversed) { if (isStateReversed) { - final double currentToTargetStateDifferenceDegrees = Math.abs(subtractFrom360Degrees(targetState.targetAngle).minus(getAngle()).getDegrees()); - return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + 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 Rotation2d getAngle() { @@ -156,6 +155,10 @@ void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) setTargetAngle(targetState.targetAngle); } + boolean isArmAtCalculatedMinimumAngle() { + return atAngle(calculatedMinimumArmAngle); + } + private void setTargetAngle(Rotation2d targetAngle) { final boolean isAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromMinimumSafeZone(); @@ -163,10 +166,16 @@ private void setTargetAngle(Rotation2d targetAngle) { final Rotation2d minSafeAngle = isAboveSafeZone ? Rotation2d.fromRadians(0) : Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle)); + calculatedMinimumArmAngle = minSafeAngle; armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minSafeAngle.getRotations()))); } + private boolean atAngle(Rotation2d targetAngle) { + final double currentToTargetAngleDifferenceDegrees = Math.abs(targetAngle.minus(getAngle()).getDegrees()); + return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + } + private void setTargetVoltage(double targetVoltage) { ArmConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage); endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage)); From 94c264b4d312b99626cd2721fdbddc7b9def5ec1 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 19:21:47 +0300 Subject: [PATCH 23/43] Update Arm.java --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index a99cbb69..badcc329 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -181,7 +181,7 @@ private void setTargetVoltage(double targetVoltage) { endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage)); } - public static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { + private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { return Rotation2d.fromDegrees(360 - angleToSubtract.getDegrees()); } From 936826dc3dc702609128211c2df5a26e31fad06e Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 19:49:17 +0300 Subject: [PATCH 24/43] Update Arm.java --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index badcc329..a5a57ff6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -100,7 +100,7 @@ public boolean atState(ArmConstants.ArmState targetState) { public boolean atTargetAngle(boolean isStateReversed) { if (isStateReversed) { - atAngle(subtractFrom360Degrees(targetState.targetAngle)); + return atAngle(subtractFrom360Degrees(targetState.targetAngle)); } return atTargetAngle(); } From 9b9fc934984b78ff8f500c1c5298301ee6d03e4d Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 19:55:50 +0300 Subject: [PATCH 25/43] fixed arm logic --- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index ac09db78..f600e4bf 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -102,8 +102,8 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) { } void setTargetPositionRotations(double targetPositionRotations) { - final double armCos = RobotContainer.ARM.getAngle().getRadians() * ArmConstants.ARM_LENGTH_METERS; - final double elevatorHeightFromArm = Math.cos(armCos); + final double armCos = RobotContainer.ARM.getAngle().getRadians(); + final double elevatorHeightFromArm = Math.cos(armCos) * ArmConstants.ARM_LENGTH_METERS; final double minimumSafeElevatorHeight = ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeZone() ? 0 : elevatorHeightFromArm) From c2591b7c2f3fc65a092ef62fdfd2a90894ce2286 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 22:56:09 +0300 Subject: [PATCH 26/43] better pid --- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index fb591657..a6efb87b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -145,7 +145,7 @@ private static void configureArmMasterMotor() { config.Feedback.FeedbackRemoteSensorID = ARM_MASTER_MOTOR.getID(); config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 100 : 0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 38 : 0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 1 : 0; config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.026331 : 0; From 8159e68ed0c0ee4cb2db682c3f2411ec093035ff Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 22:58:09 +0300 Subject: [PATCH 27/43] Update Arm.java --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index a5a57ff6..5346ba0d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -29,7 +29,6 @@ public class Arm extends MotorSubsystem { ).withEnableFOC(ArmConstants.FOC_ENABLED); public boolean isStateReversed = false; private ArmConstants.ArmState targetState = ArmConstants.ArmState.REST; - private Rotation2d calculatedMinimumArmAngle; public Arm() { setName("Arm"); @@ -155,10 +154,6 @@ void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) setTargetAngle(targetState.targetAngle); } - boolean isArmAtCalculatedMinimumAngle() { - return atAngle(calculatedMinimumArmAngle); - } - private void setTargetAngle(Rotation2d targetAngle) { final boolean isAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromMinimumSafeZone(); @@ -166,7 +161,6 @@ private void setTargetAngle(Rotation2d targetAngle) { final Rotation2d minSafeAngle = isAboveSafeZone ? Rotation2d.fromRadians(0) : Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle)); - calculatedMinimumArmAngle = minSafeAngle; armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minSafeAngle.getRotations()))); } From dc3319b28db04fef17a2729f5e7df594ceb16209 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 23:23:19 +0300 Subject: [PATCH 28/43] added gravity offset to the getAngle method --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 4 ++-- .../java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 5346ba0d..21da664b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -61,7 +61,7 @@ public void updateLog(SysIdRoutineLog log) { @Override public void updateMechanism() { ArmConstants.ARM_MECHANISM.update( - Rotation2d.fromRotations(getAngle().getRotations() + ArmConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET), + Rotation2d.fromRotations(getAngle().getRotations()), Rotation2d.fromRotations(armMasterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + ArmConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET) ); ArmConstants.END_EFFECTOR_MECHANISM.update(endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); @@ -109,7 +109,7 @@ public boolean atTargetAngle() { } public Rotation2d getAngle() { - return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); + return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION) + ArmConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET); } void setTargetState(ArmConstants.ArmState targetState) { diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index a6efb87b..9a189888 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -54,7 +54,7 @@ public class ArmConstants { 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; + static final double POSITION_OFFSET_FROM_GRAVITY_OFFSET = RobotHardwareStats.isSimulation() ? 0 : 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() ? 2.4614 : 0, From d748b5e985f93d1b42a596458170bbad990a1e44 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 18 Sep 2025 23:50:19 +0300 Subject: [PATCH 29/43] Update src/main/java/frc/trigon/robot/subsystems/arm/Arm.java Co-authored-by: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 21da664b..2c5242ee 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -155,7 +155,7 @@ void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) } private void setTargetAngle(Rotation2d targetAngle) { - final boolean isAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); + final boolean isElevatorAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromMinimumSafeZone(); final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1); final Rotation2d minSafeAngle = isAboveSafeZone From 248ca5c3910896836c6b06015222d8d4f6baca58 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 00:03:51 +0300 Subject: [PATCH 30/43] siwtched set target state commands from execute to functional so that the end effector only gets applid voltage once and not every loop --- .../frc/trigon/robot/subsystems/arm/Arm.java | 11 +++++++---- .../robot/subsystems/arm/ArmCommands.java | 18 +++++++++++------- 2 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 21da664b..9f8eb55b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -139,11 +139,11 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { setTargetVoltage(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; @@ -153,6 +153,9 @@ void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) } setTargetAngle(targetState.targetAngle); } + void setEndEffectorState(ArmConstants.ArmState targetState){ + setTargetVoltage(targetState.targetEndEffectorVoltage); + } private void setTargetAngle(Rotation2d targetAngle) { final boolean isAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); @@ -170,7 +173,7 @@ private boolean atAngle(Rotation2d targetAngle) { return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); } - private void setTargetVoltage(double targetVoltage) { + void setTargetVoltage(double targetVoltage) { ArmConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage); endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index 94d3f8e6..bd3f7a16 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -2,7 +2,7 @@ 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; @@ -34,24 +34,28 @@ public static Command getGearRatioCalulationCommand() { } public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { - return new ExecuteEndCommand( + return new FunctionalCommand( + () -> RobotContainer.ARM.setEndEffectorState(targetState), () -> RobotContainer.ARM.setTargetState(targetState, isStateReversed), - RobotContainer.ARM::stop, + null, + () -> false, RobotContainer.ARM ); } public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { - return new ExecuteEndCommand( + return new FunctionalCommand( + () -> RobotContainer.ARM.setEndEffectorState(targetState), () -> RobotContainer.ARM.setTargetState(targetState), - RobotContainer.ARM::stop, + null, + () -> false, RobotContainer.ARM ); } public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { return new ExecuteEndCommand( - () -> RobotContainer.ARM.prepareForState(targetState, isStateReversed), + () -> RobotContainer.ARM.setArmState(targetState, isStateReversed), RobotContainer.ARM::stop, RobotContainer.ARM ); @@ -59,7 +63,7 @@ public static Command getPrepareForStateCommand(ArmConstants.ArmState targetStat public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState) { return new ExecuteEndCommand( - () -> RobotContainer.ARM.prepareForState(targetState), + () -> RobotContainer.ARM.setArmState(targetState), RobotContainer.ARM::stop, RobotContainer.ARM ); From c0db5e11de58edc610c456ec1db0bc12562c08d2 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 00:17:08 +0300 Subject: [PATCH 31/43] made load coral function part of setTargetState --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- .../frc/trigon/robot/subsystems/arm/ArmCommands.java | 2 +- .../frc/trigon/robot/subsystems/elevator/Elevator.java | 10 ++++------ .../robot/subsystems/elevator/ElevatorCommands.java | 8 -------- 4 files changed, 6 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 0a0af518..e52c9d14 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -161,7 +161,7 @@ private void setTargetAngle(Rotation2d targetAngle) { final boolean isElevatorAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromMinimumSafeZone(); final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1); - final Rotation2d minSafeAngle = isAboveSafeZone + final Rotation2d minSafeAngle = isElevatorAboveSafeZone ? Rotation2d.fromRadians(0) : Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle)); armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minSafeAngle.getRotations()))); diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index bd3f7a16..13be9f9c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -46,7 +46,7 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { return new FunctionalCommand( () -> RobotContainer.ARM.setEndEffectorState(targetState), - () -> RobotContainer.ARM.setTargetState(targetState), + () -> RobotContainer.ARM.setArmState(targetState), null, () -> false, RobotContainer.ARM diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index f600e4bf..6200e954 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -89,15 +89,13 @@ public double getElevatorHeightFromMinimumSafeZone() { return getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; } - void prepareForLoadCoral() { - 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); + if (targetState == ElevatorConstants.ElevatorState.LOAD_CORAL) { + masterMotor.setControl(positionRequest.withPosition(metersToRotations(targetState.targetPositionMeters))); + return; + } setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index d2271a44..0957aa70 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -19,14 +19,6 @@ public static Command getDebbugingCommand() { ); } - public static Command getPrepareForLoadCoralCommand() { - return new StartEndCommand( - RobotContainer.ELEVATOR::prepareForLoadCoral, - 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), From 43757f9153e19e3203ee7e4330b904f0fff12dcc Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 00:20:32 +0300 Subject: [PATCH 32/43] Update Arm.java --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index e52c9d14..ec066bbe 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -61,7 +61,7 @@ public void updateLog(SysIdRoutineLog log) { @Override public void updateMechanism() { ArmConstants.ARM_MECHANISM.update( - Rotation2d.fromRotations(getAngle().getRotations()), + Rotation2d.fromRotations(getAngle().getRotations() + ArmConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET), Rotation2d.fromRotations(armMasterMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE) + ArmConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET) ); ArmConstants.END_EFFECTOR_MECHANISM.update(endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE)); @@ -109,7 +109,7 @@ public boolean atTargetAngle() { } public Rotation2d getAngle() { - return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION) + ArmConstants.POSITION_OFFSET_FROM_GRAVITY_OFFSET); + return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); } void setTargetState(ArmConstants.ArmState targetState) { From 62fcd32d558eaca74fbdc8888428b93de52fa813 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 00:36:40 +0300 Subject: [PATCH 33/43] got rid of unnecesary functions --- .../frc/trigon/robot/subsystems/arm/Arm.java | 36 +++++++------------ .../robot/subsystems/arm/ArmCommands.java | 4 +-- .../robot/subsystems/arm/ArmConstants.java | 2 +- .../robot/subsystems/elevator/Elevator.java | 2 +- 4 files changed, 17 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index ec066bbe..a437b07a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -83,29 +83,18 @@ public void sysIDDrive(double targetVoltage) { armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } - public boolean isArmAboveSafeZone() { - return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ZONE.getDegrees(); + public boolean isArmAboveSafeAngle() { + return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); } 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(); - } - - public boolean atTargetAngle(boolean isStateReversed) { - if (isStateReversed) { - return atAngle(subtractFrom360Degrees(targetState.targetAngle)); - } - return atTargetAngle(); - } - - public boolean atTargetAngle() { - return atAngle(targetState.targetAngle); + return this.targetState == targetState && atAngle(targetState.targetAngle); } public Rotation2d getAngle() { @@ -153,7 +142,8 @@ void setArmState(ArmConstants.ArmState targetState, boolean isStateReversed) { } setTargetAngle(targetState.targetAngle); } - void setEndEffectorState(ArmConstants.ArmState targetState){ + + void setEndEffectorState(ArmConstants.ArmState targetState) { setTargetVoltage(targetState.targetEndEffectorVoltage); } @@ -161,23 +151,23 @@ private void setTargetAngle(Rotation2d targetAngle) { final boolean isElevatorAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromMinimumSafeZone(); final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1); - final Rotation2d minSafeAngle = isElevatorAboveSafeZone + final Rotation2d minimumSafeAngle = isElevatorAboveSafeZone ? Rotation2d.fromRadians(0) : Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle)); - armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minSafeAngle.getRotations()))); + 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)); + } + private boolean atAngle(Rotation2d targetAngle) { final double currentToTargetAngleDifferenceDegrees = Math.abs(targetAngle.minus(getAngle()).getDegrees()); return currentToTargetAngleDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); } - void setTargetVoltage(double targetVoltage) { - ArmConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(targetVoltage); - endEffectorMotor.setControl(voltageRequest.withOutput(targetVoltage)); - } - private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { return Rotation2d.fromDegrees(360 - angleToSubtract.getDegrees()); } diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index 13be9f9c..015225c2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -37,7 +37,7 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState return new FunctionalCommand( () -> RobotContainer.ARM.setEndEffectorState(targetState), () -> RobotContainer.ARM.setTargetState(targetState, isStateReversed), - null, + interrupted -> RobotContainer.ARM.stop(), () -> false, RobotContainer.ARM ); @@ -47,7 +47,7 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState return new FunctionalCommand( () -> RobotContainer.ARM.setEndEffectorState(targetState), () -> RobotContainer.ARM.setArmState(targetState), - null, + interrupted -> RobotContainer.ARM.stop(), () -> false, 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 index 9a189888..69c370bb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -116,7 +116,7 @@ public class ArmConstants { /** * The highest point of the arms angular zone where the safety logic applies. */ - static final Rotation2d MAXIMUM_ARM_SAFE_ZONE = Rotation2d.fromDegrees(90); + 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( diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 6200e954..9d7c261e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -103,7 +103,7 @@ void setTargetPositionRotations(double targetPositionRotations) { final double armCos = RobotContainer.ARM.getAngle().getRadians(); final double elevatorHeightFromArm = Math.cos(armCos) * ArmConstants.ARM_LENGTH_METERS; final double minimumSafeElevatorHeight = ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; - final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeZone() + final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeAngle() ? 0 : elevatorHeightFromArm) + minimumSafeElevatorHeight; final double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters); From 440dc795734da3d66cac4c1002805f05817588e7 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 00:47:00 +0300 Subject: [PATCH 34/43] put logic in setTargetAngle into different function --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index a437b07a..b2f1986a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -148,14 +148,16 @@ void setEndEffectorState(ArmConstants.ArmState targetState) { } private void setTargetAngle(Rotation2d targetAngle) { + armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minimumArmSafeAngle().getRotations()))); + } + + private Rotation2d minimumArmSafeAngle() { final boolean isElevatorAboveSafeZone = RobotContainer.ELEVATOR.isElevatorAboveSafeZone(); final double heightFromSafeZone = RobotContainer.ELEVATOR.getElevatorHeightFromMinimumSafeZone(); final double cosOfMinimumSafeAngle = MathUtil.clamp(heightFromSafeZone / ArmConstants.ARM_LENGTH_METERS, 0, 1); - final Rotation2d minimumSafeAngle = isElevatorAboveSafeZone + return isElevatorAboveSafeZone ? Rotation2d.fromRadians(0) : Rotation2d.fromRadians(Math.acos(cosOfMinimumSafeAngle)); - armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minimumSafeAngle.getRotations()))); - } private void setTargetVoltage(double targetVoltage) { From fae20662c831888a7d53746b59b66e1644eba79e Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 00:52:12 +0300 Subject: [PATCH 35/43] Update Arm.java --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index b2f1986a..2e644b98 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -97,6 +97,13 @@ public boolean atState(ArmConstants.ArmState targetState) { return this.targetState == targetState && atAngle(targetState.targetAngle); } + public boolean atTargetAngle() { + if (isStateReversed) { + return atAngle(subtractFrom360Degrees(targetState.targetAngle)); + } + return atAngle(targetState.targetAngle); + } + public Rotation2d getAngle() { return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); } From 33beb6dd60cf8162870ffb0f1b354d953e3a7c41 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 01:02:25 +0300 Subject: [PATCH 36/43] separated setTargetPosition logic into differant logic --- .../trigon/robot/subsystems/elevator/Elevator.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 9d7c261e..fbc553a8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -100,14 +100,17 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) { } void setTargetPositionRotations(double targetPositionRotations) { + masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeElevatorHeightRotations()))); + } + + private double minimumSafeElevatorHeightRotations() { final double armCos = RobotContainer.ARM.getAngle().getRadians(); final double elevatorHeightFromArm = Math.cos(armCos) * ArmConstants.ARM_LENGTH_METERS; - final double minimumSafeElevatorHeight = ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; + final double minimumElevatorSafeZone = ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeAngle() ? 0 : elevatorHeightFromArm) - + minimumSafeElevatorHeight; - final double minimumSafeHeightRotations = metersToRotations(minimumSafeHeightMeters); - masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeHeightRotations))); + + minimumElevatorSafeZone; + return metersToRotations(minimumSafeHeightMeters); } private Pose3d getFirstStageComponentPose() { From cf68be911f479091e9f22c6c06742f36a22bcdcd Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 01:08:17 +0300 Subject: [PATCH 37/43] Update ArmConstants.java --- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 69c370bb..a85dfec7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -54,7 +54,7 @@ public class ArmConstants { 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 : 0 + Conversions.degreesToRotations(0) - ANGLE_ENCODER_GRAVITY_OFFSET; + 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() ? 2.4614 : 0, From d1c7edcee033208b6c8f9424be41ffc05bb968ba Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 01:13:03 +0300 Subject: [PATCH 38/43] Update ArmConstants.java --- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index a85dfec7..ab2c8de8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -54,7 +54,7 @@ public class ArmConstants { 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; + 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() ? 2.4614 : 0, From 5bf81942e13556db83f88b991c27c0ffc78082c3 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 01:40:07 +0300 Subject: [PATCH 39/43] Update Arm.java --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 2e644b98..e21d4136 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -178,7 +178,7 @@ private boolean atAngle(Rotation2d targetAngle) { } private static Rotation2d subtractFrom360Degrees(Rotation2d angleToSubtract) { - return Rotation2d.fromDegrees(360 - angleToSubtract.getDegrees()); + return Rotation2d.fromDegrees(Rotation2d.k180deg.getDegrees() * 2 - angleToSubtract.getDegrees()); } private Pose3d calculateVisualizationPose() { From bd19f72727a7532db2d194ca52d87b73c9364084 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 02:08:00 +0300 Subject: [PATCH 40/43] Update Arm.java --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index e21d4136..6056f9c2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -132,7 +132,7 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) void setTargetState(Rotation2d targetAngle, double targetVoltage) { setTargetAngle(targetAngle); - setTargetVoltage(targetVoltage); + setEndEffectorTargetVoltage(targetVoltage); } void setArmState(ArmConstants.ArmState targetState) { @@ -151,7 +151,7 @@ void setArmState(ArmConstants.ArmState targetState, boolean isStateReversed) { } void setEndEffectorState(ArmConstants.ArmState targetState) { - setTargetVoltage(targetState.targetEndEffectorVoltage); + setEndEffectorTargetVoltage(targetState.targetEndEffectorVoltage); } private void setTargetAngle(Rotation2d targetAngle) { @@ -167,7 +167,7 @@ private Rotation2d minimumArmSafeAngle() { : 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)); } From 4ea93e39b4eeee2f3265ccb3baa5669bdbc7952c Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 02:13:47 +0300 Subject: [PATCH 41/43] cleaned commands --- .../robot/subsystems/arm/ArmCommands.java | 24 ++++++------------- 1 file changed, 7 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index 015225c2..7b96ab63 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -33,24 +33,22 @@ public static Command getGearRatioCalulationCommand() { ); } + public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { + return getSetTargetStateCommand(targetState, false); + } + public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { return new FunctionalCommand( () -> RobotContainer.ARM.setEndEffectorState(targetState), - () -> RobotContainer.ARM.setTargetState(targetState, isStateReversed), + () -> RobotContainer.ARM.setArmState(targetState, isStateReversed), interrupted -> RobotContainer.ARM.stop(), () -> false, RobotContainer.ARM ); } - public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { - return new FunctionalCommand( - () -> RobotContainer.ARM.setEndEffectorState(targetState), - () -> RobotContainer.ARM.setArmState(targetState), - interrupted -> RobotContainer.ARM.stop(), - () -> false, - RobotContainer.ARM - ); + public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState) { + return getPrepareForStateCommand(targetState, false); } public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { @@ -60,12 +58,4 @@ public static Command getPrepareForStateCommand(ArmConstants.ArmState targetStat RobotContainer.ARM ); } - - public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState) { - return new ExecuteEndCommand( - () -> RobotContainer.ARM.setArmState(targetState), - RobotContainer.ARM::stop, - RobotContainer.ARM - ); - } } From df0a3f47714f7bbcb75d8ee018b50b5b496efb4a Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 02:14:35 +0300 Subject: [PATCH 42/43] Update Elevator.java --- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index fbc553a8..882fe360 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -100,10 +100,10 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) { } void setTargetPositionRotations(double targetPositionRotations) { - masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, minimumSafeElevatorHeightRotations()))); + masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, getMinimumSafeElevatorHeightRotations()))); } - private double minimumSafeElevatorHeightRotations() { + private double getMinimumSafeElevatorHeightRotations() { 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; From fb1233d967629cba738942bf11cc0aaaf21469cc Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 19 Sep 2025 02:25:53 +0300 Subject: [PATCH 43/43] changed names --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 4 ++-- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 6056f9c2..2e768d9f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -155,10 +155,10 @@ void setEndEffectorState(ArmConstants.ArmState targetState) { } private void setTargetAngle(Rotation2d targetAngle) { - armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), minimumArmSafeAngle().getRotations()))); + armMasterMotor.setControl(positionRequest.withPosition(Math.max(targetAngle.getRotations(), calculateMinimumArmSafeAngle().getRotations()))); } - private Rotation2d minimumArmSafeAngle() { + 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); diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 882fe360..bdbed2b9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -100,10 +100,10 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) { } void setTargetPositionRotations(double targetPositionRotations) { - masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, getMinimumSafeElevatorHeightRotations()))); + masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations()))); } - private double getMinimumSafeElevatorHeightRotations() { + 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;