From a1714f6f62c358bff017ded3e9b963413b0c6e1c Mon Sep 17 00:00:00 2001 From: zoe Date: Thu, 21 Aug 2025 19:55:52 -0700 Subject: [PATCH 1/2] make arm swing forward in auto - added new constant for alternative l4 position (not interchangable w/ regular l4 position) -- the arm position is the same mod 2pi but it is positive - unclear if arm will slam into reef during auto... --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/RobotState.java | 1 + 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 775715f..fe4bb13 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -294,6 +294,8 @@ public static class RobotStateConstants { public static final double C4_ELEVATOR_POS = 0.7043; // from 3/1 public static final double C4_ARM_POS = -3.211; // from 3/1 + + public static final double C4_FORWARD_ARM_POS = 2*Math.PI + C4_ARM_POS; // swing arm forward public static final double C4_ELEVATOR_POS_WITH_CORAL = 0.7026; // from 3/1 public static final double C4_ARM_POS_WITH_CORAL = -3.2239; // from 3/1 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4b79b9f..31b31c1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -383,11 +383,11 @@ public RobotContainer() { NamedCommands.registerCommand("scoreL1Coral", createScoreCommand(m_lifter.moveToCommand(RobotState.L1_CORAL))); NamedCommands.registerCommand("scoreL2Coral", createScoreCommand(m_lifter.moveToCommand(RobotState.L2_CORAL))); NamedCommands.registerCommand("scoreL3Coral", createScoreCommand(m_lifter.moveToCommand(RobotState.L3_CORAL))); - NamedCommands.registerCommand("scoreL4Coral", createScoreCommand(m_lifter.moveToCommand(RobotState.L4_CORAL))); + NamedCommands.registerCommand("scoreL4Coral", createScoreCommand(m_lifter.moveToCommand(RobotState.L4_CORAL_FORWARD))); NamedCommands.registerCommand("shootCoral", new ParallelRaceGroup( new PIDToTargetCommand(drivetrain, () -> m_ppTargetPose), Commands.sequence( - (new WaitCommand(1).until(m_lifter.isLifterAtGoal(RobotState.L4_CORAL.getArmPos(), RobotState.L4_CORAL.getElevatorPos()))), + (new WaitCommand(1).until(m_lifter.isLifterAtGoal(RobotState.L4_CORAL_FORWARD.getArmPos(), RobotState.L4_CORAL_FORWARD.getElevatorPos()))), m_collarCommandFactory.runCollarOut().withTimeout(0.5).asProxy(), m_collar.runCollarOffInstant().asProxy()))); NamedCommands.registerCommand("waitUntilCoralIntaken", new WaitCommand(2).until(m_rampSensor.coralSeenAfterRampTrigger())); diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index f5ee2fa..d616147 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -9,6 +9,7 @@ public enum RobotState { L3_CORAL(RobotStateConstants.C3_ELEVATOR_POS, RobotStateConstants.C3_ARM_POS), L3_CORAL_WITH_CORAL(RobotStateConstants.C3_ELEVATOR_POS_WITH_CORAL, RobotStateConstants.C3_ARM_POS_WITH_CORAL), L4_CORAL(RobotStateConstants.C4_ELEVATOR_POS, RobotStateConstants.C4_ARM_POS), + L4_CORAL_FORWARD(RobotStateConstants.C4_ELEVATOR_POS, RobotStateConstants.C4_FORWARD_ARM_POS), // arm swinging forward :> L4_CORAL_WITH_CORAL(RobotStateConstants.C4_ELEVATOR_POS_WITH_CORAL, RobotStateConstants.C4_ARM_POS_WITH_CORAL), L2_ALGAE(RobotStateConstants.A2_ELEVATOR_POS, RobotStateConstants.A2_ARM_POS), L3_ALGAE(RobotStateConstants.A3_ELEVATOR_POS, RobotStateConstants.A3_ARM_POS), From 49f8b0fd7452a1c3339dd2f66d61d357f9f254b8 Mon Sep 17 00:00:00 2001 From: zoe Date: Thu, 21 Aug 2025 20:18:31 -0700 Subject: [PATCH 2/2] add optimized movement logic for L4 forward command - does NOT wait for elevator to be done to move the arm - DOES wait for arm to be in a safe position to move the elevator (I think this does not slow down regular usecase -- intake --> L4 foward) --- .../lifter/LifterCommandFactory.java | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/lifter/LifterCommandFactory.java b/src/main/java/frc/robot/subsystems/lifter/LifterCommandFactory.java index daea3e5..edf1a07 100644 --- a/src/main/java/frc/robot/subsystems/lifter/LifterCommandFactory.java +++ b/src/main/java/frc/robot/subsystems/lifter/LifterCommandFactory.java @@ -45,6 +45,25 @@ public Command moveToCommand(RobotState targetRobotState) { m_elevator.moveToCommand(targetRobotState.getElevatorPos()) ) ).until(isLifterAtGoal(targetRobotState.getArmPos(), targetRobotState.getElevatorPos())).withName("moveToL4Command"); + } else if (targetRobotState == RobotState.L4_CORAL_FORWARD) { + // moves arm and elevator in parallel for maximum speed + + // keep elevator at intermediate pos to let arm clear ramp + double elevatorIntermediatePosition = Math.max(targetRobotState.getElevatorPos(), + ElevatorConstants.MIN_POS_ARM_CLEAR); + + return Commands.parallel( + Commands.sequence( + m_arm.moveToCommand(targetRobotState.getArmPos()) + ), + Commands.sequence( + Commands.waitSeconds(3).until(m_arm.isArmInsideRobotTrigger()), + m_elevator.moveToCommand(elevatorIntermediatePosition) + .until(m_arm.okToMoveElevatorDownTrigger()), + m_elevator.moveToCommand(targetRobotState.getElevatorPos()) + ) + ).until(isLifterAtGoal(targetRobotState.getArmPos(), targetRobotState.getElevatorPos())).withName("moveToL4ForwardCommand"); + } else { // move arm to intermediate pos inside the robot before moving elevator double armIntermediatePosition = MathUtil.clamp(targetRobotState.getArmPos(),