From 9389a50918e8ae50fa5f51e9c1202bc2cbf00adf Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Sat, 10 May 2025 15:14:02 -0400 Subject: [PATCH 01/25] l4 opposite triple got tha local --- .../autos/L4 Opposite - Triple.auto | 69 +++++++++++++++++-- 1 file changed, 63 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto index 7b69f7e7..e85d8e9f 100644 --- a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto +++ b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto @@ -15,14 +15,33 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Opposite 1 - Triple" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Opposite 1 - Triple" + } + } + ] } } ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -65,9 +84,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Opposite 3 - Triple" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Opposite 3 - Triple" + } + } + ] } }, { @@ -79,6 +111,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -121,9 +159,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Opposite 5 - Triple" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Opposite 5 - Triple" + } + } + ] } }, { @@ -135,6 +186,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { From f0e1326ad3db7b51cac3a70de6faaeb74a893036 Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Sat, 10 May 2025 16:55:13 -0400 Subject: [PATCH 02/25] there is a method and i need to checkout for prime testing --- src/main/java/frc/robot/RobotContainer.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ce6ba4a..ab0bc36c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1030,6 +1030,12 @@ public void generateNamedCommands() { .alongWith(new InstantCommand(() -> PathplannerWeight.setSpeeds(new ChassisSpeeds())))); NamedCommands.registerCommand( "WaitForLocal", - new WaitCommand(0.1).andThen(new WaitUntilCommand(() -> localAlign.isReady()))); + new WaitCommand(0.1) + .andThen(new WaitUntilCommand(() -> localAlign.isReady())) + .andThen(new InstantCommand(() -> beans()))); + } + + public void beans() { + System.out.println("beans"); } } From 37611683bc715c3c87220b61768f7dbca12c1ee6 Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Sat, 17 May 2025 15:11:27 -0400 Subject: [PATCH 03/25] crash control --- .../autos/L4 Opposite - Triple.auto | 25 ++++------ .../paths/L4 Opposite 1 - Triple.path | 46 +------------------ .../pathplanner/paths/L4 Processor 3.path | 8 ++-- .../paths/L4 Processor 5 - Double.path | 8 ++-- src/main/java/frc/robot/RobotContainer.java | 6 +-- .../drive/weights/LocalTagAlignWeight.java | 6 +++ 6 files changed, 27 insertions(+), 72 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto index e85d8e9f..dec081be 100644 --- a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto +++ b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto @@ -11,26 +11,19 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "WaitForLocal" - } - }, - { - "type": "path", - "data": { - "pathName": "L4 Opposite 1 - Triple" - } - } - ] + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Opposite 1 - Triple" } } ] diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path b/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path index 0e07ab9c..840ab6c8 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path @@ -45,51 +45,7 @@ } ], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "L4Left", - "waypointRelativePos": 0.29, - "endWaypointRelativePos": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "SetModeCoral" - } - }, - { - "type": "named", - "data": { - "name": "SetGantryLeft" - } - }, - { - "type": "named", - "data": { - "name": "SetupL4" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "RunToPreset" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 2.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 3.path b/src/main/deploy/pathplanner/paths/L4 Processor 3.path index 6277fd6f..83f8bbb4 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 3.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4963990066225168, - "y": 0.8723923841059593 + "x": 1.525653409090909, + "y": 0.9138636363636362 }, "prevControl": null, "nextControl": { - "x": 2.76589620503102, - "y": 1.9878123911389562 + "x": 2.795150607499412, + "y": 2.0292836433966333 }, "isLocked": false, "linkedName": "BottomSource" diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 5 - Double.path b/src/main/deploy/pathplanner/paths/L4 Processor 5 - Double.path index 3c2da219..554966b1 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 5 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 5 - Double.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4963990066225168, - "y": 0.8723923841059593 + "x": 1.525653409090909, + "y": 0.9138636363636362 }, "prevControl": null, "nextControl": { - "x": 2.162950764991328, - "y": 1.5786424526363922 + "x": 2.1922051674597203, + "y": 1.6201137048940693 }, "isLocked": false, "linkedName": "BottomSource" diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ab0bc36c..1aac654c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -869,7 +869,7 @@ public Command getAutonomousCommand() { return homing() .andThen(new InstantCommand(() -> autoRampPos = true)) .andThen(new InstantCommand(() -> premoveLift = true)) - .andThen(dashboard.getAutoChooserCommand()); + .andThen(dashboard.getAutoChooserCommand().finallyDo(() -> beans())); // return new InstantCommand(); } @@ -1031,11 +1031,11 @@ public void generateNamedCommands() { NamedCommands.registerCommand( "WaitForLocal", new WaitCommand(0.1) - .andThen(new WaitUntilCommand(() -> localAlign.isReady())) - .andThen(new InstantCommand(() -> beans()))); + .andThen(new WaitUntilCommand(() -> localAlign.isReady()).finallyDo(() -> beans()))); } public void beans() { + System.out.println("BeforeBEnassn"); System.out.println("beans"); } } diff --git a/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java b/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java index 65f25618..7d909425 100644 --- a/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java +++ b/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java @@ -107,10 +107,16 @@ public boolean isReady() { public Command getAutoCommand() { return driveCommandFactory .runVelocityCommand(() -> getSpeeds(), () -> true) + .andThen(() -> beans()) .finallyDo( () -> driveCommandFactory.runVelocityCommand(() -> new ChassisSpeeds(), () -> true)); } + public void beans() { + System.out.println("BeforeBEnassn"); + System.out.println("beans"); + } + public boolean isAutoalignComplete() { Optional vector = AprilTagAlignHelper.getAverageLocalAlignVector(getTargetTagId(), visions); From 90a6c57625794e8b8ee128591182d40f930b5932 Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Sat, 17 May 2025 18:15:59 -0400 Subject: [PATCH 04/25] code tweaks and also me tweaks (im tweaking waht) --- .../autos/L4 Opposite - Triple.auto | 18 ------- src/main/java/frc/robot/RobotContainer.java | 18 +++++-- .../drive/weights/LocalTagAlignWeight.java | 49 +++++++++++++++++-- 3 files changed, 59 insertions(+), 26 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto index dec081be..3d84859b 100644 --- a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto +++ b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto @@ -4,22 +4,10 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "EnableAprilTags" - } - }, { "type": "deadline", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "WaitForLocal" - } - }, { "type": "path", "data": { @@ -29,12 +17,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "LocalAlign" - } - }, { "type": "named", "data": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1aac654c..f054f68e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -959,7 +959,9 @@ public Command runLiftToSafe() { public void generateNamedCommands() { NamedCommands.registerCommand( - "EnableAprilTags", new InstantCommand(() -> RobotOdometry.instance.setAutoApriltags(true))); + "EnableAprilTags", + new InstantCommand(() -> System.out.println("before all of it")) + .andThen(() -> RobotOdometry.instance.setAutoApriltags(true))); NamedCommands.registerCommand( "DisableAprilTags", new InstantCommand(() -> RobotOdometry.instance.setAutoApriltags(false))); @@ -1023,8 +1025,8 @@ public void generateNamedCommands() { NamedCommands.registerCommand( "LocalAlign", - localAlign - .getAutoCommand() + new InstantCommand(() -> System.out.println("before local cluster")) + .andThen(localAlign.getAutoCommand()) .deadlineFor(autonAutoPlace(() -> coralPreset)) .until(() -> localAlign.isAutoalignComplete() || !localAlign.isReady()) .alongWith(new InstantCommand(() -> PathplannerWeight.setSpeeds(new ChassisSpeeds())))); @@ -1038,4 +1040,14 @@ public void beans() { System.out.println("BeforeBEnassn"); System.out.println("beans"); } + + // public void isAuto() { + // return Robot.getState() = RobotState.AUTONOMOUS; + // } + + public void isTrue() { + if (localAlign.isReady()) { + System.out.println("visible!"); + } + } } diff --git a/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java b/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java index 7d909425..e2c9f3aa 100644 --- a/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java +++ b/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.constants.FieldConstants; import frc.robot.sensors.apriltag.AprilTagVision; import frc.robot.sensors.gyro.Gyro; @@ -84,7 +85,8 @@ public boolean isReady() { AprilTagAlignHelper.getAverageLocalAlignVector(getTargetTagId(), visions); boolean ready = false; if (vector.isPresent()) { - double goalAngle = + double goalAngle = // a break point triggers here as soon as the robot is connected -- as in, + // when disabled, not just auto! FieldConstants.aprilTagLayout .getTagPose(getTargetTagId()) .get() @@ -97,17 +99,54 @@ public boolean isReady() { "angledelta", Math.abs((robotRotation.get().minus(new Rotation2d(goalAngle))).getDegrees())); ready = - vector.get().getNorm() < 1 + vector.get().getNorm() + < 1 // and then ready is still false because the vector to the reef is still + // greater than one && Math.abs((robotRotation.get().minus(new Rotation2d(goalAngle))).getDegrees()) < 15; + } else { + System.out.println("no lo veo"); } Logger.recordOutput("LocalTagAlign/isAlignReady", ready); + Logger.recordOutput( + "LocalTagAlign/vectorNorm", vector.isPresent() ? vector.get().getNorm() : -1); return ready; } + // public boolean isReady() { + // Optional vector = + // AprilTagAlignHelper.getAverageLocalAlignVector(getTargetTagId(), visions); + // boolean ready = false; + // if (Robot.getState() == RobotState.AUTONOMOUS) { + // if (vector.isPresent()) { + // double goalAngle = + // FieldConstants.aprilTagLayout + // .getTagPose(getTargetTagId()) + // .get() + // .toPose2d() + // .getRotation() + // .plus(Rotation2d.k180deg) + // .getRadians(); + + // Logger.recordOutput( + // "angledelta", + // Math.abs((robotRotation.get().minus(new Rotation2d(goalAngle))).getDegrees())); + // ready = + // vector.get().getNorm() < 1 + // && Math.abs((robotRotation.get().minus(new Rotation2d(goalAngle))).getDegrees()) + // < 15; + // } + + // } else { + // ready = false; + // } + // Logger.recordOutput("LocalTagAlign/isAlignReady", ready); + // return ready; + // } + public Command getAutoCommand() { - return driveCommandFactory - .runVelocityCommand(() -> getSpeeds(), () -> true) - .andThen(() -> beans()) + return new InstantCommand(() -> System.out.println("Before runVelocityCommand")) + .andThen(driveCommandFactory.runVelocityCommand(() -> getSpeeds(), () -> true)) + .andThen(() -> System.out.println("After runVelocityCommand")) .finallyDo( () -> driveCommandFactory.runVelocityCommand(() -> new ChassisSpeeds(), () -> true)); } From ce9490fc64f8a7781b40777c65773329d9bc1f7d Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Sat, 17 May 2025 18:32:16 -0400 Subject: [PATCH 05/25] if theres no local it goes in a circle. if there is it dont movee --- .../autos/L4 Opposite - Triple.auto | 42 ++++++++----------- 1 file changed, 18 insertions(+), 24 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto index 3d84859b..56148012 100644 --- a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto +++ b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto @@ -4,10 +4,22 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "EnableAprilTags" + } + }, { "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -17,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -62,12 +80,6 @@ "type": "deadline", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "WaitForLocal" - } - }, { "type": "path", "data": { @@ -86,12 +98,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "LocalAlign" - } - }, { "type": "named", "data": { @@ -137,12 +143,6 @@ "type": "deadline", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "WaitForLocal" - } - }, { "type": "path", "data": { @@ -161,12 +161,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "LocalAlign" - } - }, { "type": "named", "data": { From 76b45d77fdc692f2b9ac1c3438ddde5f03c72bd6 Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Sat, 17 May 2025 18:45:18 -0400 Subject: [PATCH 06/25] Fc = mv^2 / r --- src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto index 56148012..2db79c76 100644 --- a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto +++ b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto @@ -29,12 +29,6 @@ ] } }, - { - "type": "named", - "data": { - "name": "LocalAlign" - } - }, { "type": "named", "data": { From ccbbd16dddc44079c6f8215a8c26b7318294a9ad Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Wed, 21 May 2025 18:40:41 -0400 Subject: [PATCH 07/25] the purge --- src/main/java/frc/robot/RobotContainer.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f054f68e..cd3bbc20 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1041,10 +1041,6 @@ public void beans() { System.out.println("beans"); } - // public void isAuto() { - // return Robot.getState() = RobotState.AUTONOMOUS; - // } - public void isTrue() { if (localAlign.isReady()) { System.out.println("visible!"); From fec98081760fa00f98a26933f9aa9cabc7f26144 Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Wed, 21 May 2025 18:51:33 -0400 Subject: [PATCH 08/25] mr clean --- src/main/java/frc/robot/RobotContainer.java | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd3bbc20..29bcb007 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1040,10 +1040,4 @@ public void beans() { System.out.println("BeforeBEnassn"); System.out.println("beans"); } - - public void isTrue() { - if (localAlign.isReady()) { - System.out.println("visible!"); - } - } } From 7b45f442b66b3c1e4d6c95f863d1a54c8e15d07e Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Wed, 21 May 2025 19:23:18 -0400 Subject: [PATCH 09/25] IT WORKSSSSSS --- .../autos/L4 Opposite - Triple.auto | 6 ++++ src/main/java/frc/robot/RobotContainer.java | 19 ++++++----- .../drive/weights/LocalTagAlignWeight.java | 33 ------------------- 3 files changed, 17 insertions(+), 41 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto index 2db79c76..56148012 100644 --- a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto +++ b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto @@ -29,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 29bcb007..02cdbe22 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -166,6 +166,8 @@ public class RobotContainer { boolean homed = false; + boolean autoLocal = false; + public RobotContainer() { switch (Robot.getMode()) { @@ -421,6 +423,8 @@ public void periodic() { Logger.recordOutput("WeightSize", DriveWeightCommand.getWeightsSize()); + Logger.recordOutput("AutonLocalaligned", autoLocal); + Logger.recordOutput( "DistFromTarget", RobotOdometry.instance @@ -869,7 +873,7 @@ public Command getAutonomousCommand() { return homing() .andThen(new InstantCommand(() -> autoRampPos = true)) .andThen(new InstantCommand(() -> premoveLift = true)) - .andThen(dashboard.getAutoChooserCommand().finallyDo(() -> beans())); + .andThen(dashboard.getAutoChooserCommand()); // return new InstantCommand(); } @@ -1025,19 +1029,18 @@ public void generateNamedCommands() { NamedCommands.registerCommand( "LocalAlign", - new InstantCommand(() -> System.out.println("before local cluster")) - .andThen(localAlign.getAutoCommand()) + new InstantCommand(() -> autoLocal()) + .andThen(() -> localAlign.getAutoCommand()) .deadlineFor(autonAutoPlace(() -> coralPreset)) .until(() -> localAlign.isAutoalignComplete() || !localAlign.isReady()) .alongWith(new InstantCommand(() -> PathplannerWeight.setSpeeds(new ChassisSpeeds())))); NamedCommands.registerCommand( "WaitForLocal", - new WaitCommand(0.1) - .andThen(new WaitUntilCommand(() -> localAlign.isReady()).finallyDo(() -> beans()))); + new WaitCommand(0.1).andThen(new WaitUntilCommand(() -> localAlign.isReady()))); } - public void beans() { - System.out.println("BeforeBEnassn"); - System.out.println("beans"); + public boolean autoLocal() { + autoLocal = true; + return autoLocal; } } diff --git a/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java b/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java index e2c9f3aa..9228e277 100644 --- a/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java +++ b/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java @@ -103,8 +103,6 @@ public boolean isReady() { < 1 // and then ready is still false because the vector to the reef is still // greater than one && Math.abs((robotRotation.get().minus(new Rotation2d(goalAngle))).getDegrees()) < 15; - } else { - System.out.println("no lo veo"); } Logger.recordOutput("LocalTagAlign/isAlignReady", ready); Logger.recordOutput( @@ -112,37 +110,6 @@ public boolean isReady() { return ready; } - // public boolean isReady() { - // Optional vector = - // AprilTagAlignHelper.getAverageLocalAlignVector(getTargetTagId(), visions); - // boolean ready = false; - // if (Robot.getState() == RobotState.AUTONOMOUS) { - // if (vector.isPresent()) { - // double goalAngle = - // FieldConstants.aprilTagLayout - // .getTagPose(getTargetTagId()) - // .get() - // .toPose2d() - // .getRotation() - // .plus(Rotation2d.k180deg) - // .getRadians(); - - // Logger.recordOutput( - // "angledelta", - // Math.abs((robotRotation.get().minus(new Rotation2d(goalAngle))).getDegrees())); - // ready = - // vector.get().getNorm() < 1 - // && Math.abs((robotRotation.get().minus(new Rotation2d(goalAngle))).getDegrees()) - // < 15; - // } - - // } else { - // ready = false; - // } - // Logger.recordOutput("LocalTagAlign/isAlignReady", ready); - // return ready; - // } - public Command getAutoCommand() { return new InstantCommand(() -> System.out.println("Before runVelocityCommand")) .andThen(driveCommandFactory.runVelocityCommand(() -> getSpeeds(), () -> true)) From 47caf26e26f173cd4217084d33795715af746fb0 Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Wed, 21 May 2025 19:30:45 -0400 Subject: [PATCH 10/25] goodbye evil beans messaging --- .../autos/L4 Opposite - Triple.auto | 24 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 4 +--- .../drive/weights/LocalTagAlignWeight.java | 17 ++++--------- 3 files changed, 29 insertions(+), 16 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto index 56148012..dec081be 100644 --- a/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto +++ b/src/main/deploy/pathplanner/autos/L4 Opposite - Triple.auto @@ -80,6 +80,12 @@ "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -98,6 +104,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -143,6 +155,12 @@ "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -161,6 +179,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 02cdbe22..c4dbd67e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -963,9 +963,7 @@ public Command runLiftToSafe() { public void generateNamedCommands() { NamedCommands.registerCommand( - "EnableAprilTags", - new InstantCommand(() -> System.out.println("before all of it")) - .andThen(() -> RobotOdometry.instance.setAutoApriltags(true))); + "EnableAprilTags", new InstantCommand(() -> RobotOdometry.instance.setAutoApriltags(true))); NamedCommands.registerCommand( "DisableAprilTags", new InstantCommand(() -> RobotOdometry.instance.setAutoApriltags(false))); diff --git a/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java b/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java index 9228e277..3eee0691 100644 --- a/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java +++ b/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java @@ -85,8 +85,7 @@ public boolean isReady() { AprilTagAlignHelper.getAverageLocalAlignVector(getTargetTagId(), visions); boolean ready = false; if (vector.isPresent()) { - double goalAngle = // a break point triggers here as soon as the robot is connected -- as in, - // when disabled, not just auto! + double goalAngle = FieldConstants.aprilTagLayout .getTagPose(getTargetTagId()) .get() @@ -99,9 +98,7 @@ public boolean isReady() { "angledelta", Math.abs((robotRotation.get().minus(new Rotation2d(goalAngle))).getDegrees())); ready = - vector.get().getNorm() - < 1 // and then ready is still false because the vector to the reef is still - // greater than one + vector.get().getNorm() < 1 && Math.abs((robotRotation.get().minus(new Rotation2d(goalAngle))).getDegrees()) < 15; } Logger.recordOutput("LocalTagAlign/isAlignReady", ready); @@ -111,18 +108,12 @@ public boolean isReady() { } public Command getAutoCommand() { - return new InstantCommand(() -> System.out.println("Before runVelocityCommand")) - .andThen(driveCommandFactory.runVelocityCommand(() -> getSpeeds(), () -> true)) - .andThen(() -> System.out.println("After runVelocityCommand")) + return new InstantCommand( + () -> driveCommandFactory.runVelocityCommand(() -> getSpeeds(), () -> true)) .finallyDo( () -> driveCommandFactory.runVelocityCommand(() -> new ChassisSpeeds(), () -> true)); } - public void beans() { - System.out.println("BeforeBEnassn"); - System.out.println("beans"); - } - public boolean isAutoalignComplete() { Optional vector = AprilTagAlignHelper.getAverageLocalAlignVector(getTargetTagId(), visions); From ed1a3cfbc2026420acc85a9240ce5589746fa2ed Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Wed, 21 May 2025 19:34:48 -0400 Subject: [PATCH 11/25] kartik spree --- .../deploy/pathplanner/autos/3.5 piece.auto | 60 +++++++++++++++++-- 1 file changed, 55 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/3.5 piece.auto b/src/main/deploy/pathplanner/autos/3.5 piece.auto index 57b3d3e2..3cd2d460 100644 --- a/src/main/deploy/pathplanner/autos/3.5 piece.auto +++ b/src/main/deploy/pathplanner/autos/3.5 piece.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -65,9 +77,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "4 piece 3" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece 3" + } + } + ] } }, { @@ -79,6 +104,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -121,9 +152,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "4 piece 5" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "4 piece 5" + } + } + ] } }, { @@ -135,6 +179,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { From 740e75eabbebe0864ace2f1d4c776fe6639b49e0 Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Wed, 21 May 2025 19:42:41 -0400 Subject: [PATCH 12/25] bupdates --- .../autos/Explode the Processor.auto | 28 ++++++++- .../autos/L4 Opposite - Double.auto | 60 +++++++++++++++++-- 2 files changed, 81 insertions(+), 7 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Explode the Processor.auto b/src/main/deploy/pathplanner/autos/Explode the Processor.auto index 76e2ab8e..30697443 100644 --- a/src/main/deploy/pathplanner/autos/Explode the Processor.auto +++ b/src/main/deploy/pathplanner/autos/Explode the Processor.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -105,9 +117,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -117,6 +135,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "sequential", "data": { diff --git a/src/main/deploy/pathplanner/autos/L4 Opposite - Double.auto b/src/main/deploy/pathplanner/autos/L4 Opposite - Double.auto index e31fed76..0fad98d7 100644 --- a/src/main/deploy/pathplanner/autos/L4 Opposite - Double.auto +++ b/src/main/deploy/pathplanner/autos/L4 Opposite - Double.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -65,9 +77,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Opposite 3 - Double" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Opposite 3 - Double" + } + } + ] } }, { @@ -79,6 +104,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -121,9 +152,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Opposite 5- Double" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Opposite 5- Double" + } + } + ] } }, { @@ -135,6 +179,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { From ec34804cf357e50bea1a4ce7d11a52a945a6a30f Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Wed, 21 May 2025 19:54:00 -0400 Subject: [PATCH 13/25] several --- .../autos/L4 Processor - Double.auto | 60 +++++++++++++++++-- .../autos/L4 Processor - Triple.auto | 60 +++++++++++++++++-- .../deploy/pathplanner/autos/Process.auto | 14 ++++- 3 files changed, 123 insertions(+), 11 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/L4 Processor - Double.auto b/src/main/deploy/pathplanner/autos/L4 Processor - Double.auto index 658b3822..ba2aea08 100644 --- a/src/main/deploy/pathplanner/autos/L4 Processor - Double.auto +++ b/src/main/deploy/pathplanner/autos/L4 Processor - Double.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -65,9 +77,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Processor 3 - Double" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Processor 3 - Double" + } + } + ] } }, { @@ -79,6 +104,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -127,14 +158,33 @@ } }, { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Processor 5 - Double" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Processor 5 - Double" + } + } + ] } } ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/L4 Processor - Triple.auto b/src/main/deploy/pathplanner/autos/L4 Processor - Triple.auto index b10c7323..af339687 100644 --- a/src/main/deploy/pathplanner/autos/L4 Processor - Triple.auto +++ b/src/main/deploy/pathplanner/autos/L4 Processor - Triple.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -65,9 +77,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Processor 3" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Processor 3" + } + } + ] } }, { @@ -79,6 +104,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -127,14 +158,33 @@ } }, { - "type": "path", + "type": "deadline", "data": { - "pathName": "L4 Processor 5" + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "L4 Processor 5" + } + } + ] } } ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/Process.auto b/src/main/deploy/pathplanner/autos/Process.auto index 2139c178..9c870526 100644 --- a/src/main/deploy/pathplanner/autos/Process.auto +++ b/src/main/deploy/pathplanner/autos/Process.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { From f070b1a23487f81abdcf25d1455c9f71b3bc9903 Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Wed, 21 May 2025 19:55:53 -0400 Subject: [PATCH 14/25] last one! --- .../deploy/pathplanner/autos/SpitSpit.auto | 37 +++++++++++++++++-- 1 file changed, 34 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/SpitSpit.auto b/src/main/deploy/pathplanner/autos/SpitSpit.auto index 1b7d1ae6..a39d66f8 100644 --- a/src/main/deploy/pathplanner/autos/SpitSpit.auto +++ b/src/main/deploy/pathplanner/autos/SpitSpit.auto @@ -11,9 +11,15 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, { "type": "path", "data": { @@ -23,6 +29,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + }, { "type": "named", "data": { @@ -99,9 +111,28 @@ } }, { - "type": "path", + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "SpitProcess 3" + } + } + ] + } + }, + { + "type": "named", "data": { - "pathName": "SpitProcess 3" + "name": "LocalAlign" } }, { From 4ca6fb845b02e962ddcba6956ff031081666e3b3 Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Fri, 23 May 2025 22:28:27 -0400 Subject: [PATCH 15/25] will work for local --- .../autos/local align or else.auto | 44 +++++++++++++++ .../pathplanner/paths/LOCAL TO THE MOON.path | 54 +++++++++++++++++++ 2 files changed, 98 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/local align or else.auto create mode 100644 src/main/deploy/pathplanner/paths/LOCAL TO THE MOON.path diff --git a/src/main/deploy/pathplanner/autos/local align or else.auto b/src/main/deploy/pathplanner/autos/local align or else.auto new file mode 100644 index 00000000..83ea6729 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/local align or else.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "EnableAprilTags" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "WaitForLocal" + } + }, + { + "type": "path", + "data": { + "pathName": "LOCAL TO THE MOON" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "LocalAlign" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LOCAL TO THE MOON.path b/src/main/deploy/pathplanner/paths/LOCAL TO THE MOON.path new file mode 100644 index 00000000..3253b582 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LOCAL TO THE MOON.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.219, + "y": 3.905 + }, + "prevControl": null, + "nextControl": { + "x": 6.400941780827734, + "y": 3.9123073630131553 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.654, + "y": 4.005 + }, + "prevControl": { + "x": 6.275475461382252, + "y": 3.9176573199102847 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.75, + "maxAcceleration": 1.2, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file From 06f77a602ca07ef2939f14a256cdeb6833a3c24b Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Sat, 24 May 2025 13:39:24 -0400 Subject: [PATCH 16/25] why did it vanish? --- .../paths/L4 Opposite 1 - Triple.path | 46 ++++++++++++++++++- 1 file changed, 45 insertions(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path b/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path index 840ab6c8..0e07ab9c 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path @@ -45,7 +45,51 @@ } ], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "L4Left", + "waypointRelativePos": 0.29, + "endWaypointRelativePos": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SetModeCoral" + } + }, + { + "type": "named", + "data": { + "name": "SetGantryLeft" + } + }, + { + "type": "named", + "data": { + "name": "SetupL4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "RunToPreset" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 2.0, From 86e5a82fb929538da99155097afd9c9c30c2d6b6 Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Sat, 24 May 2025 14:20:51 -0400 Subject: [PATCH 17/25] aaaaagh --- src/main/deploy/pathplanner/paths/LOCAL TO THE MOON.path | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/LOCAL TO THE MOON.path b/src/main/deploy/pathplanner/paths/LOCAL TO THE MOON.path index 3253b582..79be5cc7 100644 --- a/src/main/deploy/pathplanner/paths/LOCAL TO THE MOON.path +++ b/src/main/deploy/pathplanner/paths/LOCAL TO THE MOON.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 0.75, + "maxVelocity": 0.3, "maxAcceleration": 1.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": false } \ No newline at end of file From dff94d51f62958ac48a65ae241db00c9f5b8f1cd Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Sat, 24 May 2025 15:51:48 -0400 Subject: [PATCH 18/25] TEAMWORK --- src/main/java/frc/robot/RobotContainer.java | 9 +++++---- .../subsystems/drive/weights/LocalTagAlignWeight.java | 6 ++++-- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c4dbd67e..635d5da5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.ProxyCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -1027,10 +1028,10 @@ public void generateNamedCommands() { NamedCommands.registerCommand( "LocalAlign", - new InstantCommand(() -> autoLocal()) - .andThen(() -> localAlign.getAutoCommand()) - .deadlineFor(autonAutoPlace(() -> coralPreset)) - .until(() -> localAlign.isAutoalignComplete() || !localAlign.isReady()) + (new ProxyCommand(localAlign::getAutoCommand)) // it just explodes as soon as this happens? + .deadlineFor(autonAutoPlace(() -> coralPreset)) // this is not the problem + .until( + () -> localAlign.isAutoalignComplete() || !localAlign.isReady()) // never gets here .alongWith(new InstantCommand(() -> PathplannerWeight.setSpeeds(new ChassisSpeeds())))); NamedCommands.registerCommand( "WaitForLocal", diff --git a/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java b/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java index 3eee0691..865585cd 100644 --- a/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java +++ b/src/main/java/frc/robot/subsystems/drive/weights/LocalTagAlignWeight.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +// import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.constants.FieldConstants; import frc.robot.sensors.apriltag.AprilTagVision; import frc.robot.sensors.gyro.Gyro; @@ -108,8 +109,9 @@ public boolean isReady() { } public Command getAutoCommand() { - return new InstantCommand( - () -> driveCommandFactory.runVelocityCommand(() -> getSpeeds(), () -> true)) + return driveCommandFactory + .runVelocityCommand(() -> getSpeeds(), () -> true) + .alongWith(new InstantCommand(() -> System.out.println("getAutoCommand is running"))) .finallyDo( () -> driveCommandFactory.runVelocityCommand(() -> new ChassisSpeeds(), () -> true)); } From 6a974527197fdccfd12f27f709b70aac76e2f26b Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Sat, 24 May 2025 16:34:12 -0400 Subject: [PATCH 19/25] cleanup crew --- src/main/java/frc/robot/RobotContainer.java | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 635d5da5..bfa3c756 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -167,8 +167,6 @@ public class RobotContainer { boolean homed = false; - boolean autoLocal = false; - public RobotContainer() { switch (Robot.getMode()) { @@ -424,8 +422,6 @@ public void periodic() { Logger.recordOutput("WeightSize", DriveWeightCommand.getWeightsSize()); - Logger.recordOutput("AutonLocalaligned", autoLocal); - Logger.recordOutput( "DistFromTarget", RobotOdometry.instance @@ -1037,9 +1033,4 @@ public void generateNamedCommands() { "WaitForLocal", new WaitCommand(0.1).andThen(new WaitUntilCommand(() -> localAlign.isReady()))); } - - public boolean autoLocal() { - autoLocal = true; - return autoLocal; - } } From c04df91ebd6f57551eebb99e81815f286a0ccf9c Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Wed, 28 May 2025 18:50:25 -0400 Subject: [PATCH 20/25] a squidgeon faster (all global constraints bumped up by .2) --- .../deploy/pathplanner/paths/Explode the processor 1.path | 4 ++-- .../deploy/pathplanner/paths/Explode the processor 2.path | 4 ++-- .../deploy/pathplanner/paths/Explode the processor 3.path | 4 ++-- .../deploy/pathplanner/paths/Explode the processor 4.path | 4 ++-- src/main/deploy/pathplanner/paths/L1 Spam Opposite 1.path | 4 ++-- src/main/deploy/pathplanner/paths/L1 Spam Opposite 2.path | 4 ++-- src/main/deploy/pathplanner/paths/L1 Spam Opposite 3.path | 4 ++-- src/main/deploy/pathplanner/paths/L1 Spam Opposite 4.path | 4 ++-- src/main/deploy/pathplanner/paths/L1 Spam Opposite 5.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Opposite 1 - Double.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Opposite 2 - Double.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Opposite 2 - Triple.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Opposite 3 - Double.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Opposite 3 - Triple.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Opposite 4 - Double.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Opposite 4 - Triple.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Opposite 5 - Triple.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Opposite 5- Double.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Processor 1.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Processor 2.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Processor 3.path | 4 ++-- .../deploy/pathplanner/paths/L4 Processor 4 - Double.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Processor 4.path | 4 ++-- .../deploy/pathplanner/paths/L4 Processor 5 - Double.path | 4 ++-- src/main/deploy/pathplanner/paths/L4 Processor 5.path | 4 ++-- src/main/deploy/pathplanner/paths/Process 2.path | 4 ++-- src/main/deploy/pathplanner/paths/Process 3.path | 4 ++-- src/main/deploy/pathplanner/paths/SpitProcess 1.path | 4 ++-- src/main/deploy/pathplanner/paths/SpitProcess 2.path | 4 ++-- src/main/deploy/pathplanner/paths/SpitProcess 3.path | 4 ++-- src/main/deploy/pathplanner/paths/SpitProcess 4.path | 4 ++-- src/main/deploy/pathplanner/settings.json | 4 ++-- 33 files changed, 66 insertions(+), 66 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Explode the processor 1.path b/src/main/deploy/pathplanner/paths/Explode the processor 1.path index 70b607ab..211d8d79 100644 --- a/src/main/deploy/pathplanner/paths/Explode the processor 1.path +++ b/src/main/deploy/pathplanner/paths/Explode the processor 1.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Explode the processor 2.path b/src/main/deploy/pathplanner/paths/Explode the processor 2.path index 38f8a9b2..b92b2d69 100644 --- a/src/main/deploy/pathplanner/paths/Explode the processor 2.path +++ b/src/main/deploy/pathplanner/paths/Explode the processor 2.path @@ -93,8 +93,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Explode the processor 3.path b/src/main/deploy/pathplanner/paths/Explode the processor 3.path index 65c2d6c3..615f1eee 100644 --- a/src/main/deploy/pathplanner/paths/Explode the processor 3.path +++ b/src/main/deploy/pathplanner/paths/Explode the processor 3.path @@ -63,8 +63,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Explode the processor 4.path b/src/main/deploy/pathplanner/paths/Explode the processor 4.path index f417f787..b21d91eb 100644 --- a/src/main/deploy/pathplanner/paths/Explode the processor 4.path +++ b/src/main/deploy/pathplanner/paths/Explode the processor 4.path @@ -88,8 +88,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 1.path b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 1.path index 61094b1e..9e705d5e 100644 --- a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 1.path +++ b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 1.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 2.path b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 2.path index 4a76f7bf..382bf178 100644 --- a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 2.path +++ b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 2.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 3.path b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 3.path index d1340835..93ce5dda 100644 --- a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 3.path +++ b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 3.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 4.path b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 4.path index fa7a2ca6..51a2aa01 100644 --- a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 4.path +++ b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 4.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 5.path b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 5.path index 85d95113..10c8f170 100644 --- a/src/main/deploy/pathplanner/paths/L1 Spam Opposite 5.path +++ b/src/main/deploy/pathplanner/paths/L1 Spam Opposite 5.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Double.path b/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Double.path index 01395827..b8ef5d69 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Double.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.5, + "maxVelocity": 1.7, + "maxAcceleration": 1.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path b/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path index 0e07ab9c..836f6160 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 1 - Triple.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 3.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Double.path b/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Double.path index cfb242ce..5229593a 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Double.path @@ -47,8 +47,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.5, - "maxAcceleration": 2.5, + "maxVelocity": 2.7, + "maxAcceleration": 2.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Triple.path b/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Triple.path index 1de717a5..2bd6c823 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Triple.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 2 - Triple.path @@ -47,8 +47,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.7, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Double.path b/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Double.path index 4a07952b..d460dac7 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Double.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.2, + "maxAcceleration": 2.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Triple.path b/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Triple.path index 66da82b7..a5f66c73 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Triple.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 3 - Triple.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.7, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Double.path b/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Double.path index 547cf7f4..5c7ebed3 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Double.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.5, + "maxVelocity": 3.2, + "maxAcceleration": 2.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Triple.path b/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Triple.path index bd6fd185..137b9552 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Triple.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 4 - Triple.path @@ -47,8 +47,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.7, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 5 - Triple.path b/src/main/deploy/pathplanner/paths/L4 Opposite 5 - Triple.path index baa6f7a1..191a4336 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 5 - Triple.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 5 - Triple.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.7, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Opposite 5- Double.path b/src/main/deploy/pathplanner/paths/L4 Opposite 5- Double.path index e99cee3c..ffa7b209 100644 --- a/src/main/deploy/pathplanner/paths/L4 Opposite 5- Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Opposite 5- Double.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.5, + "maxVelocity": 2.2, + "maxAcceleration": 2.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 1.path b/src/main/deploy/pathplanner/paths/L4 Processor 1.path index cddfa0da..f9e0d5d2 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 1.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 1.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 3.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 2.path b/src/main/deploy/pathplanner/paths/L4 Processor 2.path index 5937d206..16316292 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 2.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 2.path @@ -47,8 +47,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.7, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 3.path b/src/main/deploy/pathplanner/paths/L4 Processor 3.path index 83f8bbb4..48b84aa2 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 3.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 3.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 3.5, + "maxVelocity": 3.7, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 4 - Double.path b/src/main/deploy/pathplanner/paths/L4 Processor 4 - Double.path index 5b1283fd..9787ee65 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 4 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 4 - Double.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 4.path b/src/main/deploy/pathplanner/paths/L4 Processor 4.path index 755a362e..bd8c6a7b 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 4.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 4.path @@ -47,8 +47,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.5, + "maxVelocity": 3.2, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 5 - Double.path b/src/main/deploy/pathplanner/paths/L4 Processor 5 - Double.path index 554966b1..d80985d6 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 5 - Double.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 5 - Double.path @@ -70,8 +70,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/L4 Processor 5.path b/src/main/deploy/pathplanner/paths/L4 Processor 5.path index 94c25125..c0a2d182 100644 --- a/src/main/deploy/pathplanner/paths/L4 Processor 5.path +++ b/src/main/deploy/pathplanner/paths/L4 Processor 5.path @@ -84,8 +84,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.5, - "maxAcceleration": 3.25, + "maxVelocity": 2.8, + "maxAcceleration": 3.6, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Process 2.path b/src/main/deploy/pathplanner/paths/Process 2.path index 2d4c6e07..60581a88 100644 --- a/src/main/deploy/pathplanner/paths/Process 2.path +++ b/src/main/deploy/pathplanner/paths/Process 2.path @@ -93,8 +93,8 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Process 3.path b/src/main/deploy/pathplanner/paths/Process 3.path index 36f10800..312cfe4e 100644 --- a/src/main/deploy/pathplanner/paths/Process 3.path +++ b/src/main/deploy/pathplanner/paths/Process 3.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, + "maxVelocity": 2.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/SpitProcess 1.path b/src/main/deploy/pathplanner/paths/SpitProcess 1.path index c94bfed0..c0b21d5a 100644 --- a/src/main/deploy/pathplanner/paths/SpitProcess 1.path +++ b/src/main/deploy/pathplanner/paths/SpitProcess 1.path @@ -91,8 +91,8 @@ } ], "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 1.0, + "maxVelocity": 1.7, + "maxAcceleration": 1.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/SpitProcess 2.path b/src/main/deploy/pathplanner/paths/SpitProcess 2.path index c1eb5dbe..4267b277 100644 --- a/src/main/deploy/pathplanner/paths/SpitProcess 2.path +++ b/src/main/deploy/pathplanner/paths/SpitProcess 2.path @@ -64,8 +64,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.5, + "maxVelocity": 3.2, + "maxAcceleration": 3.7, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/SpitProcess 3.path b/src/main/deploy/pathplanner/paths/SpitProcess 3.path index 21c12302..6ba5732c 100644 --- a/src/main/deploy/pathplanner/paths/SpitProcess 3.path +++ b/src/main/deploy/pathplanner/paths/SpitProcess 3.path @@ -90,8 +90,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 2.0, + "maxVelocity": 3.2, + "maxAcceleration": 2.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/SpitProcess 4.path b/src/main/deploy/pathplanner/paths/SpitProcess 4.path index 4923baab..aa26e288 100644 --- a/src/main/deploy/pathplanner/paths/SpitProcess 4.path +++ b/src/main/deploy/pathplanner/paths/SpitProcess 4.path @@ -68,8 +68,8 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, + "maxVelocity": 1.2, + "maxAcceleration": 1.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index cdbd249b..cb8613b8 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -15,8 +15,8 @@ "SpitSpit" ], "autoFolders": [], - "defaultMaxVel": 2.0, - "defaultMaxAccel": 2.0, + "defaultMaxVel": 2.2, + "defaultMaxAccel": 2.2, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, From 9af6cd6afff6cf45ef1b6513c64dcf3f3d50d83f Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Wed, 28 May 2025 19:36:26 -0400 Subject: [PATCH 21/25] ARE YOU READY TO RUMBLEEEEE --- src/main/java/frc/robot/RobotContainer.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bfa3c756..ea0de47a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -977,7 +977,11 @@ public void generateNamedCommands() { NamedCommands.registerCommand( "SetupL4", new InstantCommand( - () -> coralPreset = gantryAuto ? CoralPreset.RightL4 : CoralPreset.LeftL4)); + () -> + coralPreset = + gantryAuto + ? CoralPreset.RightL4 + : CoralPreset.LeftL4)); // increment up l2 - l4 for testing!! NamedCommands.registerCommand( "SetupL3", new InstantCommand( From a7491e27f9e0dead7255a9dc79ec4d66b3fc579c Mon Sep 17 00:00:00 2001 From: zoe99999999 Date: Sat, 7 Jun 2025 15:35:21 -0400 Subject: [PATCH 22/25] derp fix --- src/main/java/frc/robot/sensors/gyro/GyroIONavX.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/sensors/gyro/GyroIONavX.java b/src/main/java/frc/robot/sensors/gyro/GyroIONavX.java index 172d41be..77941b51 100644 --- a/src/main/java/frc/robot/sensors/gyro/GyroIONavX.java +++ b/src/main/java/frc/robot/sensors/gyro/GyroIONavX.java @@ -57,7 +57,7 @@ public void updateInputs(GyroIOInputs inputs) { @Override public void resetGyro(GyroIOInputs inputs) { rollOffset = inputs.roll.plus(new Rotation2d(rollOffset)).getRadians(); - pitchOffset = inputs.pitch.plus(new Rotation2d(rollOffset)).getRadians(); + pitchOffset = inputs.pitch.plus(new Rotation2d(pitchOffset)).getRadians(); offset = inputs.angleRadiansRaw; } From ad6f4b9fce6393e1f6d2dbcd623d648579888292 Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 18 Jun 2025 20:02:34 -0400 Subject: [PATCH 23/25] Fiddles with intermediate stages --- build.gradle | 7 +++++++ src/main/java/frc/robot/RobotContainer.java | 14 +++++++------- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/build.gradle b/build.gradle index 1bee9653..6206ae0e 100644 --- a/build.gradle +++ b/build.gradle @@ -42,6 +42,13 @@ deploy { jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M") jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M") jvmArgs.add("-XX:+AlwaysPreTouch") + // Enable VisualVM connection + // jvmArgs.add("-Dcom.sun.management.jmxremote=true") + // jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + // jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + // jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + // jvmArgs.add("-Djava.rmi.server.hostname=10.16.40.2") // Replace TE.AM with team number } // Static files artifact diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ea0de47a..bb556a91 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -514,12 +514,12 @@ private void configureBindings() { followPathCoral.generateTrigger( () -> driveHID.getLeftBumperButton() && !followPathCoral.isAutoalignComplete()); - new Trigger( - () -> - coralOuttakeSubsystem.hasCoral() - && coralOuttakeCommandFactory.ranBack - && !coralOuttakeSubsystem.guillotineCheck()) - .onTrue(setupAutoPlace(() -> CoralPreset.PreMove).onlyIf(() -> premoveLift)); + // new Trigger( + // () -> + // coralOuttakeSubsystem.hasCoral() + // && coralOuttakeCommandFactory.ranBack + // && !coralOuttakeSubsystem.guillotineCheck()) + // .onTrue(setupAutoPlace(() -> CoralPreset.PreMove).onlyIf(() -> premoveLift)); driveController.povDown().onTrue(new InstantCommand(() -> premoveLift = true)); driveController.povUp().onTrue(new InstantCommand(() -> premoveLift = false)); @@ -869,7 +869,7 @@ private void configurePitBindings() { public Command getAutonomousCommand() { return homing() .andThen(new InstantCommand(() -> autoRampPos = true)) - .andThen(new InstantCommand(() -> premoveLift = true)) + // .andThen(new InstantCommand(() -> premoveLift = true)) .andThen(dashboard.getAutoChooserCommand()); // return new InstantCommand(); } From 17e27ad34b7f392cc023572a6dd4d12fdc2e3faa Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 18 Jun 2025 21:02:05 -0400 Subject: [PATCH 24/25] Removed other controllers --- src/main/java/frc/robot/Robot.java | 11 +- src/main/java/frc/robot/RobotContainer.java | 158 ++++++++++---------- 2 files changed, 89 insertions(+), 80 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index aace89c7..45f709f1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -132,9 +132,18 @@ public void robotInit() { @Override public void robotPeriodic() { + long time0 = System.nanoTime(); CommandScheduler.getInstance().run(); + long time1 = System.nanoTime(); PeriodicScheduler.getInstance().run(); - LoggerManager.updateLog(); + long time2 = System.nanoTime(); + // LoggerManager.updateLog(); + long time3 = System.nanoTime(); + // if (time3 - time0 > 15000000) { + System.out.println("Command scheduler time: " + (time1 - time0)); + System.out.println("Periodic scheduler time: " + (time2 - time1)); + System.out.println("Logger manager time: " + (time3 - time2)); + // } } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bb556a91..0abeab09 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -119,15 +119,15 @@ public class RobotContainer { // Controller private final CommandXboxController driveController = new CommandXboxController(0); private final CommandXboxController operatorController = new CommandXboxController(1); - private final CommandXboxController pitController = new CommandXboxController(4); + // private final CommandXboxController pitController = new CommandXboxController(4); private final XboxController driveHID = driveController.getHID(); private final XboxController opHID = operatorController.getHID(); private final XboxController operatorControllerHID = operatorController.getHID(); private final PresetBoard presetBoard = new PresetBoard(2); - private final PresetBoard simBoard = new PresetBoard(3); - private final PresetBoard motorBoard = new PresetBoard(5); + // private final PresetBoard simBoard = new PresetBoard(3); + // private final PresetBoard motorBoard = new PresetBoard(5); private final AlertsManager alertsManager; @@ -249,22 +249,22 @@ public RobotContainer() { gantrySubsystem = new GantrySubsystem( RobotConfigConstants.gantrySubsystemEnabled - ? new GantryIOSim(() -> simBoard.getLl2()) + ? new GantryIOSim(() -> /*simBoard.getLl2()*/false) : new GantryIO() {}); liftSubsystem = new LiftSubsystem( RobotConfigConstants.liftSubsystemEnabled - ? new LiftIOSim(() -> simBoard.getLl3()) + ? new LiftIOSim(() -> /*simBoard.getLl3()*/false) : new LiftIO() {}); coralOuttakeSubsystem = new CoralOuttakeSubsystem( RobotConfigConstants.coralOuttakeSubsystemEnabled - ? new CoralOuttakeIOSim(() -> simBoard.getRl2()) + ? new CoralOuttakeIOSim(() -> /*simBoard.getRl2()*/false) : new CoralOuttakeIO() {}); climberSubsystem = new ClimberSubsystem( RobotConfigConstants.climberSubsystemEnabled - ? new ClimberIOSim(() -> simBoard.getRl4()) + ? new ClimberIOSim(() -> /*simBoard.getRl4(*/false) : new ClimberIO() {}); winchSubsystem = new WinchSubsystem( @@ -272,7 +272,7 @@ public RobotContainer() { algaeIntakeSubsystem = new AlgaeSubsystem( RobotConfigConstants.algaeIntakeEnabled - ? new AlgaeIOSim(() -> simBoard.getLl4()) + ? new AlgaeIOSim(() -> /*simBoard.getLl4()*/false) : new AlgaeIO() {}); break; default: @@ -317,8 +317,8 @@ public RobotContainer() { // Otherwise we wouldn't have a drive controller during the match joystickDriveWeight = new JoystickDriveWeight( - () -> -driveController.getLeftY() - 1 * pitController.getLeftY(), - () -> -driveController.getLeftX() - 1 * pitController.getLeftX(), + () -> -driveController.getLeftY(), //- 1 * pitController.getLeftY(), + () -> -driveController.getLeftX(), //- 1 * pitController.getLeftX(), () -> -driveController.getRightX(), driveController.rightBumper(), driveController.leftTrigger(), @@ -760,18 +760,18 @@ private void configureBindings() { .setSolenoidState(() -> true) .andThen(algaeCommandFactory.processCommand())); // motor board - new Trigger(() -> motorBoard.getLl2()) - .whileTrue(liftCommandFactory.liftApplyVoltageCommand(() -> 2)); - new Trigger(() -> motorBoard.getLl3()) - .whileTrue(gantryCommandFactory.gantryApplyVoltageCommand(() -> 1)); - new Trigger(() -> motorBoard.getLl4()) - .whileTrue(coralOuttakeCommandFactory.setIntakeVoltage(() -> 4)); - new Trigger(() -> motorBoard.getRl4()) - .whileTrue(algaeCommandFactory.setMotorVoltages(() -> 10, () -> 10)); - new Trigger(() -> motorBoard.getRl3()) - .whileTrue(climberCommandFactory.elevatorApplyVoltageCommand(() -> -1)); - new Trigger(() -> motorBoard.getTrough()) - .onTrue(new InstantCommand(() -> liftSubsystem.resetEncoder())); + // new Trigger(() -> motorBoard.getLl2()) + // .whileTrue(liftCommandFactory.liftApplyVoltageCommand(() -> 2)); + // new Trigger(() -> motorBoard.getLl3()) + // .whileTrue(gantryCommandFactory.gantryApplyVoltageCommand(() -> 1)); + // new Trigger(() -> motorBoard.getLl4()) + // .whileTrue(coralOuttakeCommandFactory.setIntakeVoltage(() -> 4)); + // new Trigger(() -> motorBoard.getRl4()) + // .whileTrue(algaeCommandFactory.setMotorVoltages(() -> 10, () -> 10)); + // new Trigger(() -> motorBoard.getRl3()) + // .whileTrue(climberCommandFactory.elevatorApplyVoltageCommand(() -> -1)); + // new Trigger(() -> motorBoard.getTrough()) + // .onTrue(new InstantCommand(() -> liftSubsystem.resetEncoder())); new Trigger(operatorController.leftTrigger()) .whileTrue(new InstantCommand(() -> algaeIntakeSubsystem.setHasAlgae(false))); @@ -802,69 +802,69 @@ private void configureBindings() { new InstantCommand(() -> operatorControllerHID.setRumble(RumbleType.kBothRumble, 0))); mapPIDtoCommand(); if (TestConfig.testingMode == TestingSetting.pit) { - configurePitBindings(); + // configurePitBindings(); } else { // put in TESTBOARD triggers here } } - private void configurePitBindings() { - new Trigger(() -> (pitController.getHID().getPOV() == 0)) - .whileTrue( - climberCommandFactory.winchApplyVoltageCommand( - (pitController.getHID().getPOV() == 0 ? () -> -2.5 : () -> 2.5))); - new Trigger(() -> (pitController.getHID().getPOV() == 180)) - .whileTrue( - climberCommandFactory.winchApplyVoltageCommand( - (pitController.getHID().getPOV() == 180 ? () -> 2.5 : () -> -2.5))); + // private void configurePitBindings() { + // new Trigger(() -> (pitController.getHID().getPOV() == 0)) + // .whileTrue( + // climberCommandFactory.winchApplyVoltageCommand( + // (pitController.getHID().getPOV() == 0 ? () -> -2.5 : () -> 2.5))); + // new Trigger(() -> (pitController.getHID().getPOV() == 180)) + // .whileTrue( + // climberCommandFactory.winchApplyVoltageCommand( + // (pitController.getHID().getPOV() == 180 ? () -> 2.5 : () -> -2.5))); - new Trigger(() -> pitController.getHID().getPOV() == 270) - .whileTrue(new InstantCommand(() -> autoRampPos = false)); - new Trigger(() -> Math.abs(pitController.getRightY()) > 0.03) - .whileTrue( - climberCommandFactory.elevatorApplyVoltageCommand( - () -> -pitController.getRightY() * 4)); - pitController - .rightBumper() - .whileTrue(gantryCommandFactory.gantrySetVelocityCommand(() -> GantryConstants.alignSpeed)); - pitController - .leftBumper() - .whileTrue( - gantryCommandFactory.gantrySetVelocityCommand(() -> -GantryConstants.alignSpeed)); - pitController - .rightTrigger() - .and(() -> !algaeIntakeSubsystem.hasAlgae()) - .whileTrue( - algaeCommandFactory - .setSolenoidState(() -> true) - .andThen(algaeCommandFactory.setMotorVoltages(() -> 4, () -> 4))); - pitController - .leftTrigger() - .and(() -> algaeIntakeSubsystem.hasAlgae()) - .whileTrue( - algaeCommandFactory - .setSolenoidState(() -> true) - .andThen(algaeCommandFactory.processCommand())); - pitController - .start() - .whileTrue( - new InstantCommand(() -> liftSubsystem.resetEncoder()) - .alongWith(new InstantCommand(() -> climberSubsystem.resetEncoder())) - .alongWith(new InstantCommand(() -> autoRampPos = true))); - pitController - .b() - .whileTrue( - coralOuttakeCommandFactory - .outtake() - .finallyDo(() -> coralOuttakeCommandFactory.outtaking = false)); - pitController.y().and(() -> !coralOuttakeCommandFactory.outtaking).onTrue(runLiftToSafe()); - pitController.back().whileTrue(gantryCommandFactory.gantryHomeCommand()); - pitController - .povRight() - .onTrue(climberCommandFactory.setClampState(() -> !climberSubsystem.getSolenoidState())); - pitController.a().onTrue(setupAutoPlace(() -> coralPreset)); - pitController.x().onTrue(new InstantCommand(() -> AntiTipWeight.setAntiTipEnabled(false))); - } + // new Trigger(() -> pitController.getHID().getPOV() == 270) + // .whileTrue(new InstantCommand(() -> autoRampPos = false)); + // new Trigger(() -> Math.abs(pitController.getRightY()) > 0.03) + // .whileTrue( + // climberCommandFactory.elevatorApplyVoltageCommand( + // () -> -pitController.getRightY() * 4)); + // pitController + // .rightBumper() + // .whileTrue(gantryCommandFactory.gantrySetVelocityCommand(() -> GantryConstants.alignSpeed)); + // pitController + // .leftBumper() + // .whileTrue( + // gantryCommandFactory.gantrySetVelocityCommand(() -> -GantryConstants.alignSpeed)); + // pitController + // .rightTrigger() + // .and(() -> !algaeIntakeSubsystem.hasAlgae()) + // .whileTrue( + // algaeCommandFactory + // .setSolenoidState(() -> true) + // .andThen(algaeCommandFactory.setMotorVoltages(() -> 4, () -> 4))); + // pitController + // .leftTrigger() + // .and(() -> algaeIntakeSubsystem.hasAlgae()) + // .whileTrue( + // algaeCommandFactory + // .setSolenoidState(() -> true) + // .andThen(algaeCommandFactory.processCommand())); + // pitController + // .start() + // .whileTrue( + // new InstantCommand(() -> liftSubsystem.resetEncoder()) + // .alongWith(new InstantCommand(() -> climberSubsystem.resetEncoder())) + // .alongWith(new InstantCommand(() -> autoRampPos = true))); + // pitController + // .b() + // .whileTrue( + // coralOuttakeCommandFactory + // .outtake() + // .finallyDo(() -> coralOuttakeCommandFactory.outtaking = false)); + // pitController.y().and(() -> !coralOuttakeCommandFactory.outtaking).onTrue(runLiftToSafe()); + // pitController.back().whileTrue(gantryCommandFactory.gantryHomeCommand()); + // pitController + // .povRight() + // .onTrue(climberCommandFactory.setClampState(() -> !climberSubsystem.getSolenoidState())); + // pitController.a().onTrue(setupAutoPlace(() -> coralPreset)); + // pitController.x().onTrue(new InstantCommand(() -> AntiTipWeight.setAntiTipEnabled(false))); + // } public Command getAutonomousCommand() { return homing() From f344e0459b4d93ae16e59cb3ba2a49e40e2810ed Mon Sep 17 00:00:00 2001 From: 1023941311414 Date: Wed, 18 Jun 2025 21:03:30 -0400 Subject: [PATCH 25/25] Removed PID tracker --- src/main/java/frc/robot/Robot.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 45f709f1..19491e53 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -132,17 +132,17 @@ public void robotInit() { @Override public void robotPeriodic() { - long time0 = System.nanoTime(); + // long time0 = System.nanoTime(); CommandScheduler.getInstance().run(); - long time1 = System.nanoTime(); + // long time1 = System.nanoTime(); PeriodicScheduler.getInstance().run(); - long time2 = System.nanoTime(); + // long time2 = System.nanoTime(); // LoggerManager.updateLog(); - long time3 = System.nanoTime(); + // long time3 = System.nanoTime(); // if (time3 - time0 > 15000000) { - System.out.println("Command scheduler time: " + (time1 - time0)); - System.out.println("Periodic scheduler time: " + (time2 - time1)); - System.out.println("Logger manager time: " + (time3 - time2)); + // System.out.println("Command scheduler time: " + (time1 - time0)); + // System.out.println("Periodic scheduler time: " + (time2 - time1)); + // System.out.println("Logger manager time: " + (time3 - time2)); // } }