From c25525f61229dbf40ed67bd98cbc08a8c3e35953 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 8 Apr 2025 12:43:23 -0700 Subject: [PATCH 1/4] Tune l2 extension in sim --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 5 ++++- src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java | 4 ++-- src/main/java/frc/robot/subsystems/Superstructure.java | 2 +- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index fb4190ea..c4cbefc3 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -38,7 +38,10 @@ public class ExtensionKinematics { new Pose2d(0.26, 0.4, Rotation2d.fromDegrees(15.0)); // solveFK(L1_EXTENSION); public static final ExtensionState L1_EXTENSION = solveIK(L1_POSE); public static final ExtensionState L2_EXTENSION = - new ExtensionState(0.33, Rotation2d.fromRadians(0.569), Rotation2d.fromRadians(2.447)); + new ExtensionState( + 0.23, + Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(15)), + Rotation2d.fromRadians(2.447)); public static final Pose2d L2_POSE = solveFK(L2_EXTENSION); public static final ExtensionState L3_EXTENSION = new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); diff --git a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java index 664b90d3..83ad4796 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorSubsystem.java @@ -78,12 +78,12 @@ public void periodic() { if (Robot.ROBOT_TYPE != RobotType.REAL) Logger.recordOutput(NAME + "/Filtered Current", currentFilterValue); - if (firstBBInputs.get && !secondBBInputs.get) { + if (getFirstBeambreak() && !getSecondBeambreak()) { Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(0.0)); zeroTimer.reset(); } - if (!firstBBInputs.get && secondBBInputs.get) { + if (!getFirstBeambreak() && getSecondBeambreak()) { // Number calculated from coral length, may need tuning Tracer.trace("Manipulator/Zero", () -> io.resetEncoder(1.0)); zeroTimer.reset(); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 5551b756..bc83ce5e 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -267,7 +267,7 @@ private void configureStateTransitionCommands() { .get(SuperState.IDLE) .and(() -> !elevator.hasZeroed || !wrist.hasZeroed) .and(() -> DriverStation.isEnabled()) - .and(() -> Robot.ROBOT_TYPE != RobotType.SIM) + // .and(() -> Robot.ROBOT_TYPE != RobotType.SIM) .onTrue(this.forceState(SuperState.HOME)); // We might want to make this work when we have a piece as well? From 037537aec04de4482700159a38145a198ed138d3 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 8 Apr 2025 12:50:40 -0700 Subject: [PATCH 2/4] l3 in sim --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index c4cbefc3..4787e760 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -44,7 +44,7 @@ public class ExtensionKinematics { Rotation2d.fromRadians(2.447)); public static final Pose2d L2_POSE = solveFK(L2_EXTENSION); public static final ExtensionState L3_EXTENSION = - new ExtensionState(0.68, Rotation2d.fromRadians(1.022), Rotation2d.fromRadians(2.427)); + new ExtensionState(0.60, Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(8)), Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = new Pose2d(new Translation2d(0.25, 2.05), Rotation2d.fromDegrees(110.0)); From a2d73dc5318431eaf9b90ce9bacf4770b1516749 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 8 Apr 2025 13:09:50 -0700 Subject: [PATCH 3/4] Tune irl --- .../java/frc/robot/subsystems/ExtensionKinematics.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 4787e760..224d94af 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -39,12 +39,15 @@ public class ExtensionKinematics { public static final ExtensionState L1_EXTENSION = solveIK(L1_POSE); public static final ExtensionState L2_EXTENSION = new ExtensionState( - 0.23, - Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(15)), + 0.23 + Units.inchesToMeters(1.5), + Rotation2d.fromRadians(0.569).plus(Rotation2d.fromDegrees(20)), Rotation2d.fromRadians(2.447)); public static final Pose2d L2_POSE = solveFK(L2_EXTENSION); public static final ExtensionState L3_EXTENSION = - new ExtensionState(0.60, Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(8)), Rotation2d.fromRadians(2.427)); + new ExtensionState( + 0.60 + Units.inchesToMeters(2.0), + Rotation2d.fromRadians(1.022).minus(Rotation2d.fromDegrees(3)), + Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = new Pose2d(new Translation2d(0.25, 2.05), Rotation2d.fromDegrees(110.0)); From 57aa409a1ec48ff26b34da6beed8779961bd6bf6 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Tue, 8 Apr 2025 13:51:45 -0700 Subject: [PATCH 4/4] Change l4 target (needs testing with improved clearence extension --- src/main/java/frc/robot/subsystems/ExtensionKinematics.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java index 224d94af..8329ced4 100644 --- a/src/main/java/frc/robot/subsystems/ExtensionKinematics.java +++ b/src/main/java/frc/robot/subsystems/ExtensionKinematics.java @@ -50,7 +50,7 @@ public class ExtensionKinematics { Rotation2d.fromRadians(2.427)); public static final Pose2d L3_POSE = solveFK(L3_EXTENSION); public static final Pose2d L4_POSE = - new Pose2d(new Translation2d(0.25, 2.05), Rotation2d.fromDegrees(110.0)); + new Pose2d(new Translation2d(0.23, 2.05), Rotation2d.fromDegrees(110.0)); public static final ExtensionState L4_EXTENSION = solveIK(L4_POSE); public static final ExtensionState LOW_ALGAE_EXTENSION =