Skip to content

ArmElevator #21

New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Merged
merged 7 commits into from
Sep 26, 2025
Merged

ArmElevator #21

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
8c94072
separated endeffector into separate subsystem
ShmayaR Sep 25, 2025
b9be7d8
combined elevator and arm
ShmayaR Sep 25, 2025
0a5a902
redid at state
Strflightmight09 Sep 26, 2025
0caead5
l1
Strflightmight09 Sep 26, 2025
11044d7
ftyfvghbjnk
Strflightmight09 Sep 26, 2025
28d9c7d
mergeeegegegegegegeg
Strflightmight09 Sep 26, 2025
ea49501
code rabbit stuff
ShmayaR Sep 26, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 9 additions & 9 deletions src/main/java/frc/trigon/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,14 +25,14 @@
import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler;
import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
import frc.trigon.robot.subsystems.MotorSubsystem;
import frc.trigon.robot.subsystems.arm.Arm;
import frc.trigon.robot.subsystems.arm.ArmCommands;
import frc.trigon.robot.subsystems.arm.ArmElevator;
import frc.trigon.robot.subsystems.arm.ArmElevatorCommands;
import frc.trigon.robot.subsystems.climber.Climber;
import frc.trigon.robot.subsystems.climber.ClimberCommands;
import frc.trigon.robot.subsystems.climber.ClimberConstants;
import frc.trigon.robot.subsystems.elevator.Elevator;
import frc.trigon.robot.subsystems.elevator.ElevatorCommands;
import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
import frc.trigon.robot.subsystems.endeffector.EndEffector;
import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands;
import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants;
import frc.trigon.robot.subsystems.intake.Intake;
import frc.trigon.robot.subsystems.intake.IntakeCommands;
import frc.trigon.robot.subsystems.intake.IntakeConstants;
Expand All @@ -52,9 +52,9 @@ public class RobotContainer {
CameraConstants.OBJECT_DETECTION_CAMERA
);
public static final Swerve SWERVE = new Swerve();
public static final Arm ARM = new Arm();
public static final ArmElevator ARM_ELEVATOR = new ArmElevator();
public static final Climber CLIMBER = new Climber();
public static final Elevator ELEVATOR = new Elevator();
public static final EndEffector END_EFFECTOR = new EndEffector();
public static final Intake INTAKE = new Intake();
public static final Transporter TRANSPORTER = new Transporter();

Expand All @@ -80,9 +80,9 @@ private void configureBindings() {

private void bindDefaultCommands() {
SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand());
ARM.setDefaultCommand(ArmCommands.getRestCommand());
ARM_ELEVATOR.setDefaultCommand(ArmElevatorCommands.getDefaultCommand());
CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST));
ELEVATOR.setDefaultCommand(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST));
END_EFFECTOR.setDefaultCommand(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.REST));
INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST));
TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST));
}
Expand Down
16 changes: 8 additions & 8 deletions src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,10 @@
import frc.trigon.robot.RobotContainer;
import frc.trigon.robot.constants.LEDConstants;
import frc.trigon.robot.constants.OperatorConstants;
import frc.trigon.robot.subsystems.arm.ArmCommands;
import frc.trigon.robot.subsystems.arm.ArmConstants;
import frc.trigon.robot.subsystems.elevator.ElevatorCommands;
import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
import frc.trigon.robot.subsystems.arm.ArmElevatorCommands;
import frc.trigon.robot.subsystems.arm.ArmElevatorConstants;
import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands;
import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants;
import frc.trigon.robot.subsystems.intake.IntakeCommands;
import frc.trigon.robot.subsystems.intake.IntakeConstants;
import frc.trigon.robot.subsystems.transporter.TransporterCommands;
Expand All @@ -29,11 +29,11 @@ public static Command getCoralCollectionCommand() {
// new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)
}

private static Command getLoadCoralCommand() {
public static Command getLoadCoralCommand() {
return new ParallelCommandGroup(
ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.LOAD_CORAL),
ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.LOAD_CORAL)
).until(RobotContainer.ARM::hasGamePiece).asProxy();
ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL),
EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.LOAD_CORAL)
).until(RobotContainer.END_EFFECTOR::hasGamePiece).onlyWhile(() -> !RobotContainer.END_EFFECTOR.hasGamePiece());
}

