From 88fbc2050dc27b4f16bd532a53d40dfdc7c9884d Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Wed, 28 Feb 2024 19:09:55 -0800 Subject: [PATCH 01/49] merg conflict --- src/main/java/frc/team2412/robot/util/AutoPaths.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/util/AutoPaths.java b/src/main/java/frc/team2412/robot/util/AutoPaths.java index 184376ef..8e24983d 100644 --- a/src/main/java/frc/team2412/robot/util/AutoPaths.java +++ b/src/main/java/frc/team2412/robot/util/AutoPaths.java @@ -139,4 +139,4 @@ public static Command SubwooferLaunchCommand() { LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, LauncherSubsystem.SUBWOOFER_AIM_ANGLE); } -} +} \ No newline at end of file From c7f6fde0f990e13f7ad88fa20a5b7178bb6dc7c8 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 9 Mar 2024 10:54:39 -0800 Subject: [PATCH 02/49] merge in --- .../robot/util/{ => auto}/AutoLogic.java | 58 ++++++++++++++----- .../team2412/robot/util/auto/AutoPath.java | 19 ++++++ .../ComplexAutoPaths.java} | 20 ++++++- 3 files changed, 79 insertions(+), 18 deletions(-) rename src/main/java/frc/team2412/robot/util/{ => auto}/AutoLogic.java (66%) create mode 100644 src/main/java/frc/team2412/robot/util/auto/AutoPath.java rename src/main/java/frc/team2412/robot/util/{AutoPaths.java => auto/ComplexAutoPaths.java} (90%) diff --git a/src/main/java/frc/team2412/robot/util/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java similarity index 66% rename from src/main/java/frc/team2412/robot/util/AutoLogic.java rename to src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 7c7852d7..841f3984 100644 --- a/src/main/java/frc/team2412/robot/util/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -1,8 +1,12 @@ -package frc.team2412.robot.util; +package frc.team2412.robot.util.auto; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -19,12 +23,20 @@ import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; import frc.team2412.robot.commands.launcher.StopLauncherCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; +import java.util.HashMap; +import java.util.List; +import java.util.Map; public class AutoLogic { public static Robot r = Robot.getInstance(); public static final Subsystems s = r.subsystems; public static final Controls controls = r.controls; + List onePiecePaths = List.of(new AutoPath("Test Path", "testPath")); + + Map> commandsMap = Map.of(0, List.of(new AutoPath("Test Path", "testPath"))); + + // in place of launching command cause launcher doesnt exist public static SequentialCommandGroup vibrateControllerCommand = new SequentialCommandGroup( @@ -32,14 +44,18 @@ public class AutoLogic { new WaitCommand(1.5), new InstantCommand(() -> controls.vibrateDriveController(0.0))); - /** - * Placeholder for vision detect note - * - * @return true - */ - public static boolean dummyLogic() { - return true; - } + private static ShuffleboardTab tab = Shuffleboard.getTab("Match"); + + public static enum StartPosition { + AMP_SIDE_SUBWOOFER(), + MID_SIDE_SUBWOOFER(), + SOURCE_SIDE_SUBWOOFER(); + }; + + private static SendableChooser startPosition; + private static SendableChooser availableAutos; + private static GenericEntry amountGamePiecesEntry; + private static GenericEntry autoRoutinesEntry; public AutoLogic() { @@ -76,16 +92,15 @@ public void registerCommands() { new SetAngleLaunchCommand(s.launcherSubsystem, 0, 0)); // TODO: add retract angle // Complex Autos - NamedCommands.registerCommand("AutoLogicTest", AutoPaths.testAuto); + NamedCommands.registerCommand("AutoLogicTest", ComplexAutoPaths.testAuto); NamedCommands.registerCommand( - "MidSpeakerCenterLineN5N4N3", AutoPaths.midSpeakerCenterLineN3N2N1); + "MidSpeakerCenterLineN5N4N3", ComplexAutoPaths.midSpeakerCenterLineN3N2N1); NamedCommands.registerCommand( - "LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4N3); - NamedCommands.registerCommand("LowSpeakerCenterLineN5N4N3", AutoPaths.lowSpeakerCenterLineN5N4); + "LowSpeakerCenterLineN5N4N3", ComplexAutoPaths.lowSpeakerCenterLineN5N4N3); NamedCommands.registerCommand( - "TopSpeakerCenterLineN1N2AutoLine1", AutoPaths.TopSpeakerCenterLineN1N2AutoLine1); + "TopSpeakerCe,nterLineN1N2AutoLine1", ComplexAutoPaths.TopSpeakerCenterLineN1N2AutoLine1); NamedCommands.registerCommand( - "TopSpeakerCenterLineN1N2N3", AutoPaths.TopSpeakerCenterLineN1N2N3); + "TopSpeakerCenterLineN1N2AutoLine1", ComplexAutoPaths.TopSpeakerCenterLineN1N2N3); } // public Command getConditionalCommand(){} @@ -103,4 +118,17 @@ public static Command getAutoCommand(String pathName) { // Create a path following command using AutoBuilder. This will also trigger event markers. return AutoBuilder.followPath(path); } + + public static void initShuffleBoard() { + + tab.add("Starting Position", startPosition).withPosition(5, 1).withSize(2, 1); + amountGamePiecesEntry = tab.add("Game Pieces", 0).withPosition(5, 2).withSize(2, 1).getEntry(); + tab.add("Available Auto Variants", availableAutos).withPosition(5, 3).withSize(2, 1); + } + + // public static void updateAvailableAutos() { + + // for () + + // } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java new file mode 100644 index 00000000..330cf67b --- /dev/null +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -0,0 +1,19 @@ +package frc.team2412.robot.util.auto; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; + +public class AutoPath { + + private Pose2d startingPose; + private String displayName; + private Command autoCommand; + + public AutoPath(String displayName, String pathPlannerAutoName) { + this.displayName = displayName; + startingPose = PathPlannerAuto.getStaringPoseFromAutoFile(pathPlannerAutoName); + autoCommand = AutoBuilder.buildAuto(pathPlannerAutoName); + } +} diff --git a/src/main/java/frc/team2412/robot/util/AutoPaths.java b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java similarity index 90% rename from src/main/java/frc/team2412/robot/util/AutoPaths.java rename to src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java index 039c5fb4..752462ad 100644 --- a/src/main/java/frc/team2412/robot/util/AutoPaths.java +++ b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java @@ -1,6 +1,6 @@ -package frc.team2412.robot.util; +package frc.team2412.robot.util.auto; -import static frc.team2412.robot.util.AutoLogic.*; +import static frc.team2412.robot.util.auto.AutoLogic.*; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -10,7 +10,21 @@ import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; -public class AutoPaths { +public class ComplexAutoPaths { + + // Paths Hashmap + + // public static enum AutoRegistry { + // TEST("test auto", 0, testAuto); + // public final String autoName; + // public final int gamePieces; + // public final Command autoCommand; + // AutoRegistry(String autoName, int gamePieces, Command autoCommand) { + // this.autoName = autoName; + // this.gamePieces = gamePieces; + // this.autoCommand = autoCommand; + // } + // } // Test Auto From 11dfa102ca8c0bdf740b39341ca3e2c6609383da Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 9 Mar 2024 12:04:53 -0800 Subject: [PATCH 03/49] noob gaming --- .../PresetAmpSide1ScorePassAutoline.auto | 31 +++++++++++++ .../autos/PresetAmpSide2Score.auto | 31 +++++++++++++ .../pathplanner/autos/PresetMid1Score.auto | 6 --- .../paths/HighSpeakerHighNote2Return.path | 12 ++--- .../paths/HighSpeakerPassAutoLine.path | 2 +- src/main/java/frc/team2412/robot/Robot.java | 2 +- .../team2412/robot/util/auto/AutoLogic.java | 46 +++++++++++++++---- .../team2412/robot/util/auto/AutoPath.java | 24 +++++++++- .../robot/util/auto/ComplexAutoPaths.java | 2 +- 9 files changed, 131 insertions(+), 25 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto create mode 100644 src/main/deploy/pathplanner/autos/PresetAmpSide2Score.auto diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto b/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto new file mode 100644 index 00000000..6965221a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerPassAutoLine" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSide2Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSide2Score.auto new file mode 100644 index 00000000..fbb31c20 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetAmpSide2Score.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerHighNoteReturn" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetMid1Score.auto b/src/main/deploy/pathplanner/autos/PresetMid1Score.auto index 7822078a..281017e2 100644 --- a/src/main/deploy/pathplanner/autos/PresetMid1Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetMid1Score.auto @@ -16,12 +16,6 @@ "data": { "name": "SubwooferLaunch" } - }, - { - "type": "path", - "data": { - "pathName": "MidSpeakerPassAutoLine" - } } ] } diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerHighNote2Return.path b/src/main/deploy/pathplanner/paths/HighSpeakerHighNote2Return.path index 868873a3..f5bd17b9 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerHighNote2Return.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerHighNote2Return.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 2.8990839963661905, - "y": 5.546821904454621 + "x": 2.8898392931920998, + "y": 5.556066607628711 }, "prevControl": { - "x": 3.189191600351392, - "y": 5.318880215609106 + "x": 3.037754543977556, + "y": 5.352683137798709 }, "nextControl": { - "x": 2.640232307491642, - "y": 5.750205374284623 + "x": 2.6556051426511798, + "y": 5.878138564622476 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoLine.path b/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoLine.path index 4bc34150..8c00ac0f 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoLine.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoLine.path @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -146.8214883406072, + "rotation": 0.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index c4f87026..c8d76bca 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -19,9 +19,9 @@ import frc.team2412.robot.Subsystems.SubsystemConstants; import frc.team2412.robot.commands.diagnostic.IntakeDiagnosticCommand; import frc.team2412.robot.commands.diagnostic.LauncherDiagnosticCommand; -import frc.team2412.robot.util.AutoLogic; import frc.team2412.robot.util.MACAddress; import frc.team2412.robot.util.MatchDashboard; +import frc.team2412.robot.util.auto.AutoLogic; public class Robot extends TimedRobot { /** Singleton Stuff */ diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 841f3984..424527c2 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -3,6 +3,10 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -23,7 +27,7 @@ import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; import frc.team2412.robot.commands.launcher.StopLauncherCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; -import java.util.HashMap; +import edu.wpi.first.math.util.Units; import java.util.List; import java.util.Map; @@ -32,10 +36,40 @@ public class AutoLogic { public static final Subsystems s = r.subsystems; public static final Controls controls = r.controls; - List onePiecePaths = List.of(new AutoPath("Test Path", "testPath")); + public static enum StartPosition { + AMP_SIDE_SUBWOOFER(new Pose2d(0.73, 6.62, new Rotation2d(Units.degreesToRadians(-120)))), + MID_SIDE_SUBWOOFER(new Pose2d(1.33, 5.55, new Rotation2d(Units.degreesToRadians(180)))), + SOURCE_SIDE_SUBWOOFER(new Pose2d(0.73, 4.46, new Rotation2d(Units.degreesToRadians(120)))) + MISC(null); + + Pose2d startPose; + StartPosition(Pose2d startPose) { + this.startPose = startPose; + } + }; + + // paths lists + + List noPiecePaths = + List.of( + new AutoPath("Test Path", "testPath"), + new AutoPath("Stand Still", "PresetSourceSide1Score"), + new AutoPath("Stand Still", "PresetMid1Score"), + new AutoPath("Stand Still", "PresetAmpSide1Score"), + new AutoPath("Pass Auto Line", "PresetSourceSide1ScorePassAutoLine"), + new AutoPath("Pass Auto Line", "PresetAmpSide1ScorePassAutoLine")); + + List onePiecePaths = + List.of( + new AutoPath("AutolineN1", "PresetAmpSide2Score")), + new AutoPath("Autoline N2", "PresetMidAutoline2Score"), + new AutoPath("Autoline N3", "PresetSourceSideAutoline2Score")); - Map> commandsMap = Map.of(0, List.of(new AutoPath("Test Path", "testPath"))); + // gulp map + Map> commandsMap = Map.of(0, onePiecePaths); + + // vars // in place of launching command cause launcher doesnt exist public static SequentialCommandGroup vibrateControllerCommand = @@ -46,12 +80,6 @@ public class AutoLogic { private static ShuffleboardTab tab = Shuffleboard.getTab("Match"); - public static enum StartPosition { - AMP_SIDE_SUBWOOFER(), - MID_SIDE_SUBWOOFER(), - SOURCE_SIDE_SUBWOOFER(); - }; - private static SendableChooser startPosition; private static SendableChooser availableAutos; private static GenericEntry amountGamePiecesEntry; diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java index 330cf67b..b6c80414 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -10,10 +10,32 @@ public class AutoPath { private Pose2d startingPose; private String displayName; private Command autoCommand; + private boolean vision; - public AutoPath(String displayName, String pathPlannerAutoName) { + public AutoPath(String displayName, String pathPlannerAutoName, boolean vision) { this.displayName = displayName; startingPose = PathPlannerAuto.getStaringPoseFromAutoFile(pathPlannerAutoName); autoCommand = AutoBuilder.buildAuto(pathPlannerAutoName); + this.vision = vision; + } + + public AutoPath(String displayName, String pathPlannerAutoName) { + this(displayName, pathPlannerAutoName, false); + } + + public Pose2d getStartPose() { + return startingPose; + } + + public String getDisplayName() { + return displayName; + } + + public Command getAutoCommand() { + return autoCommand; + } + + public boolean isVision() { + return vision; } } diff --git a/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java index 910d4a4e..752462ad 100644 --- a/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java +++ b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java @@ -147,4 +147,4 @@ public static Command SubwooferLaunchCommand() { LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, LauncherSubsystem.SUBWOOFER_AIM_ANGLE); } -} \ No newline at end of file +} From d757e84f33e384508bb6b554ba576c6fbf00457d Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 9 Mar 2024 14:29:02 -0800 Subject: [PATCH 04/49] filter method also registered more paths for shuffleboard --- .../team2412/robot/util/auto/AutoLogic.java | 103 ++++++++++++++---- .../team2412/robot/util/auto/AutoPath.java | 4 +- 2 files changed, 81 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 424527c2..40322ad2 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -3,11 +3,11 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -27,7 +27,6 @@ import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; import frc.team2412.robot.commands.launcher.StopLauncherCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; -import edu.wpi.first.math.util.Units; import java.util.List; import java.util.Map; @@ -37,21 +36,30 @@ public class AutoLogic { public static final Controls controls = r.controls; public static enum StartPosition { - AMP_SIDE_SUBWOOFER(new Pose2d(0.73, 6.62, new Rotation2d(Units.degreesToRadians(-120)))), - MID_SIDE_SUBWOOFER(new Pose2d(1.33, 5.55, new Rotation2d(Units.degreesToRadians(180)))), - SOURCE_SIDE_SUBWOOFER(new Pose2d(0.73, 4.46, new Rotation2d(Units.degreesToRadians(120)))) - MISC(null); - + AMP_SIDE_SUBWOOFER( + " Amp Side Subwoofer", + new Pose2d(0.73, 6.62, new Rotation2d(Units.degreesToRadians(-120)))), + MID_SIDE_SUBWOOFER( + "Mid Side Subwoofer", new Pose2d(1.33, 5.55, new Rotation2d(Units.degreesToRadians(180)))), + SOURCE_SIDE_SUBWOOFER( + "Source Side Subwoofer", + new Pose2d(0.73, 4.46, new Rotation2d(Units.degreesToRadians(120)))), + MISC("Misc", null); + + String title; Pose2d startPose; - StartPosition(Pose2d startPose) { + + StartPosition(String title, Pose2d startPose) { + this.title = title; this.startPose = startPose; } }; // paths lists - List noPiecePaths = + private static List noPiecePaths = List.of( + // presets new AutoPath("Test Path", "testPath"), new AutoPath("Stand Still", "PresetSourceSide1Score"), new AutoPath("Stand Still", "PresetMid1Score"), @@ -59,15 +67,39 @@ public static enum StartPosition { new AutoPath("Pass Auto Line", "PresetSourceSide1ScorePassAutoLine"), new AutoPath("Pass Auto Line", "PresetAmpSide1ScorePassAutoLine")); - List onePiecePaths = + private static List onePiecePaths = List.of( - new AutoPath("AutolineN1", "PresetAmpSide2Score")), + // presets + new AutoPath("Autoline N1", "PresetAmpSide2Score"), new AutoPath("Autoline N2", "PresetMidAutoline2Score"), - new AutoPath("Autoline N3", "PresetSourceSideAutoline2Score")); + new AutoPath("Autoline N3", "PresetSourceSideAutoline2Score"), + new AutoPath("CenterLine N5", "PresetSourceSideFar2Score"), + // vision + new AutoPath("CenterLine N3 N1", "VisionMidFar2Score", true)); + + private static List twoPiecePaths = + List.of( + // presets + new AutoPath("Autoline N1", "PresetAmpSideAutoline3Score"), + new AutoPath("Autoline N2", "PresetMidAutoline3Score"), + // vision + new AutoPath("Centerline N5 N4", "VisionSourceSide3Score", true), + new AutoPath( + "Autoline N1 Centerline STEAL(N1 N2 N3 N4) N5", "VisionAmpSideFarSteal", true)); + + private static List threePiecePaths = + List.of( + // presets + new AutoPath("Autoline N2 N3 N1", "PresetMidAutoline4Score"), + // vision + new AutoPath("Autoline N1 CenterLine N1 N2", "VisionAmpSide4Score", true), + new AutoPath("Autoline N1 N2 N3", "VisionAmpSideAutoLine4Score", true), + new AutoPath("Autoline N3 N2 N1", "VisionMid4Score", true)); // gulp map - Map> commandsMap = Map.of(0, onePiecePaths); + private static Map> commandsMap = + Map.of(0, noPiecePaths, 1, onePiecePaths, 2, twoPiecePaths, 3, threePiecePaths); // vars @@ -80,13 +112,13 @@ public static enum StartPosition { private static ShuffleboardTab tab = Shuffleboard.getTab("Match"); - private static SendableChooser startPosition; - private static SendableChooser availableAutos; + private static SendableChooser startPositionChooser = + new SendableChooser(); + private static SendableChooser availableAutos = new SendableChooser(); private static GenericEntry amountGamePiecesEntry; - private static GenericEntry autoRoutinesEntry; + private static GenericEntry isVisionEntry; public AutoLogic() { - registerCommands(); } @@ -126,7 +158,7 @@ public void registerCommands() { NamedCommands.registerCommand( "LowSpeakerCenterLineN5N4N3", ComplexAutoPaths.lowSpeakerCenterLineN5N4N3); NamedCommands.registerCommand( - "TopSpeakerCe,nterLineN1N2AutoLine1", ComplexAutoPaths.TopSpeakerCenterLineN1N2AutoLine1); + "TopSpeakerCenterLineN1N2AutoLine1", ComplexAutoPaths.TopSpeakerCenterLineN1N2AutoLine1); NamedCommands.registerCommand( "TopSpeakerCenterLineN1N2AutoLine1", ComplexAutoPaths.TopSpeakerCenterLineN1N2N3); } @@ -149,14 +181,37 @@ public static Command getAutoCommand(String pathName) { public static void initShuffleBoard() { - tab.add("Starting Position", startPosition).withPosition(5, 1).withSize(2, 1); - amountGamePiecesEntry = tab.add("Game Pieces", 0).withPosition(5, 2).withSize(2, 1).getEntry(); + startPositionChooser.setDefaultOption(StartPosition.MISC.title, StartPosition.MISC); + for (StartPosition startPosition : StartPosition.values()) { + startPositionChooser.addOption(startPosition.title, startPosition); + } + + tab.add("Starting Position", startPositionChooser).withPosition(5, 1).withSize(2, 1); + + isVisionEntry = + tab.add("Use Vision Launch", false) + .withWidget(BuiltInWidgets.kToggleSwitch) + .withPosition(5, 2) + .withSize(1, 1) + .getEntry(); + amountGamePiecesEntry = tab.add("Game Pieces", 0).withPosition(5, 2).withSize(1, 1).getEntry(); tab.add("Available Auto Variants", availableAutos).withPosition(5, 3).withSize(2, 1); } - // public static void updateAvailableAutos() { + /** Takes the auto filtering entries in shuffleboard to provide a list of suitable autos */ + public static void filterAutos() { + availableAutos.close(); - // for () + List autoCommandsList = commandsMap.get((int) amountGamePiecesEntry.getInteger(0)); - // } + // List filteredList = new ArrayList(); + + for (AutoPath auto : autoCommandsList) { + if (auto.getStartPose().equals(startPositionChooser.getSelected().startPose) + && auto.isVision() == isVisionEntry.getBoolean(false)) { + // filteredList.add(auto); + availableAutos.addOption(auto.getDisplayName(), auto); + } + } + } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java index b6c80414..ab5631ce 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -18,7 +18,7 @@ public AutoPath(String displayName, String pathPlannerAutoName, boolean vision) autoCommand = AutoBuilder.buildAuto(pathPlannerAutoName); this.vision = vision; } - + public AutoPath(String displayName, String pathPlannerAutoName) { this(displayName, pathPlannerAutoName, false); } @@ -30,7 +30,7 @@ public Pose2d getStartPose() { public String getDisplayName() { return displayName; } - + public Command getAutoCommand() { return autoCommand; } From 4d530b03815fb7f5451956fa192a2722a2a4ce7f Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 9 Mar 2024 14:31:48 -0800 Subject: [PATCH 05/49] tweak tweak --- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 40322ad2..e355de67 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -60,7 +60,7 @@ public static enum StartPosition { private static List noPiecePaths = List.of( // presets - new AutoPath("Test Path", "testPath"), + new AutoPath("Test Path", "DiameterTest"), new AutoPath("Stand Still", "PresetSourceSide1Score"), new AutoPath("Stand Still", "PresetMid1Score"), new AutoPath("Stand Still", "PresetAmpSide1Score"), From 5a8c44f8cca1bf70254568a74de32938f6f2ab65 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Mon, 11 Mar 2024 18:24:57 -0700 Subject: [PATCH 06/49] taco --- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index e355de67..54382714 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -64,7 +64,7 @@ public static enum StartPosition { new AutoPath("Stand Still", "PresetSourceSide1Score"), new AutoPath("Stand Still", "PresetMid1Score"), new AutoPath("Stand Still", "PresetAmpSide1Score"), - new AutoPath("Pass Auto Line", "PresetSourceSide1ScorePassAutoLine"), + // new AutoPath("Pass Auto Line", "PresetSourceSide1ScorePassAutoLine"), new AutoPath("Pass Auto Line", "PresetAmpSide1ScorePassAutoLine")); private static List onePiecePaths = From 2feaf6084284ce4dc38fc3800033bc0f135d103a Mon Sep 17 00:00:00 2001 From: kirby Date: Mon, 11 Mar 2024 20:27:09 -0700 Subject: [PATCH 07/49] chooser works yay --- src/main/java/frc/team2412/robot/Robot.java | 1 + .../robot/util/DynamicSendableChooser.java | 171 ++++++++++++++++++ .../team2412/robot/util/auto/AutoLogic.java | 57 ++++-- 3 files changed, 213 insertions(+), 16 deletions(-) create mode 100644 src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index c8d76bca..32f9d9ba 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -88,6 +88,7 @@ public void robotInit() { autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); SmartDashboard.putString("current bot", getTypeFromAddress().toString()); + AutoLogic.initShuffleBoard(); if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { autoChooser = AutoBuilder.buildAutoChooser(); } else { diff --git a/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java new file mode 100644 index 00000000..e6d36e1f --- /dev/null +++ b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java @@ -0,0 +1,171 @@ +package frc.team2412.robot.util; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +import java.util.LinkedHashMap; +import java.util.Map; +import java.util.concurrent.atomic.AtomicInteger; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Consumer; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class DynamicSendableChooser implements Sendable, AutoCloseable { + + /** The key for the default value. */ + private static final String DEFAULT = "default"; + + /** The key for the selected option. */ + private static final String SELECTED = "selected"; + + /** The key for the active option. */ + private static final String ACTIVE = "active"; + + /** The key for the option array. */ + private static final String OPTIONS = "options"; + + /** The key for the instance number. */ + private static final String INSTANCE = ".instance"; + + /** A map linking strings to the objects they represent. */ + public final Map m_map = new LinkedHashMap<>(); + + private String m_defaultChoice = ""; + private final int m_instance; + private String m_previousVal; + private Consumer m_listener; + private static final AtomicInteger s_instances = new AtomicInteger(); + + /** Instantiates a {@link DynamicSendableChooser}. */ + @SuppressWarnings("this-escape") + public DynamicSendableChooser() { + m_instance = s_instances.getAndIncrement(); + SendableRegistry.add(this, "SendableChooser", m_instance); + } + + @Override + public void close() { + SendableRegistry.remove(this); + } + + /** + * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the + * object will appear as the given name. + * + * @param name the name of the option + * @param object the option + */ + public void addOption(String name, T object) { + m_map.put(name, object); + } + + /** + * Removes the given object from the list of options. + * + * @param object the option + */ + public void removeOption(T object) { + m_map.remove(object); + } + + /** + * Adds the given object to the list of options and marks it as the default. Functionally, this is + * very close to {@link #addOption(String, Object)} except that it will use this as the default + * option if none other is explicitly selected. + * + * @param name the name of the option + * @param object the option + */ + public void setDefaultOption(String name, T object) { + requireNonNullParam(name, "name", "setDefaultOption"); + + m_defaultChoice = name; + addOption(name, object); + } + + /** + * Returns the selected option. If there is none selected, it will return the default. If there is + * none selected and no default, then it will return {@code null}. + * + * @return the option selected + */ + public T getSelected() { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_map.get(m_selected); + } else { + return m_map.get(m_defaultChoice); + } + } finally { + m_mutex.unlock(); + } + } + + /** + * Bind a listener that's called when the selected value changes. Only one listener can be bound. + * Calling this function will replace the previous listener. + * + * @param listener The function to call that accepts the new value + */ + public void onChange(Consumer listener) { + requireNonNullParam(listener, "listener", "onChange"); + m_mutex.lock(); + m_listener = listener; + m_mutex.unlock(); + } + + private String m_selected; + private final ReentrantLock m_mutex = new ReentrantLock(); + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("String Chooser"); + builder.publishConstInteger(INSTANCE, m_instance); + builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null); + builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null); + builder.addStringProperty( + ACTIVE, + () -> { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_selected; + } else { + return m_defaultChoice; + } + } finally { + m_mutex.unlock(); + } + }, + null); + builder.addStringProperty( + SELECTED, + null, + val -> { + T choice; + Consumer listener; + m_mutex.lock(); + try { + m_selected = val; + if (!m_selected.equals(m_previousVal) && m_listener != null) { + choice = m_map.get(val); + listener = m_listener; + } else { + choice = null; + listener = null; + } + m_previousVal = val; + } finally { + m_mutex.unlock(); + } + if (listener != null) { + listener.accept(choice); + } + }); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 54382714..9c0dc71c 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -1,5 +1,7 @@ package frc.team2412.robot.util.auto; +import com.ctre.phoenix6.controls.StaticBrake; +import com.fasterxml.jackson.databind.ser.std.StdKeySerializers.Dynamic; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; @@ -27,6 +29,8 @@ import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; import frc.team2412.robot.commands.launcher.StopLauncherCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; +import frc.team2412.robot.util.DynamicSendableChooser; + import java.util.List; import java.util.Map; @@ -96,7 +100,10 @@ public static enum StartPosition { new AutoPath("Autoline N1 N2 N3", "VisionAmpSideAutoLine4Score", true), new AutoPath("Autoline N3 N2 N1", "VisionMid4Score", true)); - // gulp map + private static List fourPiecePaths; + + private static List fivePiecePaths; + // map (gulp) private static Map> commandsMap = Map.of(0, noPiecePaths, 1, onePiecePaths, 2, twoPiecePaths, 3, threePiecePaths); @@ -114,8 +121,9 @@ public static enum StartPosition { private static SendableChooser startPositionChooser = new SendableChooser(); - private static SendableChooser availableAutos = new SendableChooser(); - private static GenericEntry amountGamePiecesEntry; + private static DynamicSendableChooser availableAutos = new DynamicSendableChooser(); + private static SendableChooser gameObjects = new SendableChooser(); + private static SendableChooser isVision = new SendableChooser(); private static GenericEntry isVisionEntry; public AutoLogic() { @@ -185,33 +193,50 @@ public static void initShuffleBoard() { for (StartPosition startPosition : StartPosition.values()) { startPositionChooser.addOption(startPosition.title, startPosition); } + isVision.setDefaultOption("Presets", false); + isVision.addOption("Vision", true); + gameObjects.setDefaultOption("0", 0); + for (int i = 1; i <= 5; i++) { + gameObjects.addOption(String.valueOf(i), i); + } + + tab.add("Starting Position", startPositionChooser).withPosition(8, 0).withSize(2, 1); + tab.add("Launch Type", isVision).withPosition(8, 2); + tab.add("Game Objects", gameObjects).withPosition(8, 2); + tab.add("Available Auto Variants", availableAutos).withPosition(9, 3).withSize(2, 1); - tab.add("Starting Position", startPositionChooser).withPosition(5, 1).withSize(2, 1); + isVision.onChange(AutoLogic::filterAutos); + startPositionChooser.onChange(AutoLogic::filterAutos); + gameObjects.onChange(AutoLogic::filterAutos); - isVisionEntry = - tab.add("Use Vision Launch", false) - .withWidget(BuiltInWidgets.kToggleSwitch) - .withPosition(5, 2) - .withSize(1, 1) - .getEntry(); - amountGamePiecesEntry = tab.add("Game Pieces", 0).withPosition(5, 2).withSize(1, 1).getEntry(); - tab.add("Available Auto Variants", availableAutos).withPosition(5, 3).withSize(2, 1); + filterAutos(startPositionChooser.getSelected()); } /** Takes the auto filtering entries in shuffleboard to provide a list of suitable autos */ - public static void filterAutos() { - availableAutos.close(); + public static void filterAutos(int numGameObjects) { + + for (AutoPath auto : availableAutos.m_map.values()) { + availableAutos.removeOption(auto); + } - List autoCommandsList = commandsMap.get((int) amountGamePiecesEntry.getInteger(0)); + List autoCommandsList = commandsMap.get(numGameObjects); // List filteredList = new ArrayList(); for (AutoPath auto : autoCommandsList) { if (auto.getStartPose().equals(startPositionChooser.getSelected().startPose) - && auto.isVision() == isVisionEntry.getBoolean(false)) { + && auto.isVision() == isVision.getSelected()) { // filteredList.add(auto); availableAutos.addOption(auto.getDisplayName(), auto); } } } + public static void filterAutos(StartPosition startPosition) { + filterAutos(gameObjects.getSelected()); + } + + public static void filterAutos(Boolean isVision) { + filterAutos(gameObjects.getSelected()); + } + } From bd18f17fb7e1173c86ed5e5e19b534d1c9c0764f Mon Sep 17 00:00:00 2001 From: kirby Date: Mon, 11 Mar 2024 21:32:22 -0700 Subject: [PATCH 08/49] fumble --- .../autos/PresetAmpSideAutoLine4Score.auto | 28 +- .../autos/PresetAmpSideAutolineFar4Score.auto | 61 ++++ .../autos/VisionAmpSide5Score.auto | 67 ++++ .../pathplanner/paths/BottomNoteLaunch.path | 24 +- .../pathplanner/paths/HighCenterSteal.path | 6 +- .../paths/HighSpeakerLowNoteReturn.path | 129 +++++++ .../paths/launchSecondNoteLaunch.path | 2 +- src/main/java/frc/team2412/robot/Robot.java | 25 +- .../robot/util/DynamicSendableChooser.java | 316 +++++++++--------- .../team2412/robot/util/MatchDashboard.java | 2 +- .../team2412/robot/util/auto/AutoLogic.java | 41 ++- 11 files changed, 462 insertions(+), 239 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score.auto create mode 100644 src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto create mode 100644 src/main/deploy/pathplanner/paths/HighSpeakerLowNoteReturn.path diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto index 31854b7b..c3cf25ce 100644 --- a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto @@ -11,36 +11,12 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "SubwooferLaunch" - } - }, { "type": "path", "data": { "pathName": "HighSpeakerHighNoteReturn" } }, - { - "type": "named", - "data": { - "name": "SubwooferLaunch" - } - }, - { - "type": "path", - "data": { - "pathName": "HighSpeakerCenterLineNoteReturn" - } - }, - { - "type": "named", - "data": { - "name": "SubwooferLaunch" - } - }, { "type": "path", "data": { @@ -48,9 +24,9 @@ } }, { - "type": "named", + "type": "path", "data": { - "name": "SubwooferLaunch" + "pathName": "HighSpeakerLowNoteReturn" } } ] diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score.auto new file mode 100644 index 00000000..31854b7b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerHighNoteReturn" + } + }, + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerCenterLineNoteReturn" + } + }, + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerHighNote2Return" + } + }, + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto new file mode 100644 index 00000000..c19c5ecd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerToHighNoteLaunch2" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighNoteLaunch2ToFarHighNoteToLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "launchSecondNoteCenterLineLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToMidNote" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path b/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path index 2d3093eb..9bfdbb86 100644 --- a/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.6561390233992404, - "y": 3.9793961091774825 + "x": 2.5847640884470957, + "y": 4.095403506122328 }, "isLocked": false, "linkedName": "Podium" @@ -36,25 +36,7 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "New Event Marker", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/HighCenterSteal.path b/src/main/deploy/pathplanner/paths/HighCenterSteal.path index 1174c7cb..19c83ed8 100644 --- a/src/main/deploy/pathplanner/paths/HighCenterSteal.path +++ b/src/main/deploy/pathplanner/paths/HighCenterSteal.path @@ -84,7 +84,7 @@ }, { "waypointRelativePos": 0.25, - "rotationDegrees": 14.22369918817612, + "rotationDegrees": -172.7651863860264, "rotateFast": false }, { @@ -97,9 +97,9 @@ { "name": "New Constraints Zone", "minWaypointRelativePos": 0.9, - "maxWaypointRelativePos": 1.5, + "maxWaypointRelativePos": 2.0, "constraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerLowNoteReturn.path b/src/main/deploy/pathplanner/paths/HighSpeakerLowNoteReturn.path new file mode 100644 index 00000000..6b291ae7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HighSpeakerLowNoteReturn.path @@ -0,0 +1,129 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.73, + "y": 6.62 + }, + "prevControl": null, + "nextControl": { + "x": 1.4199314885116257, + "y": 6.600718066300997 + }, + "isLocked": false, + "linkedName": "TopSpeaker" + }, + { + "anchor": { + "x": 1.7804749123011758, + "y": 5.898120625070079 + }, + "prevControl": { + "x": 1.514854202566117, + "y": 6.248257015175384 + }, + "nextControl": { + "x": 1.9838583821311784, + "y": 5.630024233021438 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7713929062062417, + "y": 4.07539893464585 + }, + "prevControl": { + "x": 2.7954992632163718, + "y": 4.0608860165816605 + }, + "nextControl": { + "x": 2.5385405725766406, + "y": 4.215584647385511 + }, + "isLocked": false, + "linkedName": "Podium" + }, + { + "anchor": { + "x": 2.0947948202202706, + "y": 5.18627848066507 + }, + "prevControl": { + "x": 2.3979195267252282, + "y": 4.703524318453471 + }, + "nextControl": { + "x": 1.8451878345198125, + "y": 5.583800717150984 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.73, + "y": 6.62 + }, + "prevControl": { + "x": 1.3274844567707151, + "y": 6.6931650980419075 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TopSpeaker" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.6, + "rotationDegrees": 180.0, + "rotateFast": false + }, + { + "waypointRelativePos": 2.2, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -120.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/launchSecondNoteLaunch.path b/src/main/deploy/pathplanner/paths/launchSecondNoteLaunch.path index 5df50cec..8be65e6f 100644 --- a/src/main/deploy/pathplanner/paths/launchSecondNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchSecondNoteLaunch.path @@ -49,7 +49,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0, + "waypointRelativePos": 0.95, "command": { "type": "parallel", "data": { diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 32f9d9ba..dadbbd65 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -48,7 +48,7 @@ public static Robot getInstance() { public MatchDashboard dashboard; public AutoLogic autoLogic; - public SendableChooser autoChooser; + // public SendableChooser autoChooser; protected Robot(RobotType type) { // non public for singleton. Protected so test class can subclass @@ -83,17 +83,19 @@ public void robotInit() { subsystems = new Subsystems(); controls = new Controls(subsystems); - autoLogic = new AutoLogic(); - - autoChooser = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); - SmartDashboard.putString("current bot", getTypeFromAddress().toString()); - AutoLogic.initShuffleBoard(); + + // autoChooser = AutoBuilder.buildAutoChooser(); if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { - autoChooser = AutoBuilder.buildAutoChooser(); - } else { - autoChooser = new SendableChooser<>(); + AutoLogic.initShuffleBoard(); } + AutoLogic.registerCommands(); + // SmartDashboard.putData("Auto Chooser", autoChooser); + SmartDashboard.putString("current bot", getTypeFromAddress().toString()); + // if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { + // autoChooser = AutoBuilder.buildAutoChooser(); + // } else { + // autoChooser = new SendableChooser<>(); + // } Shuffleboard.startRecording(); @@ -143,7 +145,8 @@ public void autonomousInit() { // Checks if FMS is attatched and enables joystick warning if true DriverStation.silenceJoystickConnectionWarning(!DriverStation.isFMSAttached()); - autoChooser.getSelected().schedule(); + // autoChooser.getSelected().schedule(); + AutoLogic.getSelected().schedule(); } @Override diff --git a/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java index e6d36e1f..5205e0eb 100644 --- a/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java +++ b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java @@ -2,170 +2,168 @@ import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.LinkedHashMap; import java.util.Map; import java.util.concurrent.atomic.AtomicInteger; import java.util.concurrent.locks.ReentrantLock; import java.util.function.Consumer; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.util.sendable.SendableRegistry; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - public class DynamicSendableChooser implements Sendable, AutoCloseable { - /** The key for the default value. */ - private static final String DEFAULT = "default"; - - /** The key for the selected option. */ - private static final String SELECTED = "selected"; - - /** The key for the active option. */ - private static final String ACTIVE = "active"; - - /** The key for the option array. */ - private static final String OPTIONS = "options"; - - /** The key for the instance number. */ - private static final String INSTANCE = ".instance"; - - /** A map linking strings to the objects they represent. */ - public final Map m_map = new LinkedHashMap<>(); - - private String m_defaultChoice = ""; - private final int m_instance; - private String m_previousVal; - private Consumer m_listener; - private static final AtomicInteger s_instances = new AtomicInteger(); - - /** Instantiates a {@link DynamicSendableChooser}. */ - @SuppressWarnings("this-escape") - public DynamicSendableChooser() { - m_instance = s_instances.getAndIncrement(); - SendableRegistry.add(this, "SendableChooser", m_instance); - } - - @Override - public void close() { - SendableRegistry.remove(this); - } - - /** - * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the - * object will appear as the given name. - * - * @param name the name of the option - * @param object the option - */ - public void addOption(String name, T object) { - m_map.put(name, object); - } - - /** - * Removes the given object from the list of options. - * - * @param object the option - */ - public void removeOption(T object) { - m_map.remove(object); - } - - /** - * Adds the given object to the list of options and marks it as the default. Functionally, this is - * very close to {@link #addOption(String, Object)} except that it will use this as the default - * option if none other is explicitly selected. - * - * @param name the name of the option - * @param object the option - */ - public void setDefaultOption(String name, T object) { - requireNonNullParam(name, "name", "setDefaultOption"); - - m_defaultChoice = name; - addOption(name, object); - } - - /** - * Returns the selected option. If there is none selected, it will return the default. If there is - * none selected and no default, then it will return {@code null}. - * - * @return the option selected - */ - public T getSelected() { - m_mutex.lock(); - try { - if (m_selected != null) { - return m_map.get(m_selected); - } else { - return m_map.get(m_defaultChoice); - } - } finally { - m_mutex.unlock(); - } - } - - /** - * Bind a listener that's called when the selected value changes. Only one listener can be bound. - * Calling this function will replace the previous listener. - * - * @param listener The function to call that accepts the new value - */ - public void onChange(Consumer listener) { - requireNonNullParam(listener, "listener", "onChange"); - m_mutex.lock(); - m_listener = listener; - m_mutex.unlock(); - } - - private String m_selected; - private final ReentrantLock m_mutex = new ReentrantLock(); - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("String Chooser"); - builder.publishConstInteger(INSTANCE, m_instance); - builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null); - builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null); - builder.addStringProperty( - ACTIVE, - () -> { - m_mutex.lock(); - try { - if (m_selected != null) { - return m_selected; - } else { - return m_defaultChoice; - } - } finally { - m_mutex.unlock(); - } - }, - null); - builder.addStringProperty( - SELECTED, - null, - val -> { - T choice; - Consumer listener; - m_mutex.lock(); - try { - m_selected = val; - if (!m_selected.equals(m_previousVal) && m_listener != null) { - choice = m_map.get(val); - listener = m_listener; - } else { - choice = null; - listener = null; - } - m_previousVal = val; - } finally { - m_mutex.unlock(); - } - if (listener != null) { - listener.accept(choice); - } - }); - } - -} \ No newline at end of file + /** The key for the default value. */ + private static final String DEFAULT = "default"; + + /** The key for the selected option. */ + private static final String SELECTED = "selected"; + + /** The key for the active option. */ + private static final String ACTIVE = "active"; + + /** The key for the option array. */ + private static final String OPTIONS = "options"; + + /** The key for the instance number. */ + private static final String INSTANCE = ".instance"; + + /** A map linking strings to the objects they represent. */ + public final Map m_map = new LinkedHashMap<>(); + + private String m_defaultChoice = ""; + private final int m_instance; + private String m_previousVal; + private Consumer m_listener; + private static final AtomicInteger s_instances = new AtomicInteger(); + + /** Instantiates a {@link DynamicSendableChooser}. */ + @SuppressWarnings("this-escape") + public DynamicSendableChooser() { + m_instance = s_instances.getAndIncrement(); + SendableRegistry.add(this, "SendableChooser", m_instance); + } + + @Override + public void close() { + SendableRegistry.remove(this); + } + + /** + * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the + * object will appear as the given name. + * + * @param name the name of the option + * @param object the option + */ + public void addOption(String name, T object) { + m_map.put(name, object); + } + + /** + * Removes the given object from the list of options. + * + * @param object the option + */ + public void removeOption(T object) { + m_map.remove(object); + } + + /** + * Adds the given object to the list of options and marks it as the default. Functionally, this is + * very close to {@link #addOption(String, Object)} except that it will use this as the default + * option if none other is explicitly selected. + * + * @param name the name of the option + * @param object the option + */ + public void setDefaultOption(String name, T object) { + requireNonNullParam(name, "name", "setDefaultOption"); + + m_defaultChoice = name; + addOption(name, object); + } + + /** + * Returns the selected option. If there is none selected, it will return the default. If there is + * none selected and no default, then it will return {@code null}. + * + * @return the option selected + */ + public T getSelected() { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_map.get(m_selected); + } else { + return m_map.get(m_defaultChoice); + } + } finally { + m_mutex.unlock(); + } + } + + /** + * Bind a listener that's called when the selected value changes. Only one listener can be bound. + * Calling this function will replace the previous listener. + * + * @param listener The function to call that accepts the new value + */ + public void onChange(Consumer listener) { + requireNonNullParam(listener, "listener", "onChange"); + m_mutex.lock(); + m_listener = listener; + m_mutex.unlock(); + } + + private String m_selected; + private final ReentrantLock m_mutex = new ReentrantLock(); + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("String Chooser"); + builder.publishConstInteger(INSTANCE, m_instance); + builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null); + builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null); + builder.addStringProperty( + ACTIVE, + () -> { + m_mutex.lock(); + try { + if (m_selected != null) { + return m_selected; + } else { + return m_defaultChoice; + } + } finally { + m_mutex.unlock(); + } + }, + null); + builder.addStringProperty( + SELECTED, + null, + val -> { + T choice; + Consumer listener; + m_mutex.lock(); + try { + m_selected = val; + if (!m_selected.equals(m_previousVal) && m_listener != null) { + choice = m_map.get(val); + listener = m_listener; + } else { + choice = null; + listener = null; + } + m_previousVal = val; + } finally { + m_mutex.unlock(); + } + if (listener != null) { + listener.accept(choice); + } + }); + } +} diff --git a/src/main/java/frc/team2412/robot/util/MatchDashboard.java b/src/main/java/frc/team2412/robot/util/MatchDashboard.java index c0849f1f..75899a9e 100644 --- a/src/main/java/frc/team2412/robot/util/MatchDashboard.java +++ b/src/main/java/frc/team2412/robot/util/MatchDashboard.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import frc.team2412.robot.Robot; import frc.team2412.robot.Subsystems; +import frc.team2412.robot.util.auto.AutoLogic; /* * Initializes and updates shuffleboard match dashboard entries @@ -20,7 +21,6 @@ public MatchDashboard(Subsystems s) { tab.add(new FMSWidget()).withPosition(0, 0).withSize(4, 1); tab.add(field).withPosition(0, 1).withSize(4, 3); Robot r = Robot.getInstance(); - tab.add("Auto Chooser", r.autoChooser).withPosition(4, 0).withSize(2, 1); AutonomousField.configureShuffleboardTab(tab, 6, 0, "Auto Chooser", r::addPeriodic); } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 9c0dc71c..6abb4f99 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -1,7 +1,5 @@ package frc.team2412.robot.util.auto; -import com.ctre.phoenix6.controls.StaticBrake; -import com.fasterxml.jackson.databind.ser.std.StdKeySerializers.Dynamic; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; @@ -9,7 +7,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -30,7 +27,6 @@ import frc.team2412.robot.commands.launcher.StopLauncherCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; import frc.team2412.robot.util.DynamicSendableChooser; - import java.util.List; import java.util.Map; @@ -94,14 +90,23 @@ public static enum StartPosition { private static List threePiecePaths = List.of( // presets + new AutoPath("Autoline N1 N2 N3", "PresetAmpSideAutoline4Score"), new AutoPath("Autoline N2 N3 N1", "PresetMidAutoline4Score"), + new AutoPath("Autoline N3 N2 N1", "PresetSourceSideAutoline4Score"), + new AutoPath("Centerline N1 Autoline Autoline N1 N2", "PresetAmpSideAutolineFar4Score"), // vision - new AutoPath("Autoline N1 CenterLine N1 N2", "VisionAmpSide4Score", true), + new AutoPath("Autoline N1 Centerline N1 N2", "VisionAmpSide4Score", true), new AutoPath("Autoline N1 N2 N3", "VisionAmpSideAutoLine4Score", true), - new AutoPath("Autoline N3 N2 N1", "VisionMid4Score", true)); + new AutoPath("Autoline N3 N2 N1", "VisionMid4Score", true), + new AutoPath("Autoline N3 N2 N1", "VisionSourceSideAutoLine4Score", true), + new AutoPath("Autoline N1 Centerline N1 N2", "VisionAmpSide4Score")); + + private static List fourPiecePaths = + List.of( + // vision + new AutoPath("Centerline N1 AutoLine N1 N2 N3", "VisionAmpSideAutoLine5Score"), + new AutoPath("Autoline N1 Centerline N1 N2 Autoline N2", "VisionAmpSide5Score")); - private static List fourPiecePaths; - private static List fivePiecePaths; // map (gulp) @@ -121,19 +126,17 @@ public static enum StartPosition { private static SendableChooser startPositionChooser = new SendableChooser(); - private static DynamicSendableChooser availableAutos = new DynamicSendableChooser(); + private static DynamicSendableChooser availableAutos = + new DynamicSendableChooser(); private static SendableChooser gameObjects = new SendableChooser(); private static SendableChooser isVision = new SendableChooser(); private static GenericEntry isVisionEntry; - public AutoLogic() { - registerCommands(); - } - /** Registers commands in PathPlanner */ - public void registerCommands() { + public static void registerCommands() { // param: String commandName, Command command + System.out.println("register "); // Intake NamedCommands.registerCommand("StopIntake", new IntakeStopCommand(s.intakeSubsystem)); NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem)); @@ -195,14 +198,14 @@ public static void initShuffleBoard() { } isVision.setDefaultOption("Presets", false); isVision.addOption("Vision", true); - gameObjects.setDefaultOption("0", 0); - for (int i = 1; i <= 5; i++) { + gameObjects.setDefaultOption("0", 0); + for (int i = 1; i <= 3; i++) { gameObjects.addOption(String.valueOf(i), i); } tab.add("Starting Position", startPositionChooser).withPosition(8, 0).withSize(2, 1); tab.add("Launch Type", isVision).withPosition(8, 2); - tab.add("Game Objects", gameObjects).withPosition(8, 2); + tab.add("Game Objects", gameObjects).withPosition(8, 2); tab.add("Available Auto Variants", availableAutos).withPosition(9, 3).withSize(2, 1); isVision.onChange(AutoLogic::filterAutos); @@ -231,6 +234,7 @@ public static void filterAutos(int numGameObjects) { } } } + public static void filterAutos(StartPosition startPosition) { filterAutos(gameObjects.getSelected()); } @@ -239,4 +243,7 @@ public static void filterAutos(Boolean isVision) { filterAutos(gameObjects.getSelected()); } + public static Command getSelected() { + return availableAutos.getSelected().getAutoCommand(); + } } From fac7bcb01eedfec51eeea3d958b4166803d1317e Mon Sep 17 00:00:00 2001 From: kirby Date: Tue, 12 Mar 2024 20:14:54 -0700 Subject: [PATCH 09/49] yay it work --- .../autos/PresetSourceSide1Score.auto | 8 +---- .../PresetSourceSide1ScorePassAutoline.auto | 31 ++++++++++++++++++ .../autos/PresetSourceSideAutoLine4Score.auto | 2 +- .../autos/VisionSourceSide3Score.auto | 2 +- .../autos/VisionSourceSideAutoLine4Score.auto | 2 +- .../autos/VisionSourceSideFar2Score.auto | 4 +-- .../pathplanner/paths/HighCenterSteal.path | 2 +- .../paths/HighSpeakerLowNoteReturn.path | 2 +- .../paths/HighSpeakerToHighNoteLaunch.path | 4 +-- .../paths/LowNoteToLowSpeaker.path | 14 ++++---- .../paths/LowSpeakerCenterLine.path | 4 +-- .../paths/LowSpeakerMidNote2Return.path | 4 +-- .../paths/LowSpeakerPassAutoLine.path | 4 +-- .../paths/LowSpeakerToHighNoteReturn.path | 2 +- .../paths/LowSpeakerToLowNote.path | 4 +-- .../paths/MiddleLowNoteToLowSpeaker.path | 4 +-- .../paths/TopSpeakerQCenterLineN1.path | 2 +- src/main/java/frc/team2412/robot/Robot.java | 3 +- .../robot/util/DynamicSendableChooser.java | 11 +++++-- .../team2412/robot/util/MatchDashboard.java | 2 +- .../team2412/robot/util/auto/AutoLogic.java | 32 +++++++++---------- .../team2412/robot/util/auto/AutoPath.java | 25 ++++++++++++--- 22 files changed, 108 insertions(+), 60 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/PresetSourceSide1ScorePassAutoline.auto diff --git a/src/main/deploy/pathplanner/autos/PresetSourceSide1Score.auto b/src/main/deploy/pathplanner/autos/PresetSourceSide1Score.auto index c83d529a..6fa131e5 100644 --- a/src/main/deploy/pathplanner/autos/PresetSourceSide1Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetSourceSide1Score.auto @@ -3,7 +3,7 @@ "startingPose": { "position": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "rotation": 120.0 }, @@ -16,12 +16,6 @@ "data": { "name": "SubwooferLaunch" } - }, - { - "type": "path", - "data": { - "pathName": "LowSpeakerCenterLine" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/PresetSourceSide1ScorePassAutoline.auto b/src/main/deploy/pathplanner/autos/PresetSourceSide1ScorePassAutoline.auto new file mode 100644 index 00000000..0aef4b66 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetSourceSide1ScorePassAutoline.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 4.47 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "LowSpeakerPassAutoLine" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetSourceSideAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/PresetSourceSideAutoLine4Score.auto index a636b97e..53a9f7f4 100644 --- a/src/main/deploy/pathplanner/autos/PresetSourceSideAutoLine4Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetSourceSideAutoLine4Score.auto @@ -3,7 +3,7 @@ "startingPose": { "position": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "rotation": 120.0 }, diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSide3Score.auto b/src/main/deploy/pathplanner/autos/VisionSourceSide3Score.auto index 49933a47..fdb17e31 100644 --- a/src/main/deploy/pathplanner/autos/VisionSourceSide3Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionSourceSide3Score.auto @@ -3,7 +3,7 @@ "startingPose": { "position": { "x": 0.73, - "y": 4.48 + "y": 4.47 }, "rotation": 120.0 }, diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSideAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/VisionSourceSideAutoLine4Score.auto index ec8d70f8..a7dbf162 100644 --- a/src/main/deploy/pathplanner/autos/VisionSourceSideAutoLine4Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionSourceSideAutoLine4Score.auto @@ -3,7 +3,7 @@ "startingPose": { "position": { "x": 0.73, - "y": 4.48 + "y": 4.47 }, "rotation": 120.0 }, diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSideFar2Score.auto b/src/main/deploy/pathplanner/autos/VisionSourceSideFar2Score.auto index d0587928..555cc4d3 100644 --- a/src/main/deploy/pathplanner/autos/VisionSourceSideFar2Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionSourceSideFar2Score.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8, - "y": 4.55 + "x": 0.73, + "y": 4.47 }, "rotation": 120.0 }, diff --git a/src/main/deploy/pathplanner/paths/HighCenterSteal.path b/src/main/deploy/pathplanner/paths/HighCenterSteal.path index 19c83ed8..cb813521 100644 --- a/src/main/deploy/pathplanner/paths/HighCenterSteal.path +++ b/src/main/deploy/pathplanner/paths/HighCenterSteal.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.7207761529808778, + "x": 1.7207761529808776, "y": 6.857329003229436 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerLowNoteReturn.path b/src/main/deploy/pathplanner/paths/HighSpeakerLowNoteReturn.path index 6b291ae7..bd228207 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerLowNoteReturn.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerLowNoteReturn.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.4199314885116257, + "x": 1.4199314885116259, "y": 6.600718066300997 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch.path b/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch.path index b569cf4a..fdd3fbb0 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch.path @@ -4,12 +4,12 @@ { "anchor": { "x": 0.73, - "y": 6.63 + "y": 6.62 }, "prevControl": null, "nextControl": { "x": 1.0476290014881247, - "y": 6.886930039136037 + "y": 6.876930039136037 }, "isLocked": false, "linkedName": "TopSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path index 539e4dda..130bb887 100644 --- a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.77, - "y": 4.08 + "x": 0.73, + "y": 4.47 }, "prevControl": null, "nextControl": { - "x": 2.3966676963984557, - "y": 4.103137615644601 + "x": 0.3566676963984554, + "y": 4.493137615644601 }, "isLocked": false, - "linkedName": null + "linkedName": "LowSpeaker" }, { "anchor": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "prevControl": { "x": 1.1980586123334407, - "y": 3.964467068033235 + "y": 3.974467068033235 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path b/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path index 30af0d1c..15be5d2c 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path @@ -4,12 +4,12 @@ { "anchor": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "prevControl": null, "nextControl": { "x": 1.689169744135214, - "y": 2.3204424556117518 + "y": 2.3304424556117516 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path b/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path index 4d18958b..648629dc 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path @@ -33,11 +33,11 @@ { "anchor": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "prevControl": { "x": 1.679419985918358, - "y": 3.9389023196098654 + "y": 3.948902319609865 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path b/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path index c270d58a..21e23e74 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path @@ -4,12 +4,12 @@ { "anchor": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "prevControl": null, "nextControl": { "x": 1.2211813497261212, - "y": 3.05167432187596 + "y": 3.0616743218759597 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path b/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path index c89fea9a..23c01db3 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path @@ -36,7 +36,7 @@ "y": 4.47 }, "prevControl": { - "x": 2.0791600728094584, + "x": 2.079160072809459, "y": 3.3149177937310745 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path index a8833ea8..179770b8 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path @@ -4,12 +4,12 @@ { "anchor": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "prevControl": null, "nextControl": { "x": 1.6986020361229917, - "y": 4.087669396600055 + "y": 4.097669396600055 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path index a493f90b..3348acba 100644 --- a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path @@ -33,11 +33,11 @@ { "anchor": { "x": 0.73, - "y": 4.46 + "y": 4.47 }, "prevControl": { "x": 1.4899471997783693, - "y": 3.0719258673640133 + "y": 3.081925867364013 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/TopSpeakerQCenterLineN1.path b/src/main/deploy/pathplanner/paths/TopSpeakerQCenterLineN1.path index 47993557..42d480e4 100644 --- a/src/main/deploy/pathplanner/paths/TopSpeakerQCenterLineN1.path +++ b/src/main/deploy/pathplanner/paths/TopSpeakerQCenterLineN1.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.7104749123011758, + "x": 1.7104749123011755, "y": 7.595188383710101 }, "isLocked": false, diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index dadbbd65..18bfb324 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -84,11 +84,12 @@ public void robotInit() { subsystems = new Subsystems(); controls = new Controls(subsystems); + AutoLogic.registerCommands(); // autoChooser = AutoBuilder.buildAutoChooser(); if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { AutoLogic.initShuffleBoard(); } - AutoLogic.registerCommands(); + // SmartDashboard.putData("Auto Chooser", autoChooser); SmartDashboard.putString("current bot", getTypeFromAddress().toString()); // if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { diff --git a/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java index 5205e0eb..c8d0ddd1 100644 --- a/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java +++ b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java @@ -66,8 +66,15 @@ public void addOption(String name, T object) { * * @param object the option */ - public void removeOption(T object) { - m_map.remove(object); + public void removeOption(String name) { + m_map.remove(name); + } + + /** + * Removes all objects from the list of options + */ + public void clearOptions() { + m_map.clear(); } /** diff --git a/src/main/java/frc/team2412/robot/util/MatchDashboard.java b/src/main/java/frc/team2412/robot/util/MatchDashboard.java index 75899a9e..588bd9ae 100644 --- a/src/main/java/frc/team2412/robot/util/MatchDashboard.java +++ b/src/main/java/frc/team2412/robot/util/MatchDashboard.java @@ -21,6 +21,6 @@ public MatchDashboard(Subsystems s) { tab.add(new FMSWidget()).withPosition(0, 0).withSize(4, 1); tab.add(field).withPosition(0, 1).withSize(4, 3); Robot r = Robot.getInstance(); - AutonomousField.configureShuffleboardTab(tab, 6, 0, "Auto Chooser", r::addPeriodic); + AutonomousField.configureShuffleboardTab(tab, 6, 0, "Available Auto Variants", r::addPeriodic); } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 6abb4f99..f0b857c5 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -37,13 +37,13 @@ public class AutoLogic { public static enum StartPosition { AMP_SIDE_SUBWOOFER( - " Amp Side Subwoofer", + "Amp Side Subwoofer", new Pose2d(0.73, 6.62, new Rotation2d(Units.degreesToRadians(-120)))), MID_SIDE_SUBWOOFER( "Mid Side Subwoofer", new Pose2d(1.33, 5.55, new Rotation2d(Units.degreesToRadians(180)))), SOURCE_SIDE_SUBWOOFER( "Source Side Subwoofer", - new Pose2d(0.73, 4.46, new Rotation2d(Units.degreesToRadians(120)))), + new Pose2d(0.73, 4.47, new Rotation2d(Units.degreesToRadians(120)))), MISC("Misc", null); String title; @@ -65,7 +65,8 @@ public static enum StartPosition { new AutoPath("Stand Still", "PresetMid1Score"), new AutoPath("Stand Still", "PresetAmpSide1Score"), // new AutoPath("Pass Auto Line", "PresetSourceSide1ScorePassAutoLine"), - new AutoPath("Pass Auto Line", "PresetAmpSide1ScorePassAutoLine")); + new AutoPath("Pass Auto Line", "PresetAmpSide1ScorePassAutoLine"), + new AutoPath("Pass Auto Line", "PresetSourceSide1ScorePassAutoline")); private static List onePiecePaths = List.of( @@ -111,7 +112,7 @@ public static enum StartPosition { // map (gulp) private static Map> commandsMap = - Map.of(0, noPiecePaths, 1, onePiecePaths, 2, twoPiecePaths, 3, threePiecePaths); + Map.of(0, noPiecePaths, 1, onePiecePaths, 2, twoPiecePaths, 3, threePiecePaths, 4, fourPiecePaths); // vars @@ -130,13 +131,11 @@ public static enum StartPosition { new DynamicSendableChooser(); private static SendableChooser gameObjects = new SendableChooser(); private static SendableChooser isVision = new SendableChooser(); - private static GenericEntry isVisionEntry; /** Registers commands in PathPlanner */ public static void registerCommands() { // param: String commandName, Command command - System.out.println("register "); // Intake NamedCommands.registerCommand("StopIntake", new IntakeStopCommand(s.intakeSubsystem)); NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem)); @@ -199,14 +198,14 @@ public static void initShuffleBoard() { isVision.setDefaultOption("Presets", false); isVision.addOption("Vision", true); gameObjects.setDefaultOption("0", 0); - for (int i = 1; i <= 3; i++) { + for (int i = 1; i < commandsMap.size(); i++) { gameObjects.addOption(String.valueOf(i), i); } tab.add("Starting Position", startPositionChooser).withPosition(8, 0).withSize(2, 1); - tab.add("Launch Type", isVision).withPosition(8, 2); - tab.add("Game Objects", gameObjects).withPosition(8, 2); - tab.add("Available Auto Variants", availableAutos).withPosition(9, 3).withSize(2, 1); + tab.add("Launch Type", isVision).withPosition(8, 1); + tab.add("Game Objects", gameObjects).withPosition(9, 1); + tab.add("Available Auto Variants", availableAutos).withPosition(8, 2).withSize(2, 1); isVision.onChange(AutoLogic::filterAutos); startPositionChooser.onChange(AutoLogic::filterAutos); @@ -218,18 +217,17 @@ public static void initShuffleBoard() { /** Takes the auto filtering entries in shuffleboard to provide a list of suitable autos */ public static void filterAutos(int numGameObjects) { - for (AutoPath auto : availableAutos.m_map.values()) { - availableAutos.removeOption(auto); - } + // resets/clears all options + availableAutos.clearOptions(); + + // filter based off gameobejct count List autoCommandsList = commandsMap.get(numGameObjects); - // List filteredList = new ArrayList(); - + // filter more then add to chooser for (AutoPath auto : autoCommandsList) { - if (auto.getStartPose().equals(startPositionChooser.getSelected().startPose) + if (auto.getStartPose().equals(startPositionChooser.getSelected()) && auto.isVision() == isVision.getSelected()) { - // filteredList.add(auto); availableAutos.addOption(auto.getDisplayName(), auto); } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java index ab5631ce..a8dba6cf 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -1,30 +1,47 @@ package frc.team2412.robot.util.auto; +import static frc.team2412.robot.util.auto.AutoLogic.s; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.util.auto.AutoLogic.StartPosition; public class AutoPath { - private Pose2d startingPose; + private Pose2d startPose2d; + private StartPosition startPosition; private String displayName; private Command autoCommand; private boolean vision; public AutoPath(String displayName, String pathPlannerAutoName, boolean vision) { this.displayName = displayName; - startingPose = PathPlannerAuto.getStaringPoseFromAutoFile(pathPlannerAutoName); + startPose2d = PathPlannerAuto.getStaringPoseFromAutoFile(pathPlannerAutoName); autoCommand = AutoBuilder.buildAuto(pathPlannerAutoName); this.vision = vision; + + for (StartPosition pos : StartPosition.values()) { + if (startPose2d.equals(pos.startPose)) { + startPosition = pos; + return; + } + } + startPosition = StartPosition.MISC; + } public AutoPath(String displayName, String pathPlannerAutoName) { this(displayName, pathPlannerAutoName, false); } - public Pose2d getStartPose() { - return startingPose; + public StartPosition getStartPose() { + return startPosition; + } + + public Pose2d getStartPose2d() { + return startPose2d; } public String getDisplayName() { From 0c733e1534a459ad3b573a8dd667e2adbbf29764 Mon Sep 17 00:00:00 2001 From: kirby Date: Tue, 12 Mar 2024 20:17:19 -0700 Subject: [PATCH 10/49] spotless boohoo hoo --- src/main/java/frc/team2412/robot/Robot.java | 8 +++--- .../robot/util/DynamicSendableChooser.java | 4 +-- .../team2412/robot/util/MatchDashboard.java | 1 - .../team2412/robot/util/auto/AutoLogic.java | 26 ++++++++++++------- .../team2412/robot/util/auto/AutoPath.java | 3 --- 5 files changed, 21 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 18bfb324..926f893f 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -1,6 +1,5 @@ package frc.team2412.robot; -import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; @@ -10,7 +9,6 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -83,13 +81,13 @@ public void robotInit() { subsystems = new Subsystems(); controls = new Controls(subsystems); - + AutoLogic.registerCommands(); // autoChooser = AutoBuilder.buildAutoChooser(); if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { - AutoLogic.initShuffleBoard(); + AutoLogic.initShuffleBoard(); } - + // SmartDashboard.putData("Auto Chooser", autoChooser); SmartDashboard.putString("current bot", getTypeFromAddress().toString()); // if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { diff --git a/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java index c8d0ddd1..e0c2ef47 100644 --- a/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java +++ b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java @@ -70,9 +70,7 @@ public void removeOption(String name) { m_map.remove(name); } - /** - * Removes all objects from the list of options - */ + /** Removes all objects from the list of options */ public void clearOptions() { m_map.clear(); } diff --git a/src/main/java/frc/team2412/robot/util/MatchDashboard.java b/src/main/java/frc/team2412/robot/util/MatchDashboard.java index 588bd9ae..2985cfdb 100644 --- a/src/main/java/frc/team2412/robot/util/MatchDashboard.java +++ b/src/main/java/frc/team2412/robot/util/MatchDashboard.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import frc.team2412.robot.Robot; import frc.team2412.robot.Subsystems; -import frc.team2412.robot.util.auto.AutoLogic; /* * Initializes and updates shuffleboard match dashboard entries diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index f0b857c5..02ea1545 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -37,8 +36,7 @@ public class AutoLogic { public static enum StartPosition { AMP_SIDE_SUBWOOFER( - "Amp Side Subwoofer", - new Pose2d(0.73, 6.62, new Rotation2d(Units.degreesToRadians(-120)))), + "Amp Side Subwoofer", new Pose2d(0.73, 6.62, new Rotation2d(Units.degreesToRadians(-120)))), MID_SIDE_SUBWOOFER( "Mid Side Subwoofer", new Pose2d(1.33, 5.55, new Rotation2d(Units.degreesToRadians(180)))), SOURCE_SIDE_SUBWOOFER( @@ -94,7 +92,7 @@ public static enum StartPosition { new AutoPath("Autoline N1 N2 N3", "PresetAmpSideAutoline4Score"), new AutoPath("Autoline N2 N3 N1", "PresetMidAutoline4Score"), new AutoPath("Autoline N3 N2 N1", "PresetSourceSideAutoline4Score"), - new AutoPath("Centerline N1 Autoline Autoline N1 N2", "PresetAmpSideAutolineFar4Score"), + new AutoPath("Centerline N1 Autoline Autoline N1 N2", "PresetAmpSideAutolineFar4Score"), // vision new AutoPath("Autoline N1 Centerline N1 N2", "VisionAmpSide4Score", true), new AutoPath("Autoline N1 N2 N3", "VisionAmpSideAutoLine4Score", true), @@ -109,10 +107,20 @@ public static enum StartPosition { new AutoPath("Autoline N1 Centerline N1 N2 Autoline N2", "VisionAmpSide5Score")); private static List fivePiecePaths; - // map (gulp) + // map (gulp) private static Map> commandsMap = - Map.of(0, noPiecePaths, 1, onePiecePaths, 2, twoPiecePaths, 3, threePiecePaths, 4, fourPiecePaths); + Map.of( + 0, + noPiecePaths, + 1, + onePiecePaths, + 2, + twoPiecePaths, + 3, + threePiecePaths, + 4, + fourPiecePaths); // vars @@ -198,7 +206,7 @@ public static void initShuffleBoard() { isVision.setDefaultOption("Presets", false); isVision.addOption("Vision", true); gameObjects.setDefaultOption("0", 0); - for (int i = 1; i < commandsMap.size(); i++) { + for (int i = 1; i < commandsMap.size(); i++) { gameObjects.addOption(String.valueOf(i), i); } @@ -217,10 +225,9 @@ public static void initShuffleBoard() { /** Takes the auto filtering entries in shuffleboard to provide a list of suitable autos */ public static void filterAutos(int numGameObjects) { - // resets/clears all options availableAutos.clearOptions(); - + // filter based off gameobejct count List autoCommandsList = commandsMap.get(numGameObjects); @@ -233,6 +240,7 @@ public static void filterAutos(int numGameObjects) { } } + // auto filtering stuff public static void filterAutos(StartPosition startPosition) { filterAutos(gameObjects.getSelected()); } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java index a8dba6cf..550f42c9 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -1,7 +1,5 @@ package frc.team2412.robot.util.auto; -import static frc.team2412.robot.util.auto.AutoLogic.s; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Pose2d; @@ -29,7 +27,6 @@ public AutoPath(String displayName, String pathPlannerAutoName, boolean vision) } } startPosition = StartPosition.MISC; - } public AutoPath(String displayName, String pathPlannerAutoName) { From fc0b2ddcd3865919433a03f908a09301ff4ba02f Mon Sep 17 00:00:00 2001 From: kirby Date: Tue, 12 Mar 2024 20:19:40 -0700 Subject: [PATCH 11/49] added final --- .../frc/team2412/robot/util/auto/AutoPath.java | 10 +++++----- .../team2412/robot/util/auto/ComplexAutoPaths.java | 14 +++++++------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java index 550f42c9..d7f4fff8 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -8,11 +8,11 @@ public class AutoPath { - private Pose2d startPose2d; - private StartPosition startPosition; - private String displayName; - private Command autoCommand; - private boolean vision; + private final Pose2d startPose2d; + private final StartPosition startPosition; + private final String displayName; + private final Command autoCommand; + private final boolean vision; public AutoPath(String displayName, String pathPlannerAutoName, boolean vision) { this.displayName = displayName; diff --git a/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java index 752462ad..670ab43f 100644 --- a/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java +++ b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java @@ -38,7 +38,7 @@ public class ComplexAutoPaths { // Complex Autos - public static Command TopSpeakerCenterLineN1N2AutoLine1 = + public static final Command TopSpeakerCenterLineN1N2AutoLine1 = Commands.sequence( SubwooferLaunchCommand(), getAutoCommand("TopSpeakerQCenterLineN1"), @@ -59,7 +59,7 @@ public class ComplexAutoPaths { () -> true), VisionLaunchCommand()); - public static Command TopSpeakerCenterLineN1N2N3 = + public static final Command TopSpeakerCenterLineN1N2N3 = Commands.sequence( SubwooferLaunchCommand(), getAutoCommand("TopSpeakerQCenterLineN1"), @@ -80,7 +80,7 @@ public class ComplexAutoPaths { () -> true), VisionLaunchCommand()); - public static Command midSpeakerCenterLineN3N2N1 = + public static final Command midSpeakerCenterLineN3N2N1 = Commands.sequence( SubwooferLaunchCommand(), getAutoCommand("MidSpeakerQCenterLineN3"), @@ -102,7 +102,7 @@ public class ComplexAutoPaths { () -> true), VisionLaunchCommand()); - public static Command lowSpeakerCenterLineN5N4 = + public static final Command lowSpeakerCenterLineN5N4 = Commands.sequence( SubwooferLaunchCommand(), getAutoCommand("LowSpeakerQCenterLineN5"), @@ -114,7 +114,7 @@ public class ComplexAutoPaths { Commands.sequence(getAutoCommand("QCenterLineN5LCenterLineN4")), () -> false)); - public static Command lowSpeakerCenterLineN5N4N3 = + public static final Command lowSpeakerCenterLineN5N4N3 = Commands.sequence( SubwooferLaunchCommand(), getAutoCommand("LowSpeakerQCenterLineN5"), @@ -137,11 +137,11 @@ public class ComplexAutoPaths { // new command getters - public static Command VisionLaunchCommand() { + public static final Command VisionLaunchCommand() { return new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls); } - public static Command SubwooferLaunchCommand() { + public static final Command SubwooferLaunchCommand() { return new SetAngleLaunchCommand( s.launcherSubsystem, LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, From 56bb7f8c112c335cd0b518266df6b6e7bb8154aa Mon Sep 17 00:00:00 2001 From: kirby Date: Tue, 12 Mar 2024 20:20:27 -0700 Subject: [PATCH 12/49] debug --- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 02ea1545..29e988dc 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -63,7 +63,7 @@ public static enum StartPosition { new AutoPath("Stand Still", "PresetMid1Score"), new AutoPath("Stand Still", "PresetAmpSide1Score"), // new AutoPath("Pass Auto Line", "PresetSourceSide1ScorePassAutoLine"), - new AutoPath("Pass Auto Line", "PresetAmpSide1ScorePassAutoLine"), + new AutoPath("Pass Auto Line", "PresetAmpSide1ScorePassAutoline"), new AutoPath("Pass Auto Line", "PresetSourceSide1ScorePassAutoline")); private static List onePiecePaths = From bcee266ecb6a0ba8b60f2d3002f250b20331fff0 Mon Sep 17 00:00:00 2001 From: kirby Date: Tue, 12 Mar 2024 20:23:30 -0700 Subject: [PATCH 13/49] debug 2 --- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 29e988dc..84af55c5 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -79,7 +79,7 @@ public static enum StartPosition { private static List twoPiecePaths = List.of( // presets - new AutoPath("Autoline N1", "PresetAmpSideAutoline3Score"), + new AutoPath("Autoline N1 Centerline N1", "PresetAmpSideAutoline3Score"), new AutoPath("Autoline N2", "PresetMidAutoline3Score"), // vision new AutoPath("Centerline N5 N4", "VisionSourceSide3Score", true), From 7b45642f398832b6da08c9247fb6b8de119c45a2 Mon Sep 17 00:00:00 2001 From: kirby Date: Tue, 12 Mar 2024 21:27:20 -0700 Subject: [PATCH 14/49] making autos beter --- .../deploy/pathplanner/autos/New Auto.auto | 37 +++++++ .../VisionSourceSide3ScoreUnderStage.auto | 49 +++++++++ .../pathplanner/paths/HighCenterSteal.path | 8 +- ...LowLaunchNoteFourCenterLineUnderStage.path | 102 ++++++++++++++++++ .../paths/LowSpeakerToMiddleLowNote.path | 63 ++++++----- ...path => MidNoteToCenterMidNodeLaunch.path} | 60 +++++------ .../paths/MiddleLowNoteToLaunch.path | 14 +-- .../launchPosSecondCenterLineNoteLaunch.path | 23 ++-- 8 files changed, 273 insertions(+), 83 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/VisionSourceSide3ScoreUnderStage.auto create mode 100644 src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path rename src/main/deploy/pathplanner/paths/{lowLaunchNoteFourCenterLine.path => MidNoteToCenterMidNodeLaunch.path} (62%) diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 00000000..cff2b961 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.33, + "y": 5.55 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerToMidNote" + } + }, + { + "type": "path", + "data": { + "pathName": "MidNoteToCenterMidNodeLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "launchPosSecondCenterLineNoteLaunch" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSide3ScoreUnderStage.auto b/src/main/deploy/pathplanner/autos/VisionSourceSide3ScoreUnderStage.auto new file mode 100644 index 00000000..545a6232 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/VisionSourceSide3ScoreUnderStage.auto @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 4.47 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LowSpeakerToMiddleLowNote" + } + }, + { + "type": "path", + "data": { + "pathName": "MiddleLowNoteToLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "LowLaunchNoteFourCenterLineUnderStage" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HighCenterSteal.path b/src/main/deploy/pathplanner/paths/HighCenterSteal.path index cb813521..95c0b7a9 100644 --- a/src/main/deploy/pathplanner/paths/HighCenterSteal.path +++ b/src/main/deploy/pathplanner/paths/HighCenterSteal.path @@ -52,12 +52,12 @@ "y": 4.132382318818692 }, "prevControl": { - "x": 6.456562495789792, - "y": 4.119496578235857 + "x": 6.787637867361823, + "y": 4.062596022400948 }, "nextControl": { - "x": 5.062344539103492, - "y": 4.141627021992783 + "x": 4.8985812257338806, + "y": 4.177945498748141 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path new file mode 100644 index 00000000..01aa2e05 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path @@ -0,0 +1,102 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.91, + "y": 1.95 + }, + "prevControl": null, + "nextControl": { + "x": 5.250408215102145, + "y": 1.2900322943503038 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.3, + "y": 2.4481294676875076 + }, + "prevControl": { + "x": 8.337533809215634, + "y": 1.7307971969497182 + }, + "nextControl": { + "x": 8.240937624732393, + "y": 3.5769077252440225 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9896948365325304, + "y": 5.321383214195 + }, + "prevControl": { + "x": 5.059835262527669, + "y": 3.9580536303929756 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": 146.44882381880868, + "rotateFast": false + }, + { + "waypointRelativePos": 1.65, + "rotationDegrees": 176.0, + "rotateFast": false + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": 162.59131699825656, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 176.97498809243632, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 124.79, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path index 0c370098..f82cbb29 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path @@ -8,51 +8,35 @@ }, "prevControl": null, "nextControl": { - "x": 1.031653955199802, - "y": 3.6786365664623215 + "x": 1.6002032004064015, + "y": 1.5832214521571912 }, "isLocked": false, "linkedName": "LowSpeaker" }, { "anchor": { - "x": 3.034636378061356, - "y": 1.5112125236126965 + "x": 6.97, + "y": 0.8 }, "prevControl": { - "x": 1.7459946013496772, - "y": 2.4864008951782903 + "x": 4.942164998549343, + "y": 0.8 }, "nextControl": { - "x": 4.116859540132383, - "y": 0.6922328333967845 + "x": 7.077491190809847, + "y": 0.8 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.970353683931332, - "y": 0.8080477216589235 - }, - "prevControl": { - "x": 6.689293606832576, - "y": 0.8080477216589235 - }, - "nextControl": { - "x": 7.07784487474118, - "y": 0.8080477216589235 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 9.011238164994147, + "x": 9.01, "y": 0.81 }, "prevControl": { - "x": 8.449118010796635, + "x": 8.447879845802488, "y": 0.81 }, "nextControl": null, @@ -62,18 +46,31 @@ ], "rotationTargets": [ { - "waypointRelativePos": 2.6, - "rotationDegrees": 180.0, - "rotateFast": false - }, - { - "waypointRelativePos": 3, + "waypointRelativePos": 1.35, "rotationDegrees": 180.0, "rotateFast": false } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.85, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/lowLaunchNoteFourCenterLine.path b/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path similarity index 62% rename from src/main/deploy/pathplanner/paths/lowLaunchNoteFourCenterLine.path rename to src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path index fcdf0c5d..77155f74 100644 --- a/src/main/deploy/pathplanner/paths/lowLaunchNoteFourCenterLine.path +++ b/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path @@ -3,73 +3,73 @@ "waypoints": [ { "anchor": { - "x": 2.91, - "y": 1.95 + "x": 2.9, + "y": 5.55 }, "prevControl": null, "nextControl": { - "x": 3.91, - "y": 1.9500000000000002 + "x": 3.9067566423421134, + "y": 5.445130169539619 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.288745946861262, - "y": 2.477580450656397 + "x": 5.059835262527669, + "y": 4.265902246090207 }, "prevControl": { - "x": 6.088506591427597, - "y": 1.1740773031095617 + "x": 4.106065843242836, + "y": 4.566441586006087 }, "nextControl": { - "x": 8.906117742547979, - "y": 2.843334329613654 + "x": 6.137894028707098, + "y": 3.926198473430942 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.5784758596544215, - "y": 1.78422771259957 + "x": 8.3, + "y": 4.104648209296419 }, "prevControl": { - "x": 7.314430903909075, - "y": 1.8818544021435546 + "x": 8.202270809898764, + "y": 4.4595947557966555 }, "nextControl": { - "x": 5.6724949485935, - "y": 1.6640465713363868 + "x": 8.397729190101238, + "y": 3.749701662796183 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.008448377257114, - "y": 2.2002393554336663 + "x": 5.589292620026682, + "y": 4.265902246090207 }, "prevControl": { - "x": 4.979142210536673, - "y": 1.8027171189477522 + "x": 6.35984666440237, + "y": 3.9957190898956214 }, "nextControl": { - "x": 2.6107545939221435, - "y": 2.7726282381327487 + "x": 4.757269334358488, + "y": 4.55763866482688 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.30385717914293, - "y": 4.0606698356253865 + "x": 4.44, + "y": 5.78 }, "prevControl": { - "x": 2.4797706738270624, - "y": 3.3423563989985126 + "x": 4.4337047232653015, + "y": 5.001384417183249 }, "nextControl": null, "isLocked": false, @@ -81,7 +81,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.6, + "waypointRelativePos": 0, "command": { "type": "parallel", "data": { @@ -89,7 +89,7 @@ { "type": "named", "data": { - "name": "Intake" + "name": "VisionLaunch" } } ] @@ -105,13 +105,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 140.0, + "rotation": 180.0, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 124.79, + "rotation": 180.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path index 042878f6..fd501f89 100644 --- a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path +++ b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 8.3, - "y": 0.8 + "x": 9.01, + "y": 0.81 }, "prevControl": null, "nextControl": { - "x": 5.795424362277297, - "y": 1.0847998838854824 + "x": 6.502808157044885, + "y": 1.2707133785696143 }, "isLocked": false, - "linkedName": null + "linkedName": "CenterLineN5" }, { "anchor": { @@ -20,8 +20,8 @@ "y": 1.9539173222406319 }, "prevControl": { - "x": 4.517435320584927, - "y": 1.275372836459959 + "x": 4.649370441598027, + "y": 1.5685619942668465 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path b/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path index 6c3f157c..4d963f3b 100644 --- a/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 4.532094778475272, - "y": 6.303566892848072 + "x": 4.572375270876667, + "y": 6.35111108060054 }, "isLocked": false, "linkedName": null @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 4.004354294422875, - "y": 6.640734424325992 + "x": 5.478356181937588, + "y": 6.434313409167359 }, "prevControl": { - "x": 3.3446786893573788, - "y": 6.787329003229436 + "x": 6.837327548528969, + "y": 6.332621674252358 }, "nextControl": null, "isLocked": false, @@ -62,8 +62,13 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.5, - "rotationDegrees": 130.88664561136105, + "waypointRelativePos": 1.6, + "rotationDegrees": 149.98, + "rotateFast": false + }, + { + "waypointRelativePos": 2.0999999999999996, + "rotationDegrees": 151.4886678965681, "rotateFast": false } ], @@ -95,7 +100,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -162.8972710309476, + "rotation": -170.32327316993567, "rotateFast": false }, "reversed": false, From 6ae0ef5fac9eec22bfbb51d0fb9494998f3dadc2 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Thu, 14 Mar 2024 12:35:29 -0700 Subject: [PATCH 15/49] start position within tolerance code --- .../team2412/robot/util/auto/AutoLogic.java | 4 ++-- .../team2412/robot/util/auto/AutoPath.java | 21 ++++++++++++++++++- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 84af55c5..16064ac1 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -44,8 +44,8 @@ public static enum StartPosition { new Pose2d(0.73, 4.47, new Rotation2d(Units.degreesToRadians(120)))), MISC("Misc", null); - String title; - Pose2d startPose; + String title; // for shuffleboard display + Pose2d startPose; // for identifying path's starting positions for filtering StartPosition(String title, Pose2d startPose) { this.title = title; diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java index d7f4fff8..f66fb073 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -2,6 +2,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Command; import frc.team2412.robot.util.auto.AutoLogic.StartPosition; @@ -20,8 +21,10 @@ public AutoPath(String displayName, String pathPlannerAutoName, boolean vision) autoCommand = AutoBuilder.buildAuto(pathPlannerAutoName); this.vision = vision; + // in the case that the auto for whatever reason's starting pose is slightly off, + // is still able to match with a startPosition if it is close enough for (StartPosition pos : StartPosition.values()) { - if (startPose2d.equals(pos.startPose)) { + if (matchesStartPosition(pos)) { startPosition = pos; return; } @@ -52,4 +55,20 @@ public Command getAutoCommand() { public boolean isVision() { return vision; } + + /** + * Checks the x, y, and rotation of the auto's starting position and compares it with the expected + * starting position, returning true if it is considered close enough to be the same. + * + * @param expectedStartPosition + * @return if it is matching + */ + public boolean matchesStartPosition(StartPosition expectedStartPosition) { + return (MathUtil.isNear(expectedStartPosition.startPose.getX(), startPose2d.getX(), .05) + && MathUtil.isNear(expectedStartPosition.startPose.getY(), startPose2d.getY(), .05) + && MathUtil.isNear( + expectedStartPosition.startPose.getRotation().getDegrees(), + startPose2d.getRotation().getDegrees(), + 5)); + } } From 1696d89f3a672a64b3b23b7e49b9bb5415d77d78 Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 16 Mar 2024 13:27:43 -0700 Subject: [PATCH 16/49] debug --- .../pathplanner/autos/5mForwardRotate180.auto | 25 +++++ ...ore.auto => LowSpeakerCenterLineN5N4.auto} | 0 .../{New Auto.auto => MidFar4Score.auto} | 0 .../autos/VisionSourceSide3Score.auto | 2 +- .../pathplanner/paths/5mForwardRotate.path | 52 ++++++++++ .../paths/LowLaunchNoteFourCenterLine.path | 97 +++++++++++++++++++ .../paths/LowNoteToLowSpeaker.path | 10 +- src/main/java/frc/team2412/robot/Robot.java | 5 +- .../robot/subsystems/DrivebaseSubsystem.java | 2 +- .../team2412/robot/util/auto/AutoLogic.java | 3 + .../team2412/robot/util/auto/AutoPath.java | 12 ++- 11 files changed, 196 insertions(+), 12 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/5mForwardRotate180.auto rename src/main/deploy/pathplanner/autos/{PresetAmpSideFar+AutoLine4Score.auto => LowSpeakerCenterLineN5N4.auto} (100%) rename src/main/deploy/pathplanner/autos/{New Auto.auto => MidFar4Score.auto} (100%) create mode 100644 src/main/deploy/pathplanner/paths/5mForwardRotate.path create mode 100644 src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path diff --git a/src/main/deploy/pathplanner/autos/5mForwardRotate180.auto b/src/main/deploy/pathplanner/autos/5mForwardRotate180.auto new file mode 100644 index 00000000..87a18024 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/5mForwardRotate180.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.4, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "5mForwardRotate" + } + } + ] + } + }, + "folder": "Test", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideFar+AutoLine4Score.auto b/src/main/deploy/pathplanner/autos/LowSpeakerCenterLineN5N4.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/PresetAmpSideFar+AutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/LowSpeakerCenterLineN5N4.auto diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/MidFar4Score.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/MidFar4Score.auto diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSide3Score.auto b/src/main/deploy/pathplanner/autos/VisionSourceSide3Score.auto index fdb17e31..03d3a316 100644 --- a/src/main/deploy/pathplanner/autos/VisionSourceSide3Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionSourceSide3Score.auto @@ -32,7 +32,7 @@ { "type": "path", "data": { - "pathName": "lowLaunchNoteFourCenterLine" + "pathName": "LowLaunchNoteFourCenterLine" } }, { diff --git a/src/main/deploy/pathplanner/paths/5mForwardRotate.path b/src/main/deploy/pathplanner/paths/5mForwardRotate.path new file mode 100644 index 00000000..e409c153 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/5mForwardRotate.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.4, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.4, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.4, + "y": 7.0 + }, + "prevControl": { + "x": 4.4, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path new file mode 100644 index 00000000..37880f99 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path @@ -0,0 +1,97 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.91, + "y": 1.95 + }, + "prevControl": null, + "nextControl": { + "x": 5.616894662362428, + "y": 1.2460539206809202 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.3, + "y": 2.4481294676875076 + }, + "prevControl": { + "x": 8.504807866758592, + "y": 1.9936862730868328 + }, + "nextControl": { + "x": 8.073502500912618, + "y": 2.950699259968285 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.758300373743605, + "y": 2.858594288617149 + }, + "prevControl": { + "x": 5.2064298414327865, + "y": 0.24921078413750497 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": 146.44882381880868, + "rotateFast": false + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": 162.59131699825656, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 132.28885464347817, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 124.79, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path index 130bb887..df90aa16 100644 --- a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.7713929062062417, + "y": 4.07539893464585 }, "prevControl": null, "nextControl": { - "x": 0.3566676963984554, - "y": 4.493137615644601 + "x": 2.398060602604697, + "y": 4.098536550290452 }, "isLocked": false, - "linkedName": "LowSpeaker" + "linkedName": "Podium" }, { "anchor": { diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 926f893f..d51e4d24 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -145,7 +145,10 @@ public void autonomousInit() { // Checks if FMS is attatched and enables joystick warning if true DriverStation.silenceJoystickConnectionWarning(!DriverStation.isFMSAttached()); // autoChooser.getSelected().schedule(); - AutoLogic.getSelected().schedule(); + System.out.println(AutoLogic.getSelected() != null); + if (AutoLogic.getSelected() != null) { + AutoLogic.getSelected().schedule(); + } } @Override diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 9705eee8..6f8b66c6 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -67,7 +67,7 @@ public class DrivebaseSubsystem extends SubsystemBase { Robot.getInstance().getRobotType() == RobotType.PRACTICE ? new PIDConstants(5, 0, 0.5) // practice : Robot.getInstance().getRobotType() == RobotType.BONK - ? new PIDConstants(5, 0, 0.1) // bonk + ? new PIDConstants(6, 0, 0.1) // bonk : Robot.getInstance().getRobotType() == RobotType.CRANE ? new PIDConstants(3.9, 0, 0.2) // crane : new PIDConstants(0.1, 0, 0.1); // bobot TODO: tune diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 16064ac1..8e849b5e 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -58,6 +58,7 @@ public static enum StartPosition { private static List noPiecePaths = List.of( // presets + new AutoPath("Test Path Rotate", "5mForwardRotate180"), new AutoPath("Test Path", "DiameterTest"), new AutoPath("Stand Still", "PresetSourceSide1Score"), new AutoPath("Stand Still", "PresetMid1Score"), @@ -102,6 +103,8 @@ public static enum StartPosition { private static List fourPiecePaths = List.of( + // presets + new AutoPath("Autoline N2 CenterLine N3 N2", "MidFar4Score"), // vision new AutoPath("Centerline N1 AutoLine N1 N2 N3", "VisionAmpSideAutoLine5Score"), new AutoPath("Autoline N1 Centerline N1 N2 Autoline N2", "VisionAmpSide5Score")); diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java index f66fb073..567fc85a 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -10,7 +10,7 @@ public class AutoPath { private final Pose2d startPose2d; - private final StartPosition startPosition; + private StartPosition startPosition; private final String displayName; private final Command autoCommand; private final boolean vision; @@ -24,12 +24,16 @@ public AutoPath(String displayName, String pathPlannerAutoName, boolean vision) // in the case that the auto for whatever reason's starting pose is slightly off, // is still able to match with a startPosition if it is close enough for (StartPosition pos : StartPosition.values()) { - if (matchesStartPosition(pos)) { + if (!pos.equals(StartPosition.MISC) && matchesStartPosition(pos)) { startPosition = pos; - return; + break; } } - startPosition = StartPosition.MISC; + if (startPosition == null) { + startPosition = StartPosition.MISC; + } + // debug purposes + // System.out.println(startPosition + " " + displayName + " " + startPose2d.toString()); } public AutoPath(String displayName, String pathPlannerAutoName) { From ad50df222c195f54f793b6e0e7f19a2d327e351d Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 16 Mar 2024 13:32:33 -0700 Subject: [PATCH 17/49] gulp --- src/main/java/frc/team2412/robot/Robot.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index ad3900bb..c4918f21 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -1,7 +1,6 @@ package frc.team2412.robot; import com.ctre.phoenix6.SignalLogger; -import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; From e04b7d8f1ca2d37e496b1b8fdc5c0fcf84ab0e76 Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 16 Mar 2024 13:41:35 -0700 Subject: [PATCH 18/49] test --- .../autos/PresetAmpSideAutoLine3Score.auto | 55 ------------------- .../team2412/robot/util/auto/AutoLogic.java | 2 +- 2 files changed, 1 insertion(+), 56 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto deleted file mode 100644 index c305a15e..00000000 --- a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.73, - "y": 6.62 - }, - "rotation": -120.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "SubwooferLaunch" - } - }, - { - "type": "path", - "data": { - "pathName": "HighSpeakerHighNoteReturn" - } - }, - { - "type": "named", - "data": { - "name": "SubwooferLaunch" - } - }, - { - "type": "path", - "data": { - "pathName": "HighSpeakerCenterLineNoteReturn" - } - }, - { - "type": "named", - "data": { - "name": "SubwooferLaunch" - } - }, - { - "type": "path", - "data": { - "pathName": "HighSpeakerCenterLine" - } - } - ] - } - }, - "folder": "Preset Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 8e849b5e..cd2fb74d 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -80,7 +80,7 @@ public static enum StartPosition { private static List twoPiecePaths = List.of( // presets - new AutoPath("Autoline N1 Centerline N1", "PresetAmpSideAutoline3Score"), + // new AutoPath("Autoline N1 Centerline N1", "PresetAmpSideAutoline3Score"), new AutoPath("Autoline N2", "PresetMidAutoline3Score"), // vision new AutoPath("Centerline N5 N4", "VisionSourceSide3Score", true), From 00e42385eda4d988a1f46331280ea2dedefd37f1 Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 16 Mar 2024 13:43:27 -0700 Subject: [PATCH 19/49] Revert "test" This reverts commit e04b7d8f1ca2d37e496b1b8fdc5c0fcf84ab0e76. --- .../autos/PresetAmpSideAutoLine3Score.auto | 55 +++++++++++++++++++ .../team2412/robot/util/auto/AutoLogic.java | 2 +- 2 files changed, 56 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto new file mode 100644 index 00000000..c305a15e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerHighNoteReturn" + } + }, + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerCenterLineNoteReturn" + } + }, + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerCenterLine" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index cd2fb74d..8e849b5e 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -80,7 +80,7 @@ public static enum StartPosition { private static List twoPiecePaths = List.of( // presets - // new AutoPath("Autoline N1 Centerline N1", "PresetAmpSideAutoline3Score"), + new AutoPath("Autoline N1 Centerline N1", "PresetAmpSideAutoline3Score"), new AutoPath("Autoline N2", "PresetMidAutoline3Score"), // vision new AutoPath("Centerline N5 N4", "VisionSourceSide3Score", true), From 4288c1ff77544f141102452a58da5a8ae48addfc Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 16 Mar 2024 13:46:09 -0700 Subject: [PATCH 20/49] gulp --- .../autos/PresetAmpSideAutoLine4Score.auto | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto index c3cf25ce..2a639396 100644 --- a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto @@ -11,23 +11,47 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, { "type": "path", "data": { "pathName": "HighSpeakerHighNoteReturn" } }, + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, { "type": "path", "data": { "pathName": "HighSpeakerHighNote2Return" } }, + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, { "type": "path", "data": { "pathName": "HighSpeakerLowNoteReturn" } + }, + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } } ] } From 935416be6cb6a2526447bbb295f5a2161dfbed7f Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 16 Mar 2024 13:48:51 -0700 Subject: [PATCH 21/49] test --- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 8e849b5e..393cfa58 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -80,7 +80,7 @@ public static enum StartPosition { private static List twoPiecePaths = List.of( // presets - new AutoPath("Autoline N1 Centerline N1", "PresetAmpSideAutoline3Score"), + // new AutoPath("Autoline N1 Centerline N1", "PresetAmpSideAutoline3Score"), new AutoPath("Autoline N2", "PresetMidAutoline3Score"), // vision new AutoPath("Centerline N5 N4", "VisionSourceSide3Score", true), @@ -90,7 +90,7 @@ public static enum StartPosition { private static List threePiecePaths = List.of( // presets - new AutoPath("Autoline N1 N2 N3", "PresetAmpSideAutoline4Score"), + // new AutoPath("Autoline N1 N2 N3", "PresetAmpSideAutoline4Score"), new AutoPath("Autoline N2 N3 N1", "PresetMidAutoline4Score"), new AutoPath("Autoline N3 N2 N1", "PresetSourceSideAutoline4Score"), new AutoPath("Centerline N1 Autoline Autoline N1 N2", "PresetAmpSideAutolineFar4Score"), From 4d70760c95de497874b20a1b1df458ffac4b3c05 Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 16 Mar 2024 14:00:56 -0700 Subject: [PATCH 22/49] un noob --- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 393cfa58..8e849b5e 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -80,7 +80,7 @@ public static enum StartPosition { private static List twoPiecePaths = List.of( // presets - // new AutoPath("Autoline N1 Centerline N1", "PresetAmpSideAutoline3Score"), + new AutoPath("Autoline N1 Centerline N1", "PresetAmpSideAutoline3Score"), new AutoPath("Autoline N2", "PresetMidAutoline3Score"), // vision new AutoPath("Centerline N5 N4", "VisionSourceSide3Score", true), @@ -90,7 +90,7 @@ public static enum StartPosition { private static List threePiecePaths = List.of( // presets - // new AutoPath("Autoline N1 N2 N3", "PresetAmpSideAutoline4Score"), + new AutoPath("Autoline N1 N2 N3", "PresetAmpSideAutoline4Score"), new AutoPath("Autoline N2 N3 N1", "PresetMidAutoline4Score"), new AutoPath("Autoline N3 N2 N1", "PresetSourceSideAutoline4Score"), new AutoPath("Centerline N1 Autoline Autoline N1 N2", "PresetAmpSideAutolineFar4Score"), From ddbb2e5855de51b3def63ee6e46d4edd8e6d9d64 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 16 Mar 2024 14:03:07 -0700 Subject: [PATCH 23/49] Rename LowSpeakerCenterLineN5N4N3.auto to LowSpeakerCenterlineN5N4N3.auto --- ...kerCenterLineN5N4N3.auto => LowSpeakerCenterlineN5N4N3.auto} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/main/deploy/pathplanner/autos/{LowSpeakerCenterLineN5N4N3.auto => LowSpeakerCenterlineN5N4N3.auto} (99%) diff --git a/src/main/deploy/pathplanner/autos/LowSpeakerCenterLineN5N4N3.auto b/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4N3.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/LowSpeakerCenterLineN5N4N3.auto rename to src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4N3.auto index dfbf7f62..6a632df7 100644 --- a/src/main/deploy/pathplanner/autos/LowSpeakerCenterLineN5N4N3.auto +++ b/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4N3.auto @@ -22,4 +22,4 @@ }, "folder": "Complex Autos", "choreoAuto": false -} \ No newline at end of file +} From 6e328bb8c90f618c641070d1b3d152be96d13db7 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 16 Mar 2024 14:03:49 -0700 Subject: [PATCH 24/49] Rename LowSpeakerCenterLineN5N4.auto to LowSpeakerCenterlineN5N4.auto --- ...SpeakerCenterLineN5N4.auto => LowSpeakerCenterlineN5N4.auto} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/main/deploy/pathplanner/autos/{LowSpeakerCenterLineN5N4.auto => LowSpeakerCenterlineN5N4.auto} (99%) diff --git a/src/main/deploy/pathplanner/autos/LowSpeakerCenterLineN5N4.auto b/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/LowSpeakerCenterLineN5N4.auto rename to src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4.auto index 9aba1e20..8f57a91d 100644 --- a/src/main/deploy/pathplanner/autos/LowSpeakerCenterLineN5N4.auto +++ b/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4.auto @@ -22,4 +22,4 @@ }, "folder": "Complex Autos", "choreoAuto": false -} \ No newline at end of file +} From 675e36bde0bdaa5b5d049b698970d59d4db97813 Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 16 Mar 2024 14:04:18 -0700 Subject: [PATCH 25/49] grub --- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 8e849b5e..34073140 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -96,9 +96,9 @@ public static enum StartPosition { new AutoPath("Centerline N1 Autoline Autoline N1 N2", "PresetAmpSideAutolineFar4Score"), // vision new AutoPath("Autoline N1 Centerline N1 N2", "VisionAmpSide4Score", true), - new AutoPath("Autoline N1 N2 N3", "VisionAmpSideAutoLine4Score", true), + new AutoPath("Autoline N1 N2 N3", "VisionAmpSideAutoline4Score", true), new AutoPath("Autoline N3 N2 N1", "VisionMid4Score", true), - new AutoPath("Autoline N3 N2 N1", "VisionSourceSideAutoLine4Score", true), + new AutoPath("Autoline N3 N2 N1", "VisionSourceSideAutoline4Score", true), new AutoPath("Autoline N1 Centerline N1 N2", "VisionAmpSide4Score")); private static List fourPiecePaths = From 633d1adae5c59bc0c3b4c885a407ca8d16a3acbc Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 16 Mar 2024 14:04:20 -0700 Subject: [PATCH 26/49] Rename VisionSourceSideAutoLine4Score.auto to VisionSourceSideAutoline4Score.auto --- ...eAutoLine4Score.auto => VisionSourceSideAutoline4Score.auto} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/main/deploy/pathplanner/autos/{VisionSourceSideAutoLine4Score.auto => VisionSourceSideAutoline4Score.auto} (99%) diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSideAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/VisionSourceSideAutoline4Score.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/VisionSourceSideAutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/VisionSourceSideAutoline4Score.auto index a7dbf162..b72644c1 100644 --- a/src/main/deploy/pathplanner/autos/VisionSourceSideAutoLine4Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionSourceSideAutoline4Score.auto @@ -64,4 +64,4 @@ }, "folder": "Vision Autos", "choreoAuto": false -} \ No newline at end of file +} From 6da182b70821a825abe9df05b95b6b5482ff992e Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 16 Mar 2024 14:04:44 -0700 Subject: [PATCH 27/49] Rename VisionAmpSideAutoLine5Score.auto to VisionAmpSideAutoline5Score.auto --- ...SideAutoLine5Score.auto => VisionAmpSideAutoline5Score.auto} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/main/deploy/pathplanner/autos/{VisionAmpSideAutoLine5Score.auto => VisionAmpSideAutoline5Score.auto} (99%) diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoLine5Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/VisionAmpSideAutoLine5Score.auto rename to src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto index 7afeebe1..4d8f6929 100644 --- a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoLine5Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto @@ -76,4 +76,4 @@ }, "folder": "Vision Autos", "choreoAuto": false -} \ No newline at end of file +} From be647b89b362587aee3163518e80e54f556a28a3 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 16 Mar 2024 14:04:58 -0700 Subject: [PATCH 28/49] Rename VisionAmpSideAutoLine4Score.auto to VisionAmpSideAutoline4Score.auto --- ...SideAutoLine4Score.auto => VisionAmpSideAutoline4Score.auto} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/main/deploy/pathplanner/autos/{VisionAmpSideAutoLine4Score.auto => VisionAmpSideAutoline4Score.auto} (99%) diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline4Score.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/VisionAmpSideAutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/VisionAmpSideAutoline4Score.auto index 31c4cad4..5c917190 100644 --- a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoLine4Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline4Score.auto @@ -52,4 +52,4 @@ }, "folder": "Vision Autos", "choreoAuto": false -} \ No newline at end of file +} From e9b5368af7f4751c04388a49f486e1a0b43020a0 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 16 Mar 2024 14:05:36 -0700 Subject: [PATCH 29/49] Rename TopSpeakerCenterLineN1N2AutoLine1.auto to TopSpeakerCenterlineN1N2AutoLine1.auto --- ...1N2AutoLine1.auto => TopSpeakerCenterlineN1N2AutoLine1.auto} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/main/deploy/pathplanner/autos/{TopSpeakerCenterLineN1N2AutoLine1.auto => TopSpeakerCenterlineN1N2AutoLine1.auto} (99%) diff --git a/src/main/deploy/pathplanner/autos/TopSpeakerCenterLineN1N2AutoLine1.auto b/src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutoLine1.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/TopSpeakerCenterLineN1N2AutoLine1.auto rename to src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutoLine1.auto index 8b61e086..26a148b7 100644 --- a/src/main/deploy/pathplanner/autos/TopSpeakerCenterLineN1N2AutoLine1.auto +++ b/src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutoLine1.auto @@ -22,4 +22,4 @@ }, "folder": "Complex Autos", "choreoAuto": false -} \ No newline at end of file +} From aa115255dd24d78d1d38f4c2b30ab0c215fac753 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 16 Mar 2024 14:06:06 -0700 Subject: [PATCH 30/49] Rename PresetAmpSideFarAutoLine4Score.auto to PresetAmpSideFarAutoline4Score.auto --- ...rAutoLine4Score.auto => PresetAmpSideFarAutoline4Score.auto} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/main/deploy/pathplanner/autos/{PresetAmpSideFarAutoLine4Score.auto => PresetAmpSideFarAutoline4Score.auto} (99%) diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoline4Score.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoline4Score.auto index 430b21d8..db95bcad 100644 --- a/src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoLine4Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoline4Score.auto @@ -46,4 +46,4 @@ }, "folder": "Vision Autos", "choreoAuto": false -} \ No newline at end of file +} From fdfc2511140d2c72cac520ca081dfe44a12ac74d Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 16 Mar 2024 14:06:23 -0700 Subject: [PATCH 31/49] Rename PresetMidAutoLine4Score.auto to PresetMidAutoline4Score.auto --- ...resetMidAutoLine4Score.auto => PresetMidAutoline4Score.auto} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/main/deploy/pathplanner/autos/{PresetMidAutoLine4Score.auto => PresetMidAutoline4Score.auto} (99%) diff --git a/src/main/deploy/pathplanner/autos/PresetMidAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/PresetMidAutoline4Score.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/PresetMidAutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/PresetMidAutoline4Score.auto index 0abc20ed..1912b2d4 100644 --- a/src/main/deploy/pathplanner/autos/PresetMidAutoLine4Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetMidAutoline4Score.auto @@ -64,4 +64,4 @@ }, "folder": "Preset Autos", "choreoAuto": false -} \ No newline at end of file +} From 76fa84bbb0bdeb022bf69351c3300e205751c6e0 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 16 Mar 2024 14:06:46 -0700 Subject: [PATCH 32/49] Rename PresetSourceSideAutoLine4Score.auto to PresetSourceSideAutoline4Score.auto --- ...eAutoLine4Score.auto => PresetSourceSideAutoline4Score.auto} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/main/deploy/pathplanner/autos/{PresetSourceSideAutoLine4Score.auto => PresetSourceSideAutoline4Score.auto} (99%) diff --git a/src/main/deploy/pathplanner/autos/PresetSourceSideAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/PresetSourceSideAutoline4Score.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/PresetSourceSideAutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/PresetSourceSideAutoline4Score.auto index 53a9f7f4..caa27a10 100644 --- a/src/main/deploy/pathplanner/autos/PresetSourceSideAutoLine4Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetSourceSideAutoline4Score.auto @@ -64,4 +64,4 @@ }, "folder": "Preset Autos", "choreoAuto": false -} \ No newline at end of file +} From f3c1629da5cffaa59ef229eee1ed122c00f5fe08 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 16 Mar 2024 14:07:10 -0700 Subject: [PATCH 33/49] Rename TopSpeakerCenterlineN1N2AutoLine1.auto to TopSpeakerCenterlineN1N2Autoline1.auto --- ...eN1N2AutoLine1.auto => TopSpeakerCenterlineN1N2Autoline1.auto} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/deploy/pathplanner/autos/{TopSpeakerCenterlineN1N2AutoLine1.auto => TopSpeakerCenterlineN1N2Autoline1.auto} (100%) diff --git a/src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutoLine1.auto b/src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2Autoline1.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutoLine1.auto rename to src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2Autoline1.auto From c5f65cada3c458a6dbb76874251b0a5531324f15 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 16 Mar 2024 14:07:43 -0700 Subject: [PATCH 34/49] Rename PresetAmpSideAutoLine4Score.auto to PresetAmpSideAutoline4Score.auto --- ...SideAutoLine4Score.auto => PresetAmpSideAutoline4Score.auto} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/main/deploy/pathplanner/autos/{PresetAmpSideAutoLine4Score.auto => PresetAmpSideAutoline4Score.auto} (99%) diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline4Score.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto rename to src/main/deploy/pathplanner/autos/PresetAmpSideAutoline4Score.auto index 2a639396..070e94dd 100644 --- a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine4Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline4Score.auto @@ -58,4 +58,4 @@ }, "folder": "Preset Autos", "choreoAuto": false -} \ No newline at end of file +} From e09c23dc0a4345cf6752b30ec668669a12296109 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 16 Mar 2024 14:08:14 -0700 Subject: [PATCH 35/49] Rename PresetAmpSideAutoLine3Score.auto to PresetAmpSideAutoline3Score.auto --- ...SideAutoLine3Score.auto => PresetAmpSideAutoline3Score.auto} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/main/deploy/pathplanner/autos/{PresetAmpSideAutoLine3Score.auto => PresetAmpSideAutoline3Score.auto} (99%) diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline3Score.auto similarity index 99% rename from src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto rename to src/main/deploy/pathplanner/autos/PresetAmpSideAutoline3Score.auto index c305a15e..5ec74689 100644 --- a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoLine3Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline3Score.auto @@ -52,4 +52,4 @@ }, "folder": "Preset Autos", "choreoAuto": false -} \ No newline at end of file +} From c79728171b52eba285ed81767391ae88cde903ca Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 16 Mar 2024 15:10:04 -0700 Subject: [PATCH 36/49] grub 2 --- .../java/frc/team2412/robot/util/auto/AutoLogic.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 34073140..64b6d15b 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -64,8 +64,8 @@ public static enum StartPosition { new AutoPath("Stand Still", "PresetMid1Score"), new AutoPath("Stand Still", "PresetAmpSide1Score"), // new AutoPath("Pass Auto Line", "PresetSourceSide1ScorePassAutoLine"), - new AutoPath("Pass Auto Line", "PresetAmpSide1ScorePassAutoline"), - new AutoPath("Pass Auto Line", "PresetSourceSide1ScorePassAutoline")); + new AutoPath("Pass Autoline", "PresetAmpSide1ScorePassAutoline"), + new AutoPath("Pass Autoline", "PresetSourceSide1ScorePassAutoline")); private static List onePiecePaths = List.of( @@ -73,9 +73,9 @@ public static enum StartPosition { new AutoPath("Autoline N1", "PresetAmpSide2Score"), new AutoPath("Autoline N2", "PresetMidAutoline2Score"), new AutoPath("Autoline N3", "PresetSourceSideAutoline2Score"), - new AutoPath("CenterLine N5", "PresetSourceSideFar2Score"), + new AutoPath("Centerline N5", "PresetSourceSideFar2Score"), // vision - new AutoPath("CenterLine N3 N1", "VisionMidFar2Score", true)); + new AutoPath("Centerline N3 N1", "VisionMidFar2Score", true)); private static List twoPiecePaths = List.of( @@ -104,9 +104,9 @@ public static enum StartPosition { private static List fourPiecePaths = List.of( // presets - new AutoPath("Autoline N2 CenterLine N3 N2", "MidFar4Score"), + new AutoPath("Autoline N2 Centerline N3 N2", "MidFar4Score"), // vision - new AutoPath("Centerline N1 AutoLine N1 N2 N3", "VisionAmpSideAutoLine5Score"), + new AutoPath("Centerline N1 Autoline N1 N2 N3", "VisionAmpSideAutoline5Score"), new AutoPath("Autoline N1 Centerline N1 N2 Autoline N2", "VisionAmpSide5Score")); private static List fivePiecePaths; From 2692875999d6af617f17c509074c0d1bd2c1d3b1 Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 16 Mar 2024 16:02:31 -0700 Subject: [PATCH 37/49] goo goo ga ga --- .../deploy/pathplanner/autos/New Auto.auto | 43 ++++++++++++++++++ .../pathplanner/paths/BottomNoteLaunch.path | 2 +- .../HighNoteLaunch2ToFarHighNoteToLaunch.path | 12 ++--- .../paths/MidSpeakerToCenterNotetoLaunch.path | 19 +++++--- .../launchPosFirstNoteCenterLineLaunch.path | 44 ++++++------------- .../launchSecondNoteCenterLineLaunch.path | 36 ++++++++------- .../paths/launchSecondNoteLaunch.path | 32 ++++---------- 7 files changed, 105 insertions(+), 83 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 00000000..c5db46c6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.33, + "y": 5.55 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SpeakerToMidNote" + } + }, + { + "type": "path", + "data": { + "pathName": "MidNoteToCenterMidNodeLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "launchPosFirstNoteCenterLineLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "launchSecondNoteCenterLineLaunch" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path b/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path index 9bfdbb86..8e484700 100644 --- a/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 140.5993393365206, + "rotation": 163.23907497881294, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path b/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path index 31be5309..bd42028b 100644 --- a/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path +++ b/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 4.9, - "y": 6.998077532170032 + "x": 5.41, + "y": 6.91 }, "prevControl": { - "x": 5.379663050445332, - "y": 7.098633898139333 + "x": 5.889663050445332, + "y": 7.010556365969301 }, "nextControl": null, "isLocked": false, @@ -47,7 +47,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.75, - "rotationDegrees": 180.0, + "rotationDegrees": -172.5750748946377, "rotateFast": false } ], @@ -79,7 +79,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -168.16, + "rotation": -165.92, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path b/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path index 80f71ce7..884e7744 100644 --- a/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path +++ b/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path @@ -32,16 +32,16 @@ }, { "anchor": { - "x": 4.400159657462172, + "x": 4.45, "y": 4.8 }, "prevControl": { - "x": 3.9457164628614976, - "y": 4.910918393265359 + "x": 3.9750353786421866, + "y": 4.9548967669363915 }, "nextControl": { - "x": 4.876891015948452, - "y": 4.6836416147699715 + "x": 5.107679146631323, + "y": 4.5855158702064 }, "isLocked": false, "linkedName": null @@ -56,8 +56,8 @@ "y": 3.2543996516564464 }, "nextControl": { - "x": 8.94894077946899, - "y": 4.467680827130437 + "x": 9.077551692461096, + "y": 4.542485133666662 }, "isLocked": false, "linkedName": null @@ -81,6 +81,11 @@ "waypointRelativePos": 1.8, "rotationDegrees": 180.0, "rotateFast": false + }, + { + "waypointRelativePos": 2.9, + "rotationDegrees": -156.94, + "rotateFast": false } ], "constraintZones": [], diff --git a/src/main/deploy/pathplanner/paths/launchPosFirstNoteCenterLineLaunch.path b/src/main/deploy/pathplanner/paths/launchPosFirstNoteCenterLineLaunch.path index 5e7923b3..70fd0573 100644 --- a/src/main/deploy/pathplanner/paths/launchPosFirstNoteCenterLineLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchPosFirstNoteCenterLineLaunch.path @@ -8,24 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 4.092311041764941, - "y": 6.406183098080483 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.159294604303494, - "y": 6.391523640190138 - }, - "prevControl": { - "x": 5.7178954724802065, - "y": 6.3466355928860745 - }, - "nextControl": { - "x": 7.024202619833812, - "y": 6.479480387532203 + "x": 4.957219057295258, + "y": 6.611415508545304 }, "isLocked": false, "linkedName": null @@ -36,24 +20,24 @@ "y": 7.461664066185276 }, "prevControl": { - "x": 8.607035964096708, - "y": 7.292808820292638 + "x": 8.602702708501509, + "y": 7.283357704346585 }, "nextControl": { - "x": 7.845520369587389, - "y": 7.630519312077913 + "x": 7.947748466925505, + "y": 7.593599187198374 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.2095867048876965, - "y": 6.831307376900469 + "x": 5.41, + "y": 6.787329003229436 }, "prevControl": { - "x": 5.543597372909033, - "y": 7.0218803294749454 + "x": 6.670713378569614, + "y": 7.227112739939765 }, "nextControl": null, "isLocked": false, @@ -62,8 +46,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.6, - "rotationDegrees": 145.0, + "waypointRelativePos": 0.8, + "rotationDegrees": -128.61667081319877, "rotateFast": false } ], @@ -71,7 +55,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 1.0999999999999999, + "waypointRelativePos": 0.55, "command": { "type": "parallel", "data": { @@ -95,7 +79,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -156.19405648154228, + "rotation": -165.92, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path b/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path index 1ad4eea1..ead4c42a 100644 --- a/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.9, - "y": 7.0 + "x": 5.41, + "y": 6.79 }, "prevControl": null, "nextControl": { - "x": 7.082840451395188, - "y": 6.538118219093581 + "x": 7.391052103040704, + "y": 6.450099183528485 }, "isLocked": false, "linkedName": null @@ -20,31 +20,37 @@ "y": 5.777939483806896 }, "prevControl": { - "x": 8.140830696075806, - "y": 6.508271034560088 + "x": 8.331400517956347, + "y": 6.093373983450684 }, "nextControl": { - "x": 8.323025974780654, - "y": 5.548709233381218 + "x": 8.211618708951704, + "y": 5.365361587866033 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.7680860947307475, - "y": 6.609962769475088 + "x": 5.41, + "y": 6.347545266519105 }, "prevControl": { - "x": 5.764941980334411, - "y": 6.831835645653273 + "x": 7.097499909285533, + "y": 6.450161471751516 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": -148.64916364014846, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [ { @@ -73,13 +79,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -163.097682967436, + "rotation": -170.61848983925177, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": -168.16, + "rotation": -165.92, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/launchSecondNoteLaunch.path b/src/main/deploy/pathplanner/paths/launchSecondNoteLaunch.path index 8be65e6f..95c41790 100644 --- a/src/main/deploy/pathplanner/paths/launchSecondNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchSecondNoteLaunch.path @@ -3,29 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.9, - "y": 7.0 + "x": 5.41, + "y": 6.91 }, "prevControl": null, "nextControl": { - "x": 4.6833117089657605, - "y": 6.5360051440823606 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.147118924868479, - "y": 6.027546469507354 - }, - "prevControl": { - "x": 4.609354083573032, - "y": 6.434313409167359 - }, - "nextControl": { - "x": 3.596045138868069, - "y": 5.542601537826994 + "x": 5.001197430966291, + "y": 6.68743713487427 }, "isLocked": false, "linkedName": null @@ -36,8 +20,8 @@ "y": 5.55 }, "prevControl": { - "x": 3.2873615296780136, - "y": 5.574556013976893 + "x": 3.242062484124969, + "y": 5.702529119343954 }, "nextControl": null, "isLocked": false, @@ -49,7 +33,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.95, + "waypointRelativePos": 0.5, "command": { "type": "parallel", "data": { @@ -79,7 +63,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": -168.16, + "rotation": -165.92, "velocity": 0 }, "useDefaultConstraints": true From 25fadf83f35030945e826bb1e5c56916b824523c Mon Sep 17 00:00:00 2001 From: kirby Date: Sat, 16 Mar 2024 16:04:56 -0700 Subject: [PATCH 38/49] remove prev auto chooser code --- src/main/java/frc/team2412/robot/Robot.java | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index c4918f21..a3af4675 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -49,8 +49,6 @@ public static Robot getInstance() { public MatchDashboard dashboard; public AutoLogic autoLogic; - // public SendableChooser autoChooser; - protected Robot(RobotType type) { // non public for singleton. Protected so test class can subclass instance = this; @@ -86,18 +84,11 @@ public void robotInit() { controls = new Controls(subsystems); AutoLogic.registerCommands(); - // autoChooser = AutoBuilder.buildAutoChooser(); if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { AutoLogic.initShuffleBoard(); } - // SmartDashboard.putData("Auto Chooser", autoChooser); SmartDashboard.putString("current bot", getTypeFromAddress().toString()); - // if (Subsystems.SubsystemConstants.DRIVEBASE_ENABLED) { - // autoChooser = AutoBuilder.buildAutoChooser(); - // } else { - // autoChooser = new SendableChooser<>(); - // } Shuffleboard.startRecording(); @@ -147,8 +138,7 @@ public void autonomousInit() { // Checks if FMS is attatched and enables joystick warning if true DriverStation.silenceJoystickConnectionWarning(!DriverStation.isFMSAttached()); - // autoChooser.getSelected().schedule(); - System.out.println(AutoLogic.getSelected() != null); + // System.out.println(AutoLogic.getSelected() != null); if (AutoLogic.getSelected() != null) { AutoLogic.getSelected().schedule(); } From 2f60eb9dbae92f901c8b0d5ef1fde53d20f793d4 Mon Sep 17 00:00:00 2001 From: kirby Date: Mon, 18 Mar 2024 19:43:22 -0700 Subject: [PATCH 39/49] I LOVE MERGE CONFLICTS !!!! --- .../java/frc/team2412/robot/util/auto/AutoLogic.java | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 64b6d15b..ed62641a 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -26,6 +26,7 @@ import frc.team2412.robot.commands.launcher.StopLauncherCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; import frc.team2412.robot.util.DynamicSendableChooser; +import frc.team2412.robot.util.PathPlannerAutos; import java.util.List; import java.util.Map; @@ -143,13 +144,20 @@ public static enum StartPosition { private static SendableChooser gameObjects = new SendableChooser(); private static SendableChooser isVision = new SendableChooser(); + // methods + + public static Command registerAuto(String autoName, Command command, String... primaryPathNames) { + PathPlannerAutos.registerAuto(autoName, primaryPathNames); + return command.withName(autoName); + } + /** Registers commands in PathPlanner */ public static void registerCommands() { // param: String commandName, Command command // Intake NamedCommands.registerCommand("StopIntake", new IntakeStopCommand(s.intakeSubsystem)); - NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem)); + NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem, null)); NamedCommands.registerCommand( "IntakeSensorOverride", new AllInSensorOverrideCommand(s.intakeSubsystem)); // Launcher @@ -179,7 +187,7 @@ public static void registerCommands() { NamedCommands.registerCommand( "LowSpeakerCenterLineN5N4N3", ComplexAutoPaths.lowSpeakerCenterLineN5N4N3); NamedCommands.registerCommand( - "TopSpeakerCenterLineN1N2AutoLine1", ComplexAutoPaths.TopSpeakerCenterLineN1N2AutoLine1); + "TopSpeakerCenterLineN1N2AutoLine1", ComplexAutoPaths.TopSpeakerCenterLineN1N2AutoLineN1); NamedCommands.registerCommand( "TopSpeakerCenterLineN1N2AutoLine1", ComplexAutoPaths.TopSpeakerCenterLineN1N2N3); } From e23c881fc2df5d1061ed30f44ac623aae12d59b7 Mon Sep 17 00:00:00 2001 From: kirby Date: Mon, 18 Mar 2024 19:45:24 -0700 Subject: [PATCH 40/49] merge conflict nation rise up --- ...line1.auto => TopSpeakerCenterlineN1N2AutolineN1.auto} | 0 src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 8 +++++--- 2 files changed, 5 insertions(+), 3 deletions(-) rename src/main/deploy/pathplanner/autos/{TopSpeakerCenterlineN1N2Autoline1.auto => TopSpeakerCenterlineN1N2AutolineN1.auto} (100%) diff --git a/src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2Autoline1.auto b/src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutolineN1.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2Autoline1.auto rename to src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutolineN1.auto diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index ed62641a..47a0a1ba 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -183,13 +183,15 @@ public static void registerCommands() { // Complex Autos NamedCommands.registerCommand("AutoLogicTest", ComplexAutoPaths.testAuto); NamedCommands.registerCommand( - "MidSpeakerCenterLineN5N4N3", ComplexAutoPaths.midSpeakerCenterLineN3N2N1); + "MidSpeakerCenterLineN3N2N1", ComplexAutoPaths.midSpeakerCenterLineN3N2N1); NamedCommands.registerCommand( "LowSpeakerCenterLineN5N4N3", ComplexAutoPaths.lowSpeakerCenterLineN5N4N3); NamedCommands.registerCommand( - "TopSpeakerCenterLineN1N2AutoLine1", ComplexAutoPaths.TopSpeakerCenterLineN1N2AutoLineN1); + "LowSpeakerCenterLineN5N4", ComplexAutoPaths.lowSpeakerCenterLineN5N4); NamedCommands.registerCommand( - "TopSpeakerCenterLineN1N2AutoLine1", ComplexAutoPaths.TopSpeakerCenterLineN1N2N3); + "TopSpeakerCenterLineN1N2AutoLineN1", ComplexAutoPaths.TopSpeakerCenterLineN1N2AutoLineN1); + NamedCommands.registerCommand( + "TopSpeakerCenterLineN1N2N3", ComplexAutoPaths.TopSpeakerCenterLineN1N2N3); } // public Command getConditionalCommand(){} From e8eb7f012b2af1a5cbbf5443a91bc4710e4a96bb Mon Sep 17 00:00:00 2001 From: kirby Date: Tue, 19 Mar 2024 17:59:53 -0700 Subject: [PATCH 41/49] auto delay widget --- src/main/java/frc/team2412/robot/Robot.java | 4 ++-- .../frc/team2412/robot/util/auto/AutoLogic.java | 13 +++++++++++-- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index a3af4675..16cf5edb 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -139,8 +139,8 @@ public void autonomousInit() { // Checks if FMS is attatched and enables joystick warning if true DriverStation.silenceJoystickConnectionWarning(!DriverStation.isFMSAttached()); // System.out.println(AutoLogic.getSelected() != null); - if (AutoLogic.getSelected() != null) { - AutoLogic.getSelected().schedule(); + if (AutoLogic.getSelectedAuto() != null) { + AutoLogic.getSelectedAuto().schedule(); } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 47a0a1ba..5511db6e 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; @@ -135,6 +136,7 @@ public static enum StartPosition { new WaitCommand(1.5), new InstantCommand(() -> controls.vibrateDriveController(0.0))); + // shuffleboard private static ShuffleboardTab tab = Shuffleboard.getTab("Match"); private static SendableChooser startPositionChooser = @@ -144,6 +146,8 @@ public static enum StartPosition { private static SendableChooser gameObjects = new SendableChooser(); private static SendableChooser isVision = new SendableChooser(); + private static GenericEntry autoDelayEntry; + // methods public static Command registerAuto(String autoName, Command command, String... primaryPathNames) { @@ -227,6 +231,7 @@ public static void initShuffleBoard() { tab.add("Launch Type", isVision).withPosition(8, 1); tab.add("Game Objects", gameObjects).withPosition(9, 1); tab.add("Available Auto Variants", availableAutos).withPosition(8, 2).withSize(2, 1); + autoDelayEntry = tab.add("Auto Delay", 0).withPosition(8, 3).withSize(1, 1).getEntry(); isVision.onChange(AutoLogic::filterAutos); startPositionChooser.onChange(AutoLogic::filterAutos); @@ -262,7 +267,11 @@ public static void filterAutos(Boolean isVision) { filterAutos(gameObjects.getSelected()); } - public static Command getSelected() { - return availableAutos.getSelected().getAutoCommand(); + // get auto + + public static Command getSelectedAuto() { + return Commands.sequence( + Commands.waitSeconds(autoDelayEntry.getDouble(0)), + availableAutos.getSelected().getAutoCommand()); } } From b4281c78a19e4ea8e5dd96baa7e33bbb256da297 Mon Sep 17 00:00:00 2001 From: kirby Date: Wed, 20 Mar 2024 18:24:10 -0700 Subject: [PATCH 42/49] Auto Preview Fixes yay it works --- .../deploy/pathplanner/autos/nothing.auto | 18 +++++++++++ .../team2412/robot/util/AutonomousField.java | 31 ++++++++++-------- .../team2412/robot/util/MatchDashboard.java | 2 +- .../team2412/robot/util/auto/AutoLogic.java | 32 +++++++++++++++---- .../team2412/robot/util/auto/AutoPath.java | 18 +++++++++++ 5 files changed, 80 insertions(+), 21 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/nothing.auto diff --git a/src/main/deploy/pathplanner/autos/nothing.auto b/src/main/deploy/pathplanner/autos/nothing.auto new file mode 100644 index 00000000..9fc664f4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/nothing.auto @@ -0,0 +1,18 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.5, + "y": 2.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index 451dba90..19d62665 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -3,14 +3,13 @@ import com.pathplanner.lib.path.PathPlannerTrajectory; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StringSubscriber; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.Watchdog; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import frc.team2412.robot.util.auto.AutoLogic; import java.util.List; import java.util.Map; import java.util.Optional; @@ -31,14 +30,14 @@ public static void configureShuffleboardTab( tab.add("Auto display speed", DEFAULT_PLAYBACK_SPEED) .withWidget(BuiltInWidgets.kNumberSlider) .withProperties(Map.of("Min", 0.5, "Max", 2.5)) - .withPosition(columnIndex, rowIndex + 2) // Offset by height of Field2d display - .withSize(2, 1) + .withPosition(columnIndex, rowIndex + 3) // Offset by height of Field2d display + .withSize(4, 1) .getEntry(); - StringSubscriber activeAutoSub = - NetworkTableInstance.getDefault() - .getTable("Shuffleboard/Match/Auto Chooser") - .getStringTopic("active") - .subscribe(""); + // StringSubscriber activeAutoSub = + // NetworkTableInstance.getDefault() + // .getTable("Shuffleboard/Match/" + autoChooserName) + // .getStringTopic("active") + // .subscribe(""); var autonomousField = new AutonomousField(() -> speedMultiplier.getDouble(DEFAULT_PLAYBACK_SPEED)); var watchdog = @@ -46,7 +45,8 @@ public static void configureShuffleboardTab( addPeriodic.accept( () -> { watchdog.reset(); - autonomousField.update(activeAutoSub.get()); + + autonomousField.update(AutoLogic.getSelectedAutoName()); watchdog.addEpoch("AutonomousField.update()"); watchdog.disable(); if (watchdog.isExpired()) { @@ -56,7 +56,7 @@ public static void configureShuffleboardTab( UPDATE_RATE); tab.add("Selected auto", autonomousField.getField()) .withPosition(columnIndex, rowIndex) - .withSize(2, 2); + .withSize(4, 3); } // Display @@ -109,6 +109,7 @@ public Pose2d getUpdatedPose(String autoName) { lastName = Optional.of(autoName); auto = PathPlannerAutos.getAuto(autoName); trajectories = auto.trajectories; + System.out.println("num trajectories: " + trajectories.size()); trajectoryIndex = 0; lastFPGATime = fpgaTime; lastTrajectoryTimeOffset = 0; @@ -123,6 +124,7 @@ public Pose2d getUpdatedPose(String autoName) { lastFPGATime = fpgaTime; while (lastTrajectoryTimeOffset > trajectories.get(trajectoryIndex).getTotalTimeSeconds()) { lastTrajectoryTimeOffset -= trajectories.get(trajectoryIndex).getTotalTimeSeconds(); + trajectoryIndex++; if (trajectoryIndex >= trajectories.size()) { trajectoryIndex = 0; } @@ -139,11 +141,14 @@ public Pose2d getUpdatedPose(String autoName) { * * @param autoName The name of the selected PathPlanner autonomous routine. */ - public void update(String autoName) { + public void update(Optional autoName) { if (DriverStation.isEnabled()) { lastName = Optional.empty(); return; } - field.setRobotPose(getUpdatedPose(autoName)); + + if (autoName.isPresent()) { + field.setRobotPose(getUpdatedPose(autoName.get())); + } } } diff --git a/src/main/java/frc/team2412/robot/util/MatchDashboard.java b/src/main/java/frc/team2412/robot/util/MatchDashboard.java index 2985cfdb..be8fbe5f 100644 --- a/src/main/java/frc/team2412/robot/util/MatchDashboard.java +++ b/src/main/java/frc/team2412/robot/util/MatchDashboard.java @@ -20,6 +20,6 @@ public MatchDashboard(Subsystems s) { tab.add(new FMSWidget()).withPosition(0, 0).withSize(4, 1); tab.add(field).withPosition(0, 1).withSize(4, 3); Robot r = Robot.getInstance(); - AutonomousField.configureShuffleboardTab(tab, 6, 0, "Available Auto Variants", r::addPeriodic); + AutonomousField.configureShuffleboardTab(tab, 9, 0, "Available Auto Variants", r::addPeriodic); } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 5511db6e..e027e14f 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -30,6 +30,7 @@ import frc.team2412.robot.util.PathPlannerAutos; import java.util.List; import java.util.Map; +import java.util.Optional; public class AutoLogic { public static Robot r = Robot.getInstance(); @@ -46,8 +47,8 @@ public static enum StartPosition { new Pose2d(0.73, 4.47, new Rotation2d(Units.degreesToRadians(120)))), MISC("Misc", null); - String title; // for shuffleboard display - Pose2d startPose; // for identifying path's starting positions for filtering + final String title; // for shuffleboard display + final Pose2d startPose; // for identifying path's starting positions for filtering StartPosition(String title, Pose2d startPose) { this.title = title; @@ -55,8 +56,14 @@ public static enum StartPosition { } }; + static { + registerCommands(); + } + // paths lists + private static AutoPath defaultPath = new AutoPath("do nothing", "nothing"); + private static List noPiecePaths = List.of( // presets @@ -227,11 +234,11 @@ public static void initShuffleBoard() { gameObjects.addOption(String.valueOf(i), i); } - tab.add("Starting Position", startPositionChooser).withPosition(8, 0).withSize(2, 1); - tab.add("Launch Type", isVision).withPosition(8, 1); - tab.add("Game Objects", gameObjects).withPosition(9, 1); - tab.add("Available Auto Variants", availableAutos).withPosition(8, 2).withSize(2, 1); - autoDelayEntry = tab.add("Auto Delay", 0).withPosition(8, 3).withSize(1, 1).getEntry(); + tab.add("Starting Position", startPositionChooser).withPosition(7, 0).withSize(2, 1); + tab.add("Launch Type", isVision).withPosition(7, 1); + tab.add("Game Objects", gameObjects).withPosition(8, 1); + tab.add("Available Auto Variants", availableAutos).withPosition(7, 2).withSize(2, 1); + autoDelayEntry = tab.add("Auto Delay", 0).withPosition(7, 3).withSize(1, 1).getEntry(); isVision.onChange(AutoLogic::filterAutos); startPositionChooser.onChange(AutoLogic::filterAutos); @@ -269,6 +276,17 @@ public static void filterAutos(Boolean isVision) { // get auto + public static Optional getSelectedAutoName() { + if (availableAutos.getSelected() == null) { + return Optional.empty(); + } + return Optional.of(availableAutos.getSelected().getAutoName()); + } + + public static boolean chooserHasAutoSelected() { + return availableAutos.getSelected() != null; + } + public static Command getSelectedAuto() { return Commands.sequence( Commands.waitSeconds(autoDelayEntry.getDouble(0)), diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java index 567fc85a..fd367185 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -1,9 +1,17 @@ package frc.team2412.robot.util.auto; +import java.util.ArrayList; +import java.util.List; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPlannerTrajectory; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import frc.team2412.robot.util.auto.AutoLogic.StartPosition; @@ -11,12 +19,15 @@ public class AutoPath { private final Pose2d startPose2d; private StartPosition startPosition; + private String pathPlannerAutoName; private final String displayName; private final Command autoCommand; private final boolean vision; + public AutoPath(String displayName, String pathPlannerAutoName, boolean vision) { this.displayName = displayName; + this.pathPlannerAutoName = pathPlannerAutoName; startPose2d = PathPlannerAuto.getStaringPoseFromAutoFile(pathPlannerAutoName); autoCommand = AutoBuilder.buildAuto(pathPlannerAutoName); this.vision = vision; @@ -34,6 +45,7 @@ public AutoPath(String displayName, String pathPlannerAutoName, boolean vision) } // debug purposes // System.out.println(startPosition + " " + displayName + " " + startPose2d.toString()); + } public AutoPath(String displayName, String pathPlannerAutoName) { @@ -48,6 +60,10 @@ public Pose2d getStartPose2d() { return startPose2d; } + public String getAutoName() { + return pathPlannerAutoName; + } + public String getDisplayName() { return displayName; } @@ -75,4 +91,6 @@ public boolean matchesStartPosition(StartPosition expectedStartPosition) { startPose2d.getRotation().getDegrees(), 5)); } + + } From 6e4ce7835bb9756411c8a31955428154e755026e Mon Sep 17 00:00:00 2001 From: kirby Date: Wed, 20 Mar 2024 18:35:20 -0700 Subject: [PATCH 43/49] spotlessApply --- .../java/frc/team2412/robot/util/AutonomousField.java | 2 +- .../java/frc/team2412/robot/util/auto/AutoPath.java | 11 ----------- 2 files changed, 1 insertion(+), 12 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index 19d62665..26be7303 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -45,7 +45,7 @@ public static void configureShuffleboardTab( addPeriodic.accept( () -> { watchdog.reset(); - + autonomousField.update(AutoLogic.getSelectedAutoName()); watchdog.addEpoch("AutonomousField.update()"); watchdog.disable(); diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java index fd367185..a739ed25 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -1,17 +1,9 @@ package frc.team2412.robot.util.auto; -import java.util.ArrayList; -import java.util.List; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.path.PathPlannerTrajectory; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import frc.team2412.robot.util.auto.AutoLogic.StartPosition; @@ -24,7 +16,6 @@ public class AutoPath { private final Command autoCommand; private final boolean vision; - public AutoPath(String displayName, String pathPlannerAutoName, boolean vision) { this.displayName = displayName; this.pathPlannerAutoName = pathPlannerAutoName; @@ -91,6 +82,4 @@ public boolean matchesStartPosition(StartPosition expectedStartPosition) { startPose2d.getRotation().getDegrees(), 5)); } - - } From 30b77da23c91f31871827f37092bcd6eef749953 Mon Sep 17 00:00:00 2001 From: kirby Date: Wed, 20 Mar 2024 19:48:49 -0700 Subject: [PATCH 44/49] time estimator --- .../team2412/robot/util/AutonomousField.java | 3 ++ .../team2412/robot/util/auto/AutoLogic.java | 48 ++++++++++++++----- 2 files changed, 38 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index 26be7303..a65c06b0 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -33,6 +33,9 @@ public static void configureShuffleboardTab( .withPosition(columnIndex, rowIndex + 3) // Offset by height of Field2d display .withSize(4, 1) .getEntry(); + + tab.addDouble("Est. Time", AutoLogic::getEstimatedAutoDuration).withPosition(8, 3); + // StringSubscriber activeAutoSub = // NetworkTableInstance.getDefault() // .getTable("Shuffleboard/Match/" + autoChooserName) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index e027e14f..5df70530 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -3,6 +3,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPlannerTrajectory; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -28,6 +29,7 @@ import frc.team2412.robot.subsystems.LauncherSubsystem; import frc.team2412.robot.util.DynamicSendableChooser; import frc.team2412.robot.util.PathPlannerAutos; +import frc.team2412.robot.util.PathPlannerAutos.Auto; import java.util.List; import java.util.Map; import java.util.Optional; @@ -240,11 +242,11 @@ public static void initShuffleBoard() { tab.add("Available Auto Variants", availableAutos).withPosition(7, 2).withSize(2, 1); autoDelayEntry = tab.add("Auto Delay", 0).withPosition(7, 3).withSize(1, 1).getEntry(); - isVision.onChange(AutoLogic::filterAutos); - startPositionChooser.onChange(AutoLogic::filterAutos); - gameObjects.onChange(AutoLogic::filterAutos); + isVision.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); + startPositionChooser.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); + gameObjects.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); - filterAutos(startPositionChooser.getSelected()); + filterAutos(gameObjects.getSelected()); } /** Takes the auto filtering entries in shuffleboard to provide a list of suitable autos */ @@ -265,15 +267,6 @@ public static void filterAutos(int numGameObjects) { } } - // auto filtering stuff - public static void filterAutos(StartPosition startPosition) { - filterAutos(gameObjects.getSelected()); - } - - public static void filterAutos(Boolean isVision) { - filterAutos(gameObjects.getSelected()); - } - // get auto public static Optional getSelectedAutoName() { @@ -292,4 +285,33 @@ public static Command getSelectedAuto() { Commands.waitSeconds(autoDelayEntry.getDouble(0)), availableAutos.getSelected().getAutoCommand()); } + + /** + * Takes all of the trajectories of an auto to find the total estimated duration of an auto + * + * @return auto duration in seconds + */ + public static double getEstimatedAutoDuration() { + if (getSelectedAutoName().isPresent()) { + + Auto auto = PathPlannerAutos.getAuto(getSelectedAutoName().get()); + double autoTime = 0; + + for (PathPlannerTrajectory trajectory : auto.trajectories) { + autoTime += trajectory.getTotalTimeSeconds(); + } + + // TODO: more accurate estimating by viewing named commands involved + + // rounds time to two decimals + autoTime *= 100; + autoTime = ((double) ((int) autoTime)) / 100; + + // add autoDelay to estimation as well + autoTime += autoDelayEntry.getDouble(0); + + return autoTime; + } + return 0; + } } From 992dccb393162c08ca5f38cf75ca65e8b1ecf86a Mon Sep 17 00:00:00 2001 From: kirby Date: Wed, 20 Mar 2024 20:20:50 -0700 Subject: [PATCH 45/49] auto tweaking + pid tuning autos --- .../autos/LowSpeakerCenterlineN5N4.auto | 2 +- .../autos/LowSpeakerCenterlineN5N4N3.auto | 2 +- .../autos/PresetAmpSideAutoline3Score.auto | 2 +- .../autos/PresetAmpSideAutoline4Score.auto | 2 +- .../autos/PresetAmpSideFarAutoline4Score.auto | 2 +- .../autos/PresetMidAutoline4Score.auto | 2 +- .../autos/PresetSourceSideAutoline4Score.auto | 2 +- .../TopSpeakerCenterlineN1N2AutolineN1.auto | 2 +- .../pathplanner/autos/TuneRotationalPID.auto | 25 +++++++++ .../autos/TuneTranslationalPID.auto | 25 +++++++++ .../autos/VisionAmpSideAutoline4Score.auto | 2 +- .../autos/VisionAmpSideAutoline5Score.auto | 2 +- ...New Auto.auto => VisionMidFar4Score2.auto} | 6 --- ...ar4Score.auto => VisionMidFar4Score3.auto} | 0 .../autos/VisionSourceSideAutoline4Score.auto | 2 +- .../pathplanner/paths/5mForwardRotate.path | 2 +- .../{tunePID.path => TuneRotationPID.path} | 16 +++--- .../paths/TuneTranslationalPID.path | 52 +++++++++++++++++++ .../team2412/robot/util/AutonomousField.java | 6 +-- .../team2412/robot/util/auto/AutoLogic.java | 7 ++- 20 files changed, 130 insertions(+), 31 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/TuneRotationalPID.auto create mode 100644 src/main/deploy/pathplanner/autos/TuneTranslationalPID.auto rename src/main/deploy/pathplanner/autos/{New Auto.auto => VisionMidFar4Score2.auto} (82%) rename src/main/deploy/pathplanner/autos/{MidFar4Score.auto => VisionMidFar4Score3.auto} (100%) rename src/main/deploy/pathplanner/paths/{tunePID.path => TuneRotationPID.path} (82%) create mode 100644 src/main/deploy/pathplanner/paths/TuneTranslationalPID.path diff --git a/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4.auto b/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4.auto index 8f57a91d..9aba1e20 100644 --- a/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4.auto +++ b/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4.auto @@ -22,4 +22,4 @@ }, "folder": "Complex Autos", "choreoAuto": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4N3.auto b/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4N3.auto index 6a632df7..dfbf7f62 100644 --- a/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4N3.auto +++ b/src/main/deploy/pathplanner/autos/LowSpeakerCenterlineN5N4N3.auto @@ -22,4 +22,4 @@ }, "folder": "Complex Autos", "choreoAuto": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline3Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline3Score.auto index 5ec74689..c305a15e 100644 --- a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline3Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline3Score.auto @@ -52,4 +52,4 @@ }, "folder": "Preset Autos", "choreoAuto": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline4Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline4Score.auto index 070e94dd..2a639396 100644 --- a/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline4Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideAutoline4Score.auto @@ -58,4 +58,4 @@ }, "folder": "Preset Autos", "choreoAuto": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoline4Score.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoline4Score.auto index db95bcad..430b21d8 100644 --- a/src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoline4Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideFarAutoline4Score.auto @@ -46,4 +46,4 @@ }, "folder": "Vision Autos", "choreoAuto": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetMidAutoline4Score.auto b/src/main/deploy/pathplanner/autos/PresetMidAutoline4Score.auto index 1912b2d4..0abc20ed 100644 --- a/src/main/deploy/pathplanner/autos/PresetMidAutoline4Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetMidAutoline4Score.auto @@ -64,4 +64,4 @@ }, "folder": "Preset Autos", "choreoAuto": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PresetSourceSideAutoline4Score.auto b/src/main/deploy/pathplanner/autos/PresetSourceSideAutoline4Score.auto index caa27a10..53a9f7f4 100644 --- a/src/main/deploy/pathplanner/autos/PresetSourceSideAutoline4Score.auto +++ b/src/main/deploy/pathplanner/autos/PresetSourceSideAutoline4Score.auto @@ -64,4 +64,4 @@ }, "folder": "Preset Autos", "choreoAuto": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutolineN1.auto b/src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutolineN1.auto index 26a148b7..8b61e086 100644 --- a/src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutolineN1.auto +++ b/src/main/deploy/pathplanner/autos/TopSpeakerCenterlineN1N2AutolineN1.auto @@ -22,4 +22,4 @@ }, "folder": "Complex Autos", "choreoAuto": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TuneRotationalPID.auto b/src/main/deploy/pathplanner/autos/TuneRotationalPID.auto new file mode 100644 index 00000000..dd789b04 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TuneRotationalPID.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.3, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TuneRotationPID" + } + } + ] + } + }, + "folder": "Test", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TuneTranslationalPID.auto b/src/main/deploy/pathplanner/autos/TuneTranslationalPID.auto new file mode 100644 index 00000000..b86e9f35 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TuneTranslationalPID.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.3, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TuneTranslationalPID" + } + } + ] + } + }, + "folder": "Test", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline4Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline4Score.auto index 5c917190..31c4cad4 100644 --- a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline4Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline4Score.auto @@ -52,4 +52,4 @@ }, "folder": "Vision Autos", "choreoAuto": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto index 4d8f6929..7afeebe1 100644 --- a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto @@ -76,4 +76,4 @@ }, "folder": "Vision Autos", "choreoAuto": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/VisionMidFar4Score2.auto similarity index 82% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/VisionMidFar4Score2.auto index c5db46c6..460bc18a 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/VisionMidFar4Score2.auto @@ -28,12 +28,6 @@ "data": { "pathName": "launchPosFirstNoteCenterLineLaunch" } - }, - { - "type": "path", - "data": { - "pathName": "launchSecondNoteCenterLineLaunch" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/MidFar4Score.auto b/src/main/deploy/pathplanner/autos/VisionMidFar4Score3.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/MidFar4Score.auto rename to src/main/deploy/pathplanner/autos/VisionMidFar4Score3.auto diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSideAutoline4Score.auto b/src/main/deploy/pathplanner/autos/VisionSourceSideAutoline4Score.auto index b72644c1..a7dbf162 100644 --- a/src/main/deploy/pathplanner/autos/VisionSourceSideAutoline4Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionSourceSideAutoline4Score.auto @@ -64,4 +64,4 @@ }, "folder": "Vision Autos", "choreoAuto": false -} +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/5mForwardRotate.path b/src/main/deploy/pathplanner/paths/5mForwardRotate.path index e409c153..79d1fb9a 100644 --- a/src/main/deploy/pathplanner/paths/5mForwardRotate.path +++ b/src/main/deploy/pathplanner/paths/5mForwardRotate.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/src/main/deploy/pathplanner/paths/tunePID.path b/src/main/deploy/pathplanner/paths/TuneRotationPID.path similarity index 82% rename from src/main/deploy/pathplanner/paths/tunePID.path rename to src/main/deploy/pathplanner/paths/TuneRotationPID.path index ec9e6372..9d5868fb 100644 --- a/src/main/deploy/pathplanner/paths/tunePID.path +++ b/src/main/deploy/pathplanner/paths/TuneRotationPID.path @@ -4,24 +4,24 @@ { "anchor": { "x": 2.3, - "y": 2.0 + "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 3.051535872234666, - "y": 2.0 + "x": 3.3, + "y": 7.0 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.5, - "y": 2.0 + "x": 6.3, + "y": 7.0 }, "prevControl": { - "x": 4.876760993243372, - "y": 2.0 + "x": 5.3, + "y": 7.0 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 180.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/TuneTranslationalPID.path b/src/main/deploy/pathplanner/paths/TuneTranslationalPID.path new file mode 100644 index 00000000..7dbd0f72 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TuneTranslationalPID.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.3, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.3, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.3, + "y": 7.0 + }, + "prevControl": { + "x": 5.3, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index a65c06b0..89836b8c 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -30,11 +30,11 @@ public static void configureShuffleboardTab( tab.add("Auto display speed", DEFAULT_PLAYBACK_SPEED) .withWidget(BuiltInWidgets.kNumberSlider) .withProperties(Map.of("Min", 0.5, "Max", 2.5)) - .withPosition(columnIndex, rowIndex + 3) // Offset by height of Field2d display - .withSize(4, 1) + .withPosition(columnIndex + 1, rowIndex + 3) // Offset by height of Field2d display + .withSize(3, 1) .getEntry(); - tab.addDouble("Est. Time", AutoLogic::getEstimatedAutoDuration).withPosition(8, 3); + tab.addDouble("Est. Time", AutoLogic::getEstimatedAutoDuration).withPosition(columnIndex, rowIndex + 3); // StringSubscriber activeAutoSub = // NetworkTableInstance.getDefault() diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 5df70530..1753a1fb 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -71,6 +71,8 @@ public static enum StartPosition { // presets new AutoPath("Test Path Rotate", "5mForwardRotate180"), new AutoPath("Test Path", "DiameterTest"), + new AutoPath("Tune Translational PID", "TuneTranslationalPID"), + new AutoPath("Tune Rotational PID", "TuneRotationalPID"), new AutoPath("Stand Still", "PresetSourceSide1Score"), new AutoPath("Stand Still", "PresetMid1Score"), new AutoPath("Stand Still", "PresetAmpSide1Score"), @@ -109,13 +111,14 @@ public static enum StartPosition { new AutoPath("Autoline N1 Centerline N1 N2", "VisionAmpSide4Score", true), new AutoPath("Autoline N1 N2 N3", "VisionAmpSideAutoline4Score", true), new AutoPath("Autoline N3 N2 N1", "VisionMid4Score", true), + new AutoPath("Autoline N2 Centerline N3 N1", "VisionMidFar4Score2", true), + new AutoPath("Autoline N2 Centerline N3 N2", "VisionMidFar4Score3", true), new AutoPath("Autoline N3 N2 N1", "VisionSourceSideAutoline4Score", true), - new AutoPath("Autoline N1 Centerline N1 N2", "VisionAmpSide4Score")); + new AutoPath("Autoline N1 Centerline N1 N2", "VisionAmpSide4Score", true)); private static List fourPiecePaths = List.of( // presets - new AutoPath("Autoline N2 Centerline N3 N2", "MidFar4Score"), // vision new AutoPath("Centerline N1 Autoline N1 N2 N3", "VisionAmpSideAutoline5Score"), new AutoPath("Autoline N1 Centerline N1 N2 Autoline N2", "VisionAmpSide5Score")); From 3cfb86016ef6ac3db57c6ecbd1007776dc91ad16 Mon Sep 17 00:00:00 2001 From: kirby Date: Wed, 20 Mar 2024 20:23:15 -0700 Subject: [PATCH 46/49] spotless --- src/main/java/frc/team2412/robot/util/AutonomousField.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index 89836b8c..98f4bddb 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -34,7 +34,8 @@ public static void configureShuffleboardTab( .withSize(3, 1) .getEntry(); - tab.addDouble("Est. Time", AutoLogic::getEstimatedAutoDuration).withPosition(columnIndex, rowIndex + 3); + tab.addDouble("Est. Time", AutoLogic::getEstimatedAutoDuration) + .withPosition(columnIndex, rowIndex + 3); // StringSubscriber activeAutoSub = // NetworkTableInstance.getDefault() From 171287877b8b2e1bb56ae09f8bc4de59efd8d73e Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Thu, 21 Mar 2024 18:10:51 -0700 Subject: [PATCH 47/49] limelight subsytem usage in complex autos --- .../robot/util/auto/ComplexAutoPaths.java | 25 +++++++++++-------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java index 55cf12f9..7c8cca90 100644 --- a/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java +++ b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java @@ -9,6 +9,7 @@ import frc.team2412.robot.commands.launcher.FullTargetCommand; import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; +import java.util.function.BooleanSupplier; public class ComplexAutoPaths { @@ -22,7 +23,7 @@ public class ComplexAutoPaths { new ConditionalCommand( AutoLogic.getAutoCommand("TestPathTrue"), AutoLogic.getAutoCommand("TestPathFalse"), - () -> false)), + checkForTargets())), "TestPath", "TestPathTrue"); @@ -47,8 +48,8 @@ public class ComplexAutoPaths { VisionLaunchCommand(), getAutoCommand("LCenterLineN2LAutoLineN1")), Commands.sequence(getAutoCommand("QCenterLineN2LAutoLineN1")), - () -> true)), - () -> true), + checkForTargets())), + checkForTargets()), VisionLaunchCommand()), "TopSpeakerQCenterLineN1", "QCenterLineN1LCenterLineN1", @@ -73,8 +74,8 @@ public class ComplexAutoPaths { VisionLaunchCommand(), getAutoCommand("LCenterLineN2LCenterLineN3")), Commands.sequence(getAutoCommand("QCenterLineN2LCenterLineN3")), - () -> true)), - () -> true), + checkForTargets())), + checkForTargets()), VisionLaunchCommand()), "TopSpeakerQCenterLineN1", "QCenterLineN1LCenterLineN1", @@ -97,11 +98,11 @@ public class ComplexAutoPaths { VisionLaunchCommand(), getAutoCommand("N3LCenterLineN2LCenterLineN1")), getAutoCommand("QCenterLineN2LCenterLineN1"), - () -> true)), + checkForTargets())), Commands.sequence( getAutoCommand("QCenterLineN3QCenterLineN2"), getAutoCommand("QCenterLineN2LCenterLineN2")), - () -> true), + checkForTargets()), VisionLaunchCommand()), "MidSpeakerQCenterLineN3", "QCenterLineN3LCenterLineN3", @@ -121,7 +122,7 @@ public class ComplexAutoPaths { Commands.waitSeconds(0.5), getAutoCommand("LCenterLineN5LCenterLineN4")), Commands.sequence(getAutoCommand("QCenterLineN5LCenterLineN4")), - () -> false)), + checkForTargets())), "LowSpeakerQCenterLineN5", "QCenterLineN5LCenterLineN5", "LCenterLineN5LCenterLineN4"); @@ -141,12 +142,12 @@ public class ComplexAutoPaths { VisionLaunchCommand(), getAutoCommand("LCenterLineN4LCenterLineN3")), Commands.sequence(getAutoCommand("QCenterLineN4LCenterLineN3")), - () -> true)), + checkForTargets())), Commands.sequence( getAutoCommand("QCenterLineN5LCenterLineN4"), VisionLaunchCommand(), getAutoCommand("LCenterLineN4LCenterLineN3")), - () -> false), + checkForTargets()), VisionLaunchCommand()), "LowSpeakerQCenterLineN5", "QCenterLineN5QCenterLineN4", @@ -165,4 +166,8 @@ public static final Command SubwooferLaunchCommand() { LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, LauncherSubsystem.SUBWOOFER_AIM_ANGLE); } + + public static BooleanSupplier checkForTargets() { + return (s.limelightSubsystem != null ? s.limelightSubsystem::hasTargets : () -> true); + } } From 1e98f5c3a422d16dda782c6c42f251b02456cb7b Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Thu, 21 Mar 2024 18:55:58 -0700 Subject: [PATCH 48/49] null checkers + shuffleboard repositioning --- .../team2412/robot/util/AutonomousField.java | 1 - .../team2412/robot/util/MatchDashboard.java | 2 +- .../team2412/robot/util/auto/AutoLogic.java | 54 ++++++++++++------- .../robot/util/auto/ComplexAutoPaths.java | 17 +++--- 4 files changed, 47 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/AutonomousField.java b/src/main/java/frc/team2412/robot/util/AutonomousField.java index 98f4bddb..f5853231 100644 --- a/src/main/java/frc/team2412/robot/util/AutonomousField.java +++ b/src/main/java/frc/team2412/robot/util/AutonomousField.java @@ -113,7 +113,6 @@ public Pose2d getUpdatedPose(String autoName) { lastName = Optional.of(autoName); auto = PathPlannerAutos.getAuto(autoName); trajectories = auto.trajectories; - System.out.println("num trajectories: " + trajectories.size()); trajectoryIndex = 0; lastFPGATime = fpgaTime; lastTrajectoryTimeOffset = 0; diff --git a/src/main/java/frc/team2412/robot/util/MatchDashboard.java b/src/main/java/frc/team2412/robot/util/MatchDashboard.java index be8fbe5f..d8290074 100644 --- a/src/main/java/frc/team2412/robot/util/MatchDashboard.java +++ b/src/main/java/frc/team2412/robot/util/MatchDashboard.java @@ -20,6 +20,6 @@ public MatchDashboard(Subsystems s) { tab.add(new FMSWidget()).withPosition(0, 0).withSize(4, 1); tab.add(field).withPosition(0, 1).withSize(4, 3); Robot r = Robot.getInstance(); - AutonomousField.configureShuffleboardTab(tab, 9, 0, "Available Auto Variants", r::addPeriodic); + AutonomousField.configureShuffleboardTab(tab, 7, 0, "Available Auto Variants", r::addPeriodic); } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 1753a1fb..8c05dcfa 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -1,5 +1,7 @@ package frc.team2412.robot.util.auto; +import static frc.team2412.robot.Subsystems.SubsystemConstants.*; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.PathPlannerPath; @@ -172,32 +174,46 @@ public static void registerCommands() { // param: String commandName, Command command // Intake - NamedCommands.registerCommand("StopIntake", new IntakeStopCommand(s.intakeSubsystem)); - NamedCommands.registerCommand("Intake", new AllInCommand(s.intakeSubsystem, null)); NamedCommands.registerCommand( - "IntakeSensorOverride", new AllInSensorOverrideCommand(s.intakeSubsystem)); + "StopIntake", + (INTAKE_ENABLED ? new IntakeStopCommand(s.intakeSubsystem) : Commands.none())); + NamedCommands.registerCommand( + "Intake", (INTAKE_ENABLED ? new AllInCommand(s.intakeSubsystem, null) : Commands.none())); + NamedCommands.registerCommand( + "IntakeSensorOverride", + (INTAKE_ENABLED ? new AllInSensorOverrideCommand(s.intakeSubsystem) : Commands.none())); // Launcher NamedCommands.registerCommand( "VisionLaunch", - Commands.sequence( - new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls), - new FeederInCommand(s.intakeSubsystem))); + (LAUNCHER_ENABLED && INTAKE_ENABLED && APRILTAGS_ENABLED + ? Commands.sequence( + new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls), + new FeederInCommand(s.intakeSubsystem)) + : Commands.none())); NamedCommands.registerCommand( "SubwooferLaunch", - new SetAngleLaunchCommand( - s.launcherSubsystem, - LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, - LauncherSubsystem.SUBWOOFER_AIM_ANGLE) - .andThen(new WaitCommand(1)) - .andThen(new FeederInCommand(s.intakeSubsystem).andThen(new WaitCommand(0.5)))); - NamedCommands.registerCommand("StopLaunch", new StopLauncherCommand(s.launcherSubsystem)); + (LAUNCHER_ENABLED && INTAKE_ENABLED + ? new SetAngleLaunchCommand( + s.launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.SUBWOOFER_AIM_ANGLE) + .andThen(new WaitCommand(1)) + .andThen(new FeederInCommand(s.intakeSubsystem)) + .andThen(new WaitCommand(0.5)) + : Commands.none())); + NamedCommands.registerCommand( + "StopLaunch", + (LAUNCHER_ENABLED ? new StopLauncherCommand(s.launcherSubsystem) : Commands.none())); NamedCommands.registerCommand( "RetractPivot", - new SetAngleLaunchCommand(s.launcherSubsystem, 0, 0)); // TODO: add retract angle + (LAUNCHER_ENABLED && INTAKE_ENABLED + ? new SetAngleLaunchCommand(s.launcherSubsystem, 0, 0) + : Commands.none())); // TODO: add retract angle // Complex Autos NamedCommands.registerCommand("AutoLogicTest", ComplexAutoPaths.testAuto); + NamedCommands.registerCommand( "MidSpeakerCenterLineN3N2N1", ComplexAutoPaths.midSpeakerCenterLineN3N2N1); NamedCommands.registerCommand( @@ -239,11 +255,11 @@ public static void initShuffleBoard() { gameObjects.addOption(String.valueOf(i), i); } - tab.add("Starting Position", startPositionChooser).withPosition(7, 0).withSize(2, 1); - tab.add("Launch Type", isVision).withPosition(7, 1); - tab.add("Game Objects", gameObjects).withPosition(8, 1); - tab.add("Available Auto Variants", availableAutos).withPosition(7, 2).withSize(2, 1); - autoDelayEntry = tab.add("Auto Delay", 0).withPosition(7, 3).withSize(1, 1).getEntry(); + tab.add("Starting Position", startPositionChooser).withPosition(5, 0).withSize(2, 1); + tab.add("Launch Type", isVision).withPosition(5, 1); + tab.add("Game Objects", gameObjects).withPosition(6, 1); + tab.add("Available Auto Variants", availableAutos).withPosition(5, 2).withSize(2, 1); + autoDelayEntry = tab.add("Auto Delay", 0).withPosition(5, 3).withSize(1, 1).getEntry(); isVision.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); startPositionChooser.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); diff --git a/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java index 7c8cca90..4c48ffa4 100644 --- a/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java +++ b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java @@ -1,5 +1,6 @@ package frc.team2412.robot.util.auto; +import static frc.team2412.robot.Subsystems.SubsystemConstants.*; import static frc.team2412.robot.util.auto.AutoLogic.*; import edu.wpi.first.wpilibj2.command.Command; @@ -157,17 +158,21 @@ public class ComplexAutoPaths { // new command getters public static final Command VisionLaunchCommand() { - return new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls); + return (LAUNCHER_ENABLED && INTAKE_ENABLED && APRILTAGS_ENABLED + ? new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls) + : Commands.none()); } public static final Command SubwooferLaunchCommand() { - return new SetAngleLaunchCommand( - s.launcherSubsystem, - LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, - LauncherSubsystem.SUBWOOFER_AIM_ANGLE); + return (LAUNCHER_ENABLED + ? new SetAngleLaunchCommand( + s.launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.SUBWOOFER_AIM_ANGLE) + : Commands.none()); } public static BooleanSupplier checkForTargets() { - return (s.limelightSubsystem != null ? s.limelightSubsystem::hasTargets : () -> true); + return (LIMELIGHT_ENABLED ? s.limelightSubsystem::hasTargets : () -> true); } } From 9d104ceacba620a17192ea89a554d74d753f68d2 Mon Sep 17 00:00:00 2001 From: kirby Date: Thu, 21 Mar 2024 19:08:08 -0700 Subject: [PATCH 49/49] fixed --- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 8c05dcfa..02fd9d60 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -122,8 +122,8 @@ public static enum StartPosition { List.of( // presets // vision - new AutoPath("Centerline N1 Autoline N1 N2 N3", "VisionAmpSideAutoline5Score"), - new AutoPath("Autoline N1 Centerline N1 N2 Autoline N2", "VisionAmpSide5Score")); + new AutoPath("Centerline N1 Autoline N1 N2 N3", "VisionAmpSideAutoline5Score", true), + new AutoPath("Autoline N1 Centerline N1 N2 Autoline N2", "VisionAmpSide5Score", true)); private static List fivePiecePaths;