From bb6f483d6c88c0d1a16bd87d308cecd11a4274a0 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 8 Sep 2025 18:44:41 +0300 Subject: [PATCH 1/8] Added Ejecting commands --- .../CoralEjectionCommands.java | 31 +++++++++++++++++++ .../robot/subsystems/intake/Intake.java | 2 +- 2 files changed, 32 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java new file mode 100644 index 00000000..1d274562 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -0,0 +1,31 @@ +package frc.trigon.robot.commands.commandfactories; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.subsystems.intake.IntakeCommands; +import frc.trigon.robot.subsystems.intake.IntakeConstants; +import frc.trigon.robot.subsystems.transporter.TransporterCommands; +import frc.trigon.robot.subsystems.transporter.TransporterConstants; + +public class CoralEjectionCommands { + public static Command getCoralEjectionCommand() { + return GeneralCommands.getContinuousConditionalCommand( + getEjectCoralFromIntakeCommand(), + getEjectCoralFromArmCommand(), + () -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.INTAKE.hasCoral() + ); + } + + private static Command getEjectCoralFromIntakeCommand() { + return new ParallelCommandGroup( + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.EJECT), + TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.EJECT) + ); + } + + private static Command getEjectCoralFromArmCommand() { + return new PrintCommand("Ejecting coral from arm - Not implemented yet"); //TODO: Shamya do this + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 32c94df6..8e76507d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -84,7 +84,7 @@ public boolean atTargetAngle() { return angleDifferenceFromTargetAngleDegrees < IntakeConstants.ANGLE_TOLERANCE.getDegrees(); } - public boolean hasGamePiece() { + public boolean hasCoral() { return IntakeConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } From 186cde255b8939ccd31a19d33bfc85081106c086 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 9 Sep 2025 19:36:16 +0300 Subject: [PATCH 2/8] added ejecting coral from arm/gripper --- .../commandfactories/CoralEjectionCommands.java | 10 ++++++++-- .../frc/trigon/robot/subsystems/arm/Arm.java | 11 +++++++++++ .../trigon/robot/subsystems/arm/ArmCommands.java | 16 ++++++++++++++++ .../robot/subsystems/arm/ArmConstants.java | 1 + 4 files changed, 36 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java index 1d274562..c0ffa902 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -2,8 +2,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.subsystems.arm.ArmCommands; +import frc.trigon.robot.subsystems.arm.ArmConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.transporter.TransporterCommands; @@ -26,6 +28,10 @@ private static Command getEjectCoralFromIntakeCommand() { } private static Command getEjectCoralFromArmCommand() { - return new PrintCommand("Ejecting coral from arm - Not implemented yet"); //TODO: Shamya do this + return new SequentialCommandGroup( + ArmCommands.getSetPrepareTargetStateCommand(ArmConstants.ArmState.EJECT).until(RobotContainer.ARM::atTargetAngle), + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.EJECT) + + ); } } \ No newline at end of file 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 d191f552..5b667f62 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -117,6 +117,17 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { setTargetVoltage(targetVoltage); } + void setPrepareTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) { + if (isStateReversed) + setTargetAngle(Rotation2d.fromDegrees(360 - targetState.targetAngle.getDegrees())); + setPrepareTargetState(targetState); + } + + void setPrepareTargetState(ArmConstants.ArmState targetState) { + this.targetState = targetState; + setTargetAngle(targetState.targetAngle); + } + private Rotation2d getAngle() { return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); } 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 78c9a6cc..0368b294 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -47,4 +47,20 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState RobotContainer.ARM ); } + + public static Command getSetPrepareTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { + return new StartEndCommand( + () -> RobotContainer.ARM.setPrepareTargetState(targetState, isStateReversed), + RobotContainer.ARM::stop, + RobotContainer.ARM + ); + } + + public static Command getSetPrepareTargetStateCommand(ArmConstants.ArmState targetState) { + return new StartEndCommand( + () -> RobotContainer.ARM.setPrepareTargetState(targetState), + RobotContainer.ARM::stop, + RobotContainer.ARM + ); + } } diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 197dfc7e..9b4c6f80 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -229,6 +229,7 @@ public enum ArmState { REST(Rotation2d.fromDegrees(0), 0), REST_FOR_CLIMB(Rotation2d.fromDegrees(0), 0), HOLD_ALGAE(Rotation2d.fromDegrees(90), -4), + EJECT(Rotation2d.fromDegrees(60), 4), PREPARE_SCORE_L1(Rotation2d.fromDegrees(80), 0), SCORE_L1(Rotation2d.fromDegrees(75), 4), PREPARE_SCORE_L2(Rotation2d.fromDegrees(95), 0), From 4b5b70a1cd11b3e7e0c7fb0dfbb838c4cf6df2f6 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 9 Sep 2025 19:37:58 +0300 Subject: [PATCH 3/8] Update CoralEjectionCommands.java --- .../robot/commands/commandfactories/CoralEjectionCommands.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java index c0ffa902..c97c29d7 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -31,7 +31,6 @@ private static Command getEjectCoralFromArmCommand() { return new SequentialCommandGroup( ArmCommands.getSetPrepareTargetStateCommand(ArmConstants.ArmState.EJECT).until(RobotContainer.ARM::atTargetAngle), ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.EJECT) - ); } } \ No newline at end of file From 15b3458b3e56e10b8cb4e22d68c09aa423445a01 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Wed, 10 Sep 2025 13:50:44 +0300 Subject: [PATCH 4/8] made atTargetAngle and atState functions that check for reversed states --- .../CoralEjectionCommands.java | 2 +- .../frc/trigon/robot/subsystems/arm/Arm.java | 19 +++++++++++++++---- .../robot/subsystems/arm/ArmCommands.java | 8 ++++---- 3 files changed, 20 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java index c97c29d7..c53d3d6b 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -29,7 +29,7 @@ private static Command getEjectCoralFromIntakeCommand() { private static Command getEjectCoralFromArmCommand() { return new SequentialCommandGroup( - ArmCommands.getSetPrepareTargetStateCommand(ArmConstants.ArmState.EJECT).until(RobotContainer.ARM::atTargetAngle), + ArmCommands.getPrepareStateCommand(ArmConstants.ArmState.EJECT).until(RobotContainer.ARM::atTargetAngle), ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.EJECT) ); } 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 5b667f62..a9302ae5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -80,10 +80,19 @@ public void sysIDDrive(double targetVoltage) { armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } + public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) { + return this.targetState == targetState && atTargetAngle(isStateReversed); + } + public boolean atState(ArmConstants.ArmState targetState) { return this.targetState == targetState && atTargetAngle(); } + public boolean atTargetAngle(boolean isStateReversed) { + final double currentToTargetStateDifferenceDegrees = Math.abs(360 - targetState.targetAngle.minus(getAngle()).getDegrees()); + return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + } + public boolean atTargetAngle() { final double currentToTargetStateDifferenceDegrees = Math.abs(targetState.targetAngle.minus(getAngle()).getDegrees()); return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); @@ -95,6 +104,7 @@ public boolean hasGamePiece() { void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) { if (isStateReversed) { + this.targetState = targetState; setTargetState( Rotation2d.fromDegrees(360 - targetState.targetAngle.getDegrees()) , targetState.targetEndEffectorVoltage @@ -117,13 +127,14 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { setTargetVoltage(targetVoltage); } - void setPrepareTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) { + void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) { if (isStateReversed) - setTargetAngle(Rotation2d.fromDegrees(360 - targetState.targetAngle.getDegrees())); - setPrepareTargetState(targetState); + this.targetState = targetState; + setTargetAngle(Rotation2d.fromDegrees(360 - targetState.targetAngle.getDegrees())); + prepareForState(targetState); } - void setPrepareTargetState(ArmConstants.ArmState targetState) { + void prepareForState(ArmConstants.ArmState targetState) { this.targetState = targetState; setTargetAngle(targetState.targetAngle); } 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 0368b294..5c1293cd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -48,17 +48,17 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState ); } - public static Command getSetPrepareTargetStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { + public static Command getPrepareStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { return new StartEndCommand( - () -> RobotContainer.ARM.setPrepareTargetState(targetState, isStateReversed), + () -> RobotContainer.ARM.prepareForState(targetState, isStateReversed), RobotContainer.ARM::stop, RobotContainer.ARM ); } - public static Command getSetPrepareTargetStateCommand(ArmConstants.ArmState targetState) { + public static Command getPrepareStateCommand(ArmConstants.ArmState targetState) { return new StartEndCommand( - () -> RobotContainer.ARM.setPrepareTargetState(targetState), + () -> RobotContainer.ARM.prepareForState(targetState), RobotContainer.ARM::stop, RobotContainer.ARM ); From d45045a2e148d9211f84ccd8b7f0af3c29260421 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Wed, 10 Sep 2025 13:52:47 +0300 Subject: [PATCH 5/8] forget to add the logic for the reversed states --- .../java/frc/trigon/robot/subsystems/arm/Arm.java | 11 ++++++++--- 1 file changed, 8 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 a9302ae5..334cdada 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -81,7 +81,9 @@ public void sysIDDrive(double targetVoltage) { } public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) { - return this.targetState == targetState && atTargetAngle(isStateReversed); + if (isStateReversed) + return this.targetState == targetState && atTargetAngle(isStateReversed); + return atState(targetState); } public boolean atState(ArmConstants.ArmState targetState) { @@ -89,8 +91,11 @@ public boolean atState(ArmConstants.ArmState targetState) { } public boolean atTargetAngle(boolean isStateReversed) { - final double currentToTargetStateDifferenceDegrees = Math.abs(360 - targetState.targetAngle.minus(getAngle()).getDegrees()); - return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + if (isStateReversed) { + final double currentToTargetStateDifferenceDegrees = Math.abs(360 - targetState.targetAngle.minus(getAngle()).getDegrees()); + return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); + } + return atTargetAngle(); } public boolean atTargetAngle() { From 8e453a2257d69f267445a2d5d2aae33cf9e8ea6e Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Wed, 10 Sep 2025 17:56:42 +0300 Subject: [PATCH 6/8] added variable to know if the state is reversed or not --- .../java/frc/trigon/robot/subsystems/arm/Arm.java | 14 ++++++++++---- 1 file changed, 10 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 334cdada..cea2342e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -25,6 +25,7 @@ public class Arm extends MotorSubsystem { ArmConstants.ARM_DEFAULT_MAXIMUM_ACCELERATION, ArmConstants.ARM_DEFAULT_MAXIMUM_JERK ).withEnableFOC(ArmConstants.FOC_ENABLED); + public boolean isStateReversed = false; private ArmConstants.ArmState targetState = ArmConstants.ArmState.REST; public Arm() { @@ -92,7 +93,7 @@ public boolean atState(ArmConstants.ArmState targetState) { public boolean atTargetAngle(boolean isStateReversed) { if (isStateReversed) { - final double currentToTargetStateDifferenceDegrees = Math.abs(360 - targetState.targetAngle.minus(getAngle()).getDegrees()); + final double currentToTargetStateDifferenceDegrees = Math.abs(Rotation2d.fromDegrees(360).minus(targetState.targetAngle).minus(getAngle()).getDegrees()); return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); } return atTargetAngle(); @@ -108,10 +109,11 @@ public boolean hasGamePiece() { } void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) { + this.isStateReversed = isStateReversed; if (isStateReversed) { this.targetState = targetState; setTargetState( - Rotation2d.fromDegrees(360 - targetState.targetAngle.getDegrees()) + Rotation2d.fromDegrees(360).minus(targetState.targetAngle) , targetState.targetEndEffectorVoltage ); return; @@ -120,6 +122,7 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) } void setTargetState(ArmConstants.ArmState targetState) { + this.isStateReversed = false; this.targetState = targetState; setTargetState( targetState.targetAngle, @@ -133,13 +136,16 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { } void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) { - if (isStateReversed) + this.isStateReversed = isStateReversed; + if (isStateReversed) { this.targetState = targetState; - setTargetAngle(Rotation2d.fromDegrees(360 - targetState.targetAngle.getDegrees())); + setTargetAngle(Rotation2d.fromDegrees(360).minus(targetState.targetAngle)); + } prepareForState(targetState); } void prepareForState(ArmConstants.ArmState targetState) { + this.isStateReversed = false; this.targetState = targetState; setTargetAngle(targetState.targetAngle); } From e0fed6037decfc62ed6fcf6b8a8c8dbb65bf22c5 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Wed, 10 Sep 2025 19:34:16 +0300 Subject: [PATCH 7/8] fixed logic issues and renamed stuff --- .../commands/commandfactories/CoralEjectionCommands.java | 2 +- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 8 ++++---- .../java/frc/trigon/robot/subsystems/arm/ArmCommands.java | 4 ++-- .../frc/trigon/robot/subsystems/arm/ArmConstants.java | 1 + 4 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java index c53d3d6b..cb52f00d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -29,7 +29,7 @@ private static Command getEjectCoralFromIntakeCommand() { private static Command getEjectCoralFromArmCommand() { return new SequentialCommandGroup( - ArmCommands.getPrepareStateCommand(ArmConstants.ArmState.EJECT).until(RobotContainer.ARM::atTargetAngle), + ArmCommands.getPrepareForStateCommand(ArmConstants.ArmState.EJECT).until(RobotContainer.ARM::atTargetAngle), ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.EJECT) ); } 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 cea2342e..a8f0a3c9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -93,7 +93,7 @@ public boolean atState(ArmConstants.ArmState targetState) { public boolean atTargetAngle(boolean isStateReversed) { if (isStateReversed) { - final double currentToTargetStateDifferenceDegrees = Math.abs(Rotation2d.fromDegrees(360).minus(targetState.targetAngle).minus(getAngle()).getDegrees()); + final double currentToTargetStateDifferenceDegrees = Math.abs(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle).minus(getAngle()).getDegrees()); return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); } return atTargetAngle(); @@ -113,7 +113,7 @@ void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) if (isStateReversed) { this.targetState = targetState; setTargetState( - Rotation2d.fromDegrees(360).minus(targetState.targetAngle) + ArmConstants.FULL_ROTATION.minus(targetState.targetAngle) , targetState.targetEndEffectorVoltage ); return; @@ -136,10 +136,10 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { } void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) { - this.isStateReversed = isStateReversed; if (isStateReversed) { + this.isStateReversed = isStateReversed; this.targetState = targetState; - setTargetAngle(Rotation2d.fromDegrees(360).minus(targetState.targetAngle)); + setTargetAngle(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle)); } prepareForState(targetState); } 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 5c1293cd..c7bcd58c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -48,7 +48,7 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState ); } - public static Command getPrepareStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { + public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { return new StartEndCommand( () -> RobotContainer.ARM.prepareForState(targetState, isStateReversed), RobotContainer.ARM::stop, @@ -56,7 +56,7 @@ public static Command getPrepareStateCommand(ArmConstants.ArmState targetState, ); } - public static Command getPrepareStateCommand(ArmConstants.ArmState targetState) { + public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState) { return new StartEndCommand( () -> RobotContainer.ARM.prepareForState(targetState), RobotContainer.ARM::stop, 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 9b4c6f80..0bdb08ef 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -120,6 +120,7 @@ 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(); From 265fc44860419ebcced2c5815fa5ede22c2677b1 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Wed, 10 Sep 2025 20:53:08 +0300 Subject: [PATCH 8/8] switched reversed state functions to get called by regular ones --- .../frc/trigon/robot/subsystems/arm/Arm.java | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 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 a8f0a3c9..1b14eb1a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -108,26 +108,26 @@ public boolean hasGamePiece() { return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } + void setTargetState(ArmConstants.ArmState targetState) { + this.isStateReversed = false; + this.targetState = targetState; + setTargetState(targetState, false); + } + void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) { this.isStateReversed = isStateReversed; + this.targetState = targetState; + if (isStateReversed) { - this.targetState = targetState; setTargetState( ArmConstants.FULL_ROTATION.minus(targetState.targetAngle) , targetState.targetEndEffectorVoltage ); return; } - setTargetState(targetState); - } - - void setTargetState(ArmConstants.ArmState targetState) { - this.isStateReversed = false; - this.targetState = targetState; setTargetState( targetState.targetAngle, - targetState.targetEndEffectorVoltage - ); + targetState.targetEndEffectorVoltage); } void setTargetState(Rotation2d targetAngle, double targetVoltage) { @@ -135,18 +135,18 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { setTargetVoltage(targetVoltage); } + void prepareForState(ArmConstants.ArmState targetState) { + prepareForState(targetState, false); + } + void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) { + this.isStateReversed = isStateReversed; + this.targetState = targetState; + if (isStateReversed) { - this.isStateReversed = isStateReversed; - this.targetState = targetState; setTargetAngle(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle)); + return; } - prepareForState(targetState); - } - - void prepareForState(ArmConstants.ArmState targetState) { - this.isStateReversed = false; - this.targetState = targetState; setTargetAngle(targetState.targetAngle); }