private static Command getIntakeSequenceCommand() {
Expand Down
14 changes: 8 additions & 6 deletions src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,10 @@
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.trigon.robot.RobotContainer;
import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler;
import frc.trigon.robot.subsystems.arm.ArmCommands;
import frc.trigon.robot.subsystems.arm.ArmConstants;
import frc.trigon.robot.subsystems.arm.ArmElevatorCommands;
import frc.trigon.robot.subsystems.arm.ArmElevatorConstants;
import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands;
import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants;
import frc.trigon.robot.subsystems.intake.IntakeCommands;
import frc.trigon.robot.subsystems.intake.IntakeConstants;
import frc.trigon.robot.subsystems.transporter.TransporterCommands;
Expand All @@ -16,7 +18,7 @@ public class CoralEjectionCommands {
public static Command getCoralEjectionCommand() {
return GeneralCommands.getContinuousConditionalCommand(
getEjectCoralFromIntakeCommand(),
getEjectCoralFromArmCommand(),
getEjectCoralFromEndEffectorCommand(),
() -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.INTAKE.hasCoral()
).onlyIf(SimulationFieldHandler::isHoldingCoral);
}
Expand All @@ -28,10 +30,10 @@ private static Command getEjectCoralFromIntakeCommand() {
);
}

private static Command getEjectCoralFromArmCommand() {
private static Command getEjectCoralFromEndEffectorCommand() {
return new SequentialCommandGroup(
ArmCommands.getPrepareForStateCommand(ArmConstants.ArmState.EJECT).until(RobotContainer.ARM::atPrepareAngle),
ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.EJECT)
ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.EJECT).until(RobotContainer.ARM_ELEVATOR::atTargetState),
EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.EJECT)
);
}
}
63 changes: 26 additions & 37 deletions src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,10 @@
import frc.trigon.robot.constants.FieldConstants;
import frc.trigon.robot.constants.OperatorConstants;
import frc.trigon.robot.misc.ReefChooser;
import frc.trigon.robot.subsystems.arm.ArmCommands;
import frc.trigon.robot.subsystems.arm.ArmConstants;
import frc.trigon.robot.subsystems.elevator.ElevatorCommands;
import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
import frc.trigon.robot.subsystems.arm.ArmElevatorCommands;
import frc.trigon.robot.subsystems.arm.ArmElevatorConstants;
import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands;
import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants;
import frc.trigon.robot.subsystems.swerve.SwerveCommands;
import lib.utilities.flippable.FlippablePose2d;
import lib.utilities.flippable.FlippableTranslation2d;
Expand All @@ -26,19 +26,22 @@ public class CoralPlacingCommands {
private static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER;

public static Command getScoreInReefCommand(boolean shouldScoreRight) {
return new ConditionalCommand(
getAutonomouslyScoreCommand(shouldScoreRight),
getScoreCommand(shouldScoreRight),
() -> SHOULD_SCORE_AUTONOMOUSLY
return new SequentialCommandGroup(
CoralCollectionCommands.getLoadCoralCommand(),
new ConditionalCommand(
getAutonomouslyScoreCommand(shouldScoreRight),
getScoreCommand(shouldScoreRight),
() -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1
)
);
}

private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) {
return new SequentialCommandGroup(
getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isArmAndElevatorAtPrepareState(shouldScoreRight)),
getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isReadyToScore(shouldScoreRight)),
new ParallelCommandGroup(
ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState),
ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore)
ArmElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore),
EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL)
)
);
}
Expand All @@ -47,16 +50,15 @@ private static Command getScoreCommand(boolean shouldScoreRight) {
return new SequentialCommandGroup(
getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER),
new ParallelCommandGroup(
ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState),
ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore)
ArmElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore),
EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL)
)
);
}

private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) {
return new ParallelCommandGroup(
ElevatorCommands.getPrepareStateCommand(REEF_CHOOSER::getElevatorState),
ArmCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore),
ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore),
getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight).asProxy()
);
}
Expand Down Expand Up @@ -106,9 +108,8 @@ public static FlippablePose2d calculateClosestScoringPose(boolean shouldScoreRig
return new FlippablePose2d(closestScoringPose.transformBy(scoringPoseToBranch), false);
}

private static boolean isArmAndElevatorAtPrepareState(boolean shouldScoreRight) {
return RobotContainer.ELEVATOR.atPreparedTargetState()
&& RobotContainer.ARM.atPrepareAngle()
private static boolean isReadyToScore(boolean shouldScoreRight) {
return RobotContainer.ARM_ELEVATOR.atState(REEF_CHOOSER.getArmElevatorState().prepareState, shouldReverseScore())
&& RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight));
}

Expand All @@ -134,8 +135,7 @@ public enum ScoringLevel {
L3(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0)),
L4(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0));

public final ElevatorConstants.ElevatorState elevatorState;
public final ArmConstants.ArmState armState;
public final ArmElevatorConstants.ArmElevatorState armElevatorState;
public final int level = calculateLevel();
final double xTransformMeters, positiveYTransformMeters;
final Rotation2d rotationTransform;
Expand All @@ -153,8 +153,7 @@ public enum ScoringLevel {
this.xTransformMeters = xTransformMeters;
this.positiveYTransformMeters = positiveYTransformMeters;
this.rotationTransform = rotationTransform;
this.elevatorState = determineElevatorState();
this.armState = determineArmState();
this.armElevatorState = determineArmElevatorState();
}

/**
Expand All @@ -176,22 +175,12 @@ public FlippablePose2d calculateTargetPlacingPosition(FieldConstants.ReefClockPo
return new FlippablePose2d(reefCenterPose.plus(transform), true);
}

private ElevatorConstants.ElevatorState determineElevatorState() {
return switch (level) {
case 1 -> ElevatorConstants.ElevatorState.SCORE_L1;
case 2 -> ElevatorConstants.ElevatorState.SCORE_L2;
case 3 -> ElevatorConstants.ElevatorState.SCORE_L3;
case 4 -> ElevatorConstants.ElevatorState.SCORE_L4;
default -> throw new IllegalStateException("Unexpected value: " + ordinal());
};
}

private ArmConstants.ArmState determineArmState() {
private ArmElevatorConstants.ArmElevatorState determineArmElevatorState() {
return switch (level) {
case 1 -> ArmConstants.ArmState.SCORE_L1;
case 2 -> ArmConstants.ArmState.SCORE_L2;
case 3 -> ArmConstants.ArmState.SCORE_L3;
case 4 -> ArmConstants.ArmState.SCORE_L4;
case 1 -> ArmElevatorConstants.ArmElevatorState.SCORE_L1;
case 2 -> ArmElevatorConstants.ArmElevatorState.SCORE_L2;
case 3 -> ArmElevatorConstants.ArmElevatorState.SCORE_L3;
case 4 -> ArmElevatorConstants.ArmElevatorState.SCORE_L4;
default -> throw new IllegalStateException("Unexpected value: " + ordinal());
};
}
Expand Down
11 changes: 3 additions & 8 deletions src/main/java/frc/trigon/robot/misc/ReefChooser.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,7 @@
import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands;
import frc.trigon.robot.constants.FieldConstants;
import frc.trigon.robot.constants.OperatorConstants;
import frc.trigon.robot.subsystems.elevator.ElevatorConstants;
import frc.trigon.robot.subsystems.arm.ArmConstants;
import frc.trigon.robot.subsystems.arm.ArmElevatorConstants;
import lib.utilities.flippable.FlippablePose2d;

import java.util.function.Supplier;
Expand Down Expand Up @@ -45,12 +44,8 @@ public FlippablePose2d calculateTargetScoringPose() {
return scoringLevel.calculateTargetPlacingPosition(clockPosition, reefSide);
}

public ArmConstants.ArmState getArmState() {
return scoringLevel.armState;
}

public ElevatorConstants.ElevatorState getElevatorState() {
return scoringLevel.elevatorState;
public ArmElevatorConstants.ArmElevatorState getArmElevatorState() {
return scoringLevel.armElevatorState;
}

private void configureBindings() {
Expand Down
20 changes: 10 additions & 10 deletions src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
import frc.trigon.robot.RobotContainer;
import frc.trigon.robot.subsystems.arm.ArmConstants;
import frc.trigon.robot.subsystems.arm.ArmElevatorConstants;
import frc.trigon.robot.subsystems.intake.IntakeConstants;
import frc.trigon.robot.subsystems.transporter.TransporterConstants;
import lib.utilities.flippable.FlippablePose3d;
Expand Down Expand Up @@ -78,7 +78,7 @@ private static void updateCollection() {
final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose());
final Pose3d
coralCollectionPose = robotPose.plus(toTransform(IntakeConstants.CORAL_COLLECTION_POSE)),
algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateGamePieceCollectionPose()));
algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.ARM_ELEVATOR.calculateGamePieceCollectionPose()));

if (isCollectingCoral() && HELD_CORAL_INDEX == null) {
HELD_CORAL_INDEX = getIndexOfCollectedGamePiece(coralCollectionPose, CORAL_ON_FIELD, SimulatedGamePieceConstants.CORAL_INTAKE_TOLERANCE_METERS);
Expand Down Expand Up @@ -124,20 +124,20 @@ private static boolean isCollectingCoral() {
}

private static boolean isCollectingAlgae() {
return RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L2) || RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L3);
return RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L2) || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L3);
}

private static boolean isCoralLoading() {
return RobotContainer.ARM.atState(ArmConstants.ArmState.LOAD_CORAL);
return RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL);
}

private static void updateEjection() {
if (HELD_CORAL_INDEX != null)
updateCoralEjection();

if (HELD_ALGAE_INDEX != null && RobotContainer.ARM.isEjecting()) {
if (HELD_ALGAE_INDEX != null && RobotContainer.END_EFFECTOR.isEjecting()) {
final SimulatedGamePiece heldAlgae = ALGAE_ON_FIELD.get(HELD_ALGAE_INDEX);
heldAlgae.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d());
heldAlgae.release(RobotContainer.END_EFFECTOR.calculateLinearEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d());
HELD_ALGAE_INDEX = null;
}
}
Expand All @@ -149,8 +149,8 @@ private static void updateCoralEjection() {
heldCoral.release(RobotContainer.INTAKE.calculateLinearIntakeVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), IntakeConstants.CORAL_COLLECTION_POSE.getTranslation());
HELD_CORAL_INDEX = null;
}
if (isCoralInEndEffector() && RobotContainer.ARM.isEjecting()) {
heldCoral.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), RobotContainer.ARM.calculateGamePieceCollectionPose().getTranslation());
if (isCoralInEndEffector() && RobotContainer.END_EFFECTOR.isEjecting()) {
heldCoral.release(RobotContainer.END_EFFECTOR.calculateLinearEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), RobotContainer.ARM_ELEVATOR.calculateGamePieceCollectionPose().getTranslation());
HELD_CORAL_INDEX = null;
}
}
Expand All @@ -160,8 +160,8 @@ private static void updateCoralEjection() {
*/
private static void updateHeldGamePiecePoses() {
final Pose3d
robotRelativeHeldCoralPosition = IS_CORAL_IN_END_EFFECTOR ? RobotContainer.ARM.calculateGamePieceCollectionPose() : TransporterConstants.COLLECTED_CORAL_POSE,
robotRelativeHeldAlgaePosition = RobotContainer.ARM.calculateGamePieceCollectionPose();
robotRelativeHeldCoralPosition = IS_CORAL_IN_END_EFFECTOR ? RobotContainer.ARM_ELEVATOR.calculateGamePieceCollectionPose() : TransporterConstants.COLLECTED_CORAL_POSE,
robotRelativeHeldAlgaePosition = RobotContainer.ARM_ELEVATOR.calculateGamePieceCollectionPose();
updateHeldGamePiecePose(robotRelativeHeldCoralPosition, CORAL_ON_FIELD, HELD_CORAL_INDEX);
updateHeldGamePiecePose(robotRelativeHeldAlgaePosition, ALGAE_ON_FIELD, HELD_ALGAE_INDEX);
}
Expand Down
Loading
Loading