diff --git a/.gitignore b/.gitignore index f069a0f2..a1b578d5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,8 @@ # This gitignore has been specially created by the WPILib team. # If you remove items from this file, intellisense might break. +BuildConstants.java + ### C++ ### # Prerequisites *.d diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 49968e30..a7faa89e 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -14,7 +14,7 @@ "Normal", "OBJ" ], - "defaultMaxVel": 5.0, + "defaultMaxVel": 4.5, "defaultMaxAccel": 3.5, "defaultMaxAngVel": 1137.21, "defaultMaxAngAccel": 1492.9, diff --git a/README.md b/README.md index 7422b698..035d2365 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,14 @@ -![Gradle Build](https://img.shields.io/github/actions/workflow/status/Patribots4738/Crescendo2024/gradle.yml?label=Gradle%20Build&logo=Gradle) +![Gradle Build](https://img.shields.io/github/actions/workflow/status/Patribots4738/Crescendo2024/gradle.yml?label=Gradle%20Build&logo=Gradle) | [`src/main/java/frc/robot`](src/main/java/frc/robot) shortcut -[![Game Manual](https://soflofrc.firstinflorida.org/wp-content/uploads/sites/23/2023/09/FIRST-IN-SHOW_CRESCENDO_FRC_SocialHQPDP_FB_Cover-1.png)](https://firstfrc.blob.core.windows.net/frc2024/Manual/2024GameManual.pdf) -![Robot Image](images/robot.gif) + + + + - ____ @@ -18,19 +19,23 @@ ____ This repository is entirely student-created and maintained. -Attached to this repository is a GitHub project called [Crescendo 2024]() in which we utilize as the Agile framework to organize our workflow. With Agile, we map out the season by dividing it into many week long sprints. As a team, we agree upon & decide what must be accomplished in each sprint. We declare each assignment by making issues and then implement it in an associated branch. An estimated priority & size are assigned to each issue/assignment which is then filtered into five categories: +Attached to this repository is a GitHub project called [Crescendo 2024]() in which we utilize as the Agile framework to organize our workflow. With Agile, we map out the season by dividing it into many week-long sprints. As a team, we agree upon & decide what must be accomplished in each sprint. We declare each assignment by making issues and then implement it in an associated branch. An estimated priority & size are assigned to each issue/assignment which is then filtered into five categories: - **Backlog** -> Issues that have no status. Essentially a large to-do list. - **Ready** -> Issues that are assigned to a programmer & are ready to begin. - **In Progress** -> Issues that are currently being worked on by a programmer. - **In Review** -> Issues where the assigned programmer has requested revision by colleagues. - - **Done** -> Resolved issues with corresponding branches which have merged into our master branch called [`main`](https://github.com/Patribots4738/Crescendo2024/tree/main/src/main). + - **Done** -> Resolved issues with corresponding branches which have merged into our master branch, [`main`](https://github.com/Patribots4738/Crescendo2024/tree/main/src/main). -We also love [drawing boards]()! +We also love [drawing boards](https://www.tldraw.com/v/YKJloESPqAyu62wxqEQ8U?v=1783,102,6548,3115&p=page)! We are a team of students, for students, and we are proud to be a part of the _FIRST®_ community. Thanks for checking us out, & be sure to star this repository if you find anything helpful or interesting! ### [See how we did!](https://www.statbotics.io/team/4738) + + + + ___ @@ -41,7 +46,7 @@ ___ - Shooting while driving - Fully simulated robot, mechanisms, and motors - Auto alignment to amp, speaker, stage, and source - - Color sensor oriented piece control + - Note possession via proximity sensors - Log replay using Advantage Scope - Modular autonomous routines - April Tag interpretation on Limelight -- @@ -78,12 +83,12 @@ ___ ### Path Generation & Modular Autonomous Paths - > We use `PathPlanner` to construct a modular autonomous. In `PathPlanner`, we use named waypoints, scheduled commands, & bezier curves to generate a singular auto path between a starting position, preferable shooting position, or note location. We then link multiple auto paths together to make one predetermined autonomous. Using note detection & logic, we are able to make quick decisions on whether or not we got the note, or if we should skip trying to get a note that isn't there. Additionally, there is no predetermined starting position when starting a chain of autonomous paths. This year's abundance of April tags allow us to generate a path from anywhere in the starting zone as long as our Limelight has a clear view of a tag. Feel free to check out our [Modular Auto Drawing Board]() :> + > We use `PathPlanner` to construct a modular autonomous. In `PathPlanner`, we use named waypoints, scheduled commands, & bezier curves to generate a singular auto path between a starting position, preferable shooting position, or note location. We then link multiple auto paths together to make one predetermined autonomous. Using note detection & logic, we are able to make quick decisions on whether or not we got the note, or if we should skip trying to get a note that isn't there. Additionally, there is no predetermined starting position when starting a chain of autonomous paths. This year's abundance of April tags allow us to generate a path from anywhere in the starting zone as long as our Limelight has a clear view of a tag. Feel free to check out our [Modular Auto Drawing Board](https://www.tldraw.com/v/YKJloESPqAyu62wxqEQ8U?v=-1110,-527,5267,2375&p=7BIJ2pOYpdPJlpt0lYwGi) :>
### Note Detection - > Using `Limelight`'s machine learning algorithm accelerated by a `Google Coral`, we can detect notes from ~13 feet away. Note detection is incorporated in our modular autonomous logic, allowing the robot to hone in on a note and intake it by itself by using path generation and a holonomic drive controller. This is especially useful when the robot is at the center line where path generation without vision can become inaccurate, preventing notes from bumping off of a swerve module. Additionally, we can skip over a spike if no note is detected, helping us cut off the faster robots on the other alliance. Those opposing robots are fast! + > Using `Limelight`'s machine learning algorithm accelerated by a `Google Coral`, we detected notes from ~13 feet away at SDR and Bayou regional. Note detection was incorporated in our modular autonomous logic, allowing the robot to hone in on a note and intake it by itself by using path generation and a holonomic drive controller. This was especially useful when the robot is at the center line where path generation without vision became inaccurate, preventing notes from bumping off of a swerve module. Additionally, we could skip over a spike if no note was detected, helping us cut off the faster robots on the other alliance. Those opposing robots are fast! > Here is a GIF of Terry detecting a note and chasing it! 🦖🔎 @@ -97,7 +102,7 @@ ___ > We use Xbox controllers to control the robot. However, the range of the joystick axis from the Xbox controller is the shape of a skewed square with chamfered edges which is preferable for usage. In PatriBoxController, we remapped the range of input to a circle that is easier to use. Here's our [desmos](https://www.desmos.com/calculator/e07raajzh5) if you want to check out the math! ### Shooting While Driving -> Our robot is able to shoot notes into the speaker while moving! This feature grants us a shorter cycle time and is most evident in autonomous. Shooting while driving uses the real and simulated values of [`noteTrajectory`](src/main/java/frc/robot/commands/logging/NoteTrajectory). If you are curious to learn more, visit our section dedicated to note trajectory in _Simulation and Testing_ or check out the [math]() behind it! +> Our robot is able to shoot notes into the speaker while moving! This feature grants us a shorter cycle time and is most evident in autonomous. Shooting while driving uses the real and simulated values of [`NoteTrajectory.java`](src/main/java/frc/robot/commands/logging/NoteTrajectory.java). If you are curious to learn more, visit our section dedicated to note trajectory in _Simulation and Testing_ or check out the [math](https://www.tldraw.com/v/YKJloESPqAyu62wxqEQ8U?v=-2813,-1245,8760,3950&p=bJblJdcnza_8k5Wko9ZFH) behind it! ### Auto Alignment w/ April Tags > Another feature for user-friendliness is our robot's ability to align to field objects such as the speaker, stage & amp. When aligned to the speaker, the driver can move the robot anywhere on the field whilst the shooter always faces the speaker. For the amp & stage, the robot becomes locked in a certain axis. This helps the driver with steering & alignment. @@ -116,37 +121,34 @@ _____ Our code is formatted in a command-based system on VS Code using Java. ### Subsystems - > [`robot/subsystems`](src/main/java/frc/robot/subsystems) Folder containing class files for each subsystem on the robot. - > - **Intake** [`robot/subsystems/intake`](src/main/java/frc/robot/subsystems/intake) An under-the-bumper intake run by a `Neo 550`. - > - **Swerve** [`robot/subsystems/swerve`](src/main/java/frc/robot/subsystems/swerve) Drivetrain with four swerve modules using 4 `Neo Vortex`s and 4 `Neo 550`s. - > - **Shooter** [`robot/subsystems/shooter`](src/main/java/frc/robot/subsystems/shooter) A shooter that uses 2 `Neo Vortexs` & pivot which uses 1 `Neo 550` with an absolute encoder. - > - **Elevator** [`robot/subsystems/elevator`](src/main/java/frc/robot/subsystems/elevator) Elevator for amp & trap placement which uses 1 `Neo v1.1`. - > - **Indexer** [`robot/subsystems/indexer`](src/main/java/frc/robot/subsystems/indexer) The Indexer between intake & shooter which uses a `Neo 550`. - > - **Climb** [`robot/subsystems/climb`](src/main/java/frc/robot/subsystems/climb) Two independently driven climbs that conform to the curve of the unoccupied chain on stage to keep the robot level with the ground. Uses one `Neo Vortex` each. - > - **LEDS** [`robot/subsystems/limelight`](src/main/java/frc/robot/subsystems/leds) Our `WS2812B` LED strip. - > - **Limelight** [`robot/subsystems/limelight`](src/main/java/frc/robot/subsystems/limelight) Interaction between the `Limelight` (2 and 3) and the robot. + > [`subsystems/`](src/main/java/frc/robot/subsystems) Folder containing class files for each subsystem on the robot. + > - **Intake** [`subsystems/Intake`](src/main/java/frc/robot/subsystems/Intake.java) An under-the-bumper intake run by a `Neo 550`. + > - **Swerve** [`subsystems/Swerve`](src/main/java/frc/robot/subsystems/Swerve.java) Drivetrain with four swerve modules using 4 `Neo Vortex`s and 4 `Neo 550`s. + > - **Shooter** [`subsystems/Shooter`](src/main/java/frc/robot/subsystems/Shooter.java) A shooter that uses 2 `Neo Vortexs` & pivot which uses 1 `Neo 550` with an absolute encoder. + > - **Elevator** [`subsystems/Elevator`](src/main/java/frc/robot/subsystems/Elevator.java) Elevator for amp & trap placement which uses 1 `Neo v1.1`. + > - **Indexer** [`subsystems/Indexer`](src/main/java/frc/robot/subsystems/Indexer.java) The Indexer between intake & shooter which uses a `Neo 550`. + > - **Climb** [`subsystems/Climb`](src/main/java/frc/robot/subsystems/Climb.java) Two independently driven climbs that conform to the curve of the unoccupied chain on stage to keep the robot level with the ground. Uses one `Neo Vortex` each. + > - **Limelight** [`subsystems/Limelight`](src/main/java/frc/robot/subsystems/Limelight.java) Interaction between the `Limelight` (2 and 3) and the robot. ### Commands -> [`robot/commands`](src/main/java/frc/robot/commands) Folder containing command files that control the robot. -> - **Alignment Commands** [`robot/commands/drive/alignmentCmds`](src/main/frc/robot/commands/drive/alignmentCmds) File of commands that help auto-align the robot to be locked on a certain axis when relative to a field object such as the stage or amp. This helps the driver with steering & alignment. -> - **Network Table PID Tuner** [`robot/commands/logging/NTPIDTuner`](src/main/frc/robot/commands/logging/NTPIDTuner) Command file that allows us to alter & tune the PID values via `Advantage Scope`'s Network Tables Tuner for ease of access. -> - **Selective Conditional Command** [`robot/commands/managers/SelectiveConditionalCommand`](src/main/frc/robot/commands/managers/SelectiveConditionalCommand) Command that is similar to `Commands.either` but instead of just checking the boolean when the Command is originally run, it constantly checks the boolean supplier and runs the correct command. -> - **Piece Control** [`robot/commands/manager/PieceControl`](src/main/robot/commands/manager/PieceControl) Command file containing various other commands that subsystems use to manage possession over the game piece. +> [`commands/`](src/main/java/frc/robot/commands) Folder containing command files that control the robot. +> - **Alignment Commands** [`commands/drive/AlignmentCmds`](src/main/java/frc/robot/commands/drive/AlignmentCmds.java) File of commands that help auto-align the robot to be locked on a certain axis when relative to a field object such as the stage or amp. This helps the driver with steering & alignment. +> - **Network Table PID Tuner** [`commands/logging/NTPIDTuner`](src/main/java/frc/robot/commands/logging/NTPIDTuner.java) Command file that allows us to alter & tune the PID values via `Advantage Scope`'s Network Tables Tuner for ease of access. +> - **Selective Conditional Command** [`commands/managers/SelectiveConditionalCommand`](src/main/java/frc/robot/commands/managers/SelectiveConditionalCommand.java) Command that is similar to `Commands.either` but instead of just checking the boolean when the Command is originally run, it constantly checks the boolean supplier and runs the correct command. +> - **Piece Control** [`commands/managers/PieceControl`](src/main/java/frc/robot/commands/managers/PieceControl.java) Command file containing various other commands that subsystems use to manage possession over the game piece. > - **getAutomaticShooterSpeeds** This is the default command for the shooter subsystem. It brings the shooter up to speed whenever the robot is in our alliance's wing and we have note possession to make the process of intaking, indexing, and then shooting significantly faster ### Utilities -> [`robot/util`](src/main/java/frc/robot/util) Folder containing values, logic, and math used by other files to help them function. -> - **Constants** [`robot/util/constants`](src/main/java/frc/robot/util/constants) contains constants used throughout the robot code to prevent a mismatch in data & hardcoding values (Ex. PIDFF values & current limits). -> - **Calc** [`robot/util/calc`](src/main/java/frc/robot/util/calc) contains the calculations required for pivot alinment & shooter speeds when shooting while driving. -> - **Auto** [`robot/util/auto`](src/main/java/frc/robot/util/auto) Folder containing the storage files for `Choreo` and `PathPlanner`. +> [`util/`](src/main/java/frc/robot/util) Folder containing values, logic, and math used by other files to help them function. +> - **Constants** [`util/constants`](src/main/java/frc/robot/util/Constants.java) contains constants used throughout the robot code to prevent a mismatch in data & hardcoding values (Ex. PIDFF values & current limits). +> - **Calc** [`util/calc`](src/main/java/frc/robot/util/calc) contains the calculations required for pivot alinment & shooter speeds when shooting while driving. +> - **Auto** [`util/auto`](src/main/java/frc/robot/util/auto) Folder containing the storage files for `Choreo` and `PathPlanner`. ## Controls 🎮 -[![Driver](/images/drivercontrols.png)](https://www.tldraw.com/r/EolJKYU3QEqxw71uyAqPS?viewport=2339,251,5877,2813&page=page:page) - -[![Operator](/images/operatorcontrols.png)](https://www.tldraw.com/r/EolJKYU3QEqxw71uyAqPS?viewport=2339,251,5877,2813&page=page:page) +[![Driver](/images/drivercontrols.png)](https://www.tldraw.com/v/YKJloESPqAyu62wxqEQ8U?v=811,257,6108,2754&p=page) ## Components & Tools 🛠️ [![Hardware](/images/hardware.png)](https://www.tldraw.com/r/EolJKYU3QEqxw71uyAqPS?viewport=-4823,-6599,9853,4716&page=page:g60UEEXm6O2yBIoLYfVVB) -[![Software](/images/software.png)](https://www.tldraw.com/r/EolJKYU3QEqxw71uyAqPS?viewport=-4823,-6599,9853,4716&page=page:g60UEEXm6O2yBIoLYfVVB) +[![Software](/images/software.png)](https://www.tldraw.com/r/EolJKYU3QEqxw71uyAqPS?viewport=-4823,-6599,9853,4716&page=page:g60UEEXm6O2yBIoLYfVVB) \ No newline at end of file diff --git a/build.gradle b/build.gradle index c95d364c..e32d5895 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,7 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2024.3.2" + id "com.peterabeles.gversion" version "1.10" } repositories { @@ -17,6 +18,7 @@ def ROBOT_MAIN_CLASS = "frc.robot.Main" // Define my targets (RoboRIO) and artifacts (deployable files) // This is added by GradleRIO's backing project DeployUtils. deploy { + targets { roborio(getTargetTypeClass('RoboRIO')) { // Team number is loaded either from the .wpilib/wpilib_preferences.json @@ -72,6 +74,9 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.github.shueja:Monologue:v1.0.0-beta6' + + def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" } test { @@ -102,3 +107,34 @@ wpi.java.configureTestTasks(test) tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' } + +repositories { + maven { + url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit") + credentials { + username = "Mechanical-Advantage-Bot" + password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051" + } + } + mavenLocal() +} + +configurations.all { + exclude group: "edu.wpi.first.wpilibj" +} + +task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { + mainClass = "org.littletonrobotics.junction.CheckInstall" + classpath = sourceSets.main.runtimeClasspath +} +compileJava.finalizedBy checkAkitInstall + +project.compileJava.dependsOn(createVersionFile) +gversion { + srcDir = "src/main/java/" + classPackage = "frc.robot" + className = "BuildConstants" + dateFormat = "yyyy-MM-dd HH:mm:ss z" + timeZone = "America/Los_Angeles" // Use preferred time zone + indent = " " +} \ No newline at end of file diff --git a/images/GIF/photoSlideshow.gif b/images/GIF/photoSlideshow.gif new file mode 100644 index 00000000..27cc4a73 Binary files /dev/null and b/images/GIF/photoSlideshow.gif differ diff --git a/images/drivercontrols.png b/images/drivercontrols.png index ef65de9d..7ad91478 100644 Binary files a/images/drivercontrols.png and b/images/drivercontrols.png differ diff --git a/images/terry/IMG_0010.jpg b/images/terry/IMG_0010.jpg new file mode 100644 index 00000000..0f136c2e Binary files /dev/null and b/images/terry/IMG_0010.jpg differ diff --git a/images/terry/IMG_0298.jpg b/images/terry/IMG_0298.jpg new file mode 100644 index 00000000..906b81d6 Binary files /dev/null and b/images/terry/IMG_0298.jpg differ diff --git a/images/terry/IMG_0308.jpg b/images/terry/IMG_0308.jpg new file mode 100644 index 00000000..16c9cc90 Binary files /dev/null and b/images/terry/IMG_0308.jpg differ diff --git a/images/terry/IMG_1836.jpg b/images/terry/IMG_1836.jpg new file mode 100644 index 00000000..ca671a82 Binary files /dev/null and b/images/terry/IMG_1836.jpg differ diff --git a/images/terry/IMG_3623.jpg b/images/terry/IMG_3623.jpg new file mode 100644 index 00000000..82c3681d Binary files /dev/null and b/images/terry/IMG_3623.jpg differ diff --git a/images/terry/IMG_3624.jpg b/images/terry/IMG_3624.jpg new file mode 100644 index 00000000..dcea1d83 Binary files /dev/null and b/images/terry/IMG_3624.jpg differ diff --git a/images/terry/IMG_3625.jpg b/images/terry/IMG_3625.jpg new file mode 100644 index 00000000..d1a18506 Binary files /dev/null and b/images/terry/IMG_3625.jpg differ diff --git a/images/terry/thumbnail_IMG_3626.jpg b/images/terry/thumbnail_IMG_3626.jpg new file mode 100644 index 00000000..d959c06b Binary files /dev/null and b/images/terry/thumbnail_IMG_3626.jpg differ diff --git a/src/main/deploy/pathplanner/autos/S C1-5 S Over W1 DEF.auto b/src/main/deploy/pathplanner/autos/S C1-5 S Over W1 DEF.auto new file mode 100644 index 00000000..a88d7aaf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S C1-5 S Over W1 DEF.auto @@ -0,0 +1,69 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2422530556147342, + "y": 7.161301131112901 + }, + "rotation": -124.91237183543709 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "FullPowerPreload2" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S C1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C1toC5default" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S C1-5 S Over W1.auto b/src/main/deploy/pathplanner/autos/S C1-5 S Over W1.auto index e5434ad5..52ede7d1 100644 --- a/src/main/deploy/pathplanner/autos/S C1-5 S Over W1.auto +++ b/src/main/deploy/pathplanner/autos/S C1-5 S Over W1.auto @@ -5,36 +5,16 @@ "x": 1.2422530556147342, "y": 7.161301131112901 }, - "rotation": -128.23889760719103 + "rotation": -124.91237183543709 }, "command": { "type": "sequential", "data": { "commands": [ { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ShootInstantlyWhenReady" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "PrepareSWD" - } - } - ] + "name": "FullPowerPreload2" } }, { @@ -65,15 +45,15 @@ "data": { "commands": [ { - "type": "wait", + "type": "named", "data": { - "waitTime": 1.7 + "name": "StopAll" } }, { - "type": "named", + "type": "wait", "data": { - "name": "StopAll" + "waitTime": 1.0 } }, { diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S C2-3 S 2 OBJ.auto b/src/main/deploy/pathplanner/autos/S C2-3 S.auto similarity index 63% rename from src/main/deploy/pathplanner/autos/S W3-1 S C2-3 S 2 OBJ.auto rename to src/main/deploy/pathplanner/autos/S C2-3 S.auto index aa72bbba..bd0c6f7d 100644 --- a/src/main/deploy/pathplanner/autos/S W3-1 S C2-3 S 2 OBJ.auto +++ b/src/main/deploy/pathplanner/autos/S C2-3 S.auto @@ -2,17 +2,23 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.3387789373563073, - "y": 5.55 + "x": 1.3216735004898588, + "y": 6.301761136557157 }, - "rotation": 179.22577983507193 + "rotation": -151.5616809371522 }, "command": { "type": "sequential", "data": { "commands": [ { - "type": "deadline", + "type": "named", + "data": { + "name": "FullPowerPreload3" + } + }, + { + "type": "race", "data": { "commands": [ { @@ -20,62 +26,57 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "ShootWhenReady" + "pathName": "S C2" } }, { - "type": "path", + "type": "wait", "data": { - "pathName": "S W3-1 2" + "waitTime": 0.3 } - }, + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ { "type": "named", "data": { - "name": "ShootInstantlyWhenReady" + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 } }, { "type": "named", "data": { - "name": "EnableLimelight" + "name": "ToIndexer" } } ] } - }, - { - "type": "named", - "data": { - "name": "PrepareSWD" - } } ] } }, - { - "type": "path", - "data": { - "pathName": "S C2 N" - } - }, - { - "type": "named", - "data": { - "name": "C2toC3" - } - }, { "type": "named", "data": { - "name": "C3toC1" + "name": "C2toC3nonOBJ" } } ] } }, - "folder": "OBJ", + "folder": "Normal", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S C4-1 S DEF.auto b/src/main/deploy/pathplanner/autos/S C4-1 S DEF.auto new file mode 100644 index 00000000..cc15e83c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S C4-1 S DEF.auto @@ -0,0 +1,108 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2698318789694694, + "y": 3.0612493923756046 + }, + "rotation": 116.80464903166519 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P R" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "BoostShooterR" + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "R C4" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C4toC1default" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S C5-1 S DEF.auto b/src/main/deploy/pathplanner/autos/S C5-1 S DEF.auto new file mode 100644 index 00000000..82015e51 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S C5-1 S DEF.auto @@ -0,0 +1,108 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2698318789694694, + "y": 3.0612493923756046 + }, + "rotation": 116.80464903166519 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P R" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "BoostShooterR" + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "R C5" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C5toC1default" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S C5-1 S.auto b/src/main/deploy/pathplanner/autos/S C5-1 S.auto index 24db8690..79b329d0 100644 --- a/src/main/deploy/pathplanner/autos/S C5-1 S.auto +++ b/src/main/deploy/pathplanner/autos/S C5-1 S.auto @@ -5,7 +5,7 @@ "x": 1.2698318789694694, "y": 3.0612493923756046 }, - "rotation": 117.98745319075599 + "rotation": 116.80464903166519 }, "command": { "type": "sequential", @@ -35,9 +35,22 @@ } }, { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareSWD" + "commands": [ + { + "type": "named", + "data": { + "name": "BoostShooterR" + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] } } ] @@ -54,7 +67,7 @@ { "type": "path", "data": { - "pathName": "S C5" + "pathName": "R C5" } }, { diff --git a/src/main/deploy/pathplanner/autos/S C5-3 3-5 S.auto b/src/main/deploy/pathplanner/autos/S C5-3 3-5 S.auto new file mode 100644 index 00000000..7129ad78 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S C5-3 3-5 S.auto @@ -0,0 +1,88 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.701401686491316, + "y": 4.439424481685842 + }, + "rotation": 120.60276179805513 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "FullPowerPreload3" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S C5 2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C5toC3nonOBJ" + } + }, + { + "type": "named", + "data": { + "name": "C3toC5nonOBJ" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S W1 S C1-5 S DEF.auto b/src/main/deploy/pathplanner/autos/S W1 S C1-5 S DEF.auto new file mode 100644 index 00000000..9adcf5ca --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S W1 S C1-5 S DEF.auto @@ -0,0 +1,107 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.182727334398768, + "y": 6.395529951663276 + }, + "rotation": -142.27500495788928 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P CA1" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady2" + } + }, + { + "type": "path", + "data": { + "pathName": "CA1 W1" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "W1 C1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C1toC5default" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S C1-2 S 2.auto b/src/main/deploy/pathplanner/autos/S W1 S C1-5 S.auto similarity index 86% rename from src/main/deploy/pathplanner/autos/S W3-1 S C1-2 S 2.auto rename to src/main/deploy/pathplanner/autos/S W1 S C1-5 S.auto index d39a1d50..28d295a9 100644 --- a/src/main/deploy/pathplanner/autos/S W3-1 S C1-2 S 2.auto +++ b/src/main/deploy/pathplanner/autos/S W1 S C1-5 S.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.29, - "y": 5.55 + "x": 1.182727334398768, + "y": 6.395529951663276 }, - "rotation": 179.22577983507193 + "rotation": -142.27500495788928 }, "command": { "type": "sequential", @@ -20,21 +20,21 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "ShootWhenReady" + "pathName": "P CA1" } }, { - "type": "path", + "type": "named", "data": { - "pathName": "S W3-1 2" + "name": "ShootInstantlyWhenReady2" } }, { - "type": "named", + "type": "path", "data": { - "name": "EnableLimelight" + "pathName": "CA1 W1" } }, { @@ -109,13 +109,7 @@ { "type": "named", "data": { - "name": "C1toC2nonOBJ" - } - }, - { - "type": "path", - "data": { - "pathName": "L C2" + "name": "C1toC5nonOBJ" } } ] diff --git a/src/main/deploy/pathplanner/autos/S W1 S C2-5 S DEF.auto b/src/main/deploy/pathplanner/autos/S W1 S C2-5 S DEF.auto new file mode 100644 index 00000000..b041bcfd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S W1 S C2-5 S DEF.auto @@ -0,0 +1,107 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3140196323188638, + "y": 7.14361029292672 + }, + "rotation": -142.27500495788928 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P CA1" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady2" + } + }, + { + "type": "path", + "data": { + "pathName": "CA1 W1" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "W1 C2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C2toC5default" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S W2 S C1-2 S W1 S C3-5 S.auto b/src/main/deploy/pathplanner/autos/S W2 S C1-2 S W1 S C3-5 S.auto new file mode 100644 index 00000000..d1259765 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S W2 S C1-2 S W1 S C3-5 S.auto @@ -0,0 +1,221 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3575481986811355, + "y": 5.572415049000463 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P CA2" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady2" + } + }, + { + "type": "path", + "data": { + "pathName": "CA2 W2" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady2" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "W2 C1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C1toC2nonOBJ" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "C2 CA1" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady2" + } + }, + { + "type": "path", + "data": { + "pathName": "CA1 W1" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady2" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "W1 C3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C3toC5nonOBJ" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S W2 S C1-5 S.auto b/src/main/deploy/pathplanner/autos/S W2 S C1-5 S.auto new file mode 100644 index 00000000..a51877c6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S W2 S C1-5 S.auto @@ -0,0 +1,120 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3575481986811355, + "y": 5.572415049000463 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P CA2" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady2" + } + }, + { + "type": "path", + "data": { + "pathName": "CA2 W2" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady1" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "W2 C1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C1toC5nonOBJ" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S W2 S C3-5 S DEF.auto b/src/main/deploy/pathplanner/autos/S W2 S C3-5 S DEF.auto new file mode 100644 index 00000000..2fddb543 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S W2 S C3-5 S DEF.auto @@ -0,0 +1,107 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.36, + "y": 5.572415049000463 + }, + "rotation": -179.42706130231647 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P CA2" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady2" + } + }, + { + "type": "path", + "data": { + "pathName": "CA2 W2" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "W2 C3" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C3toC5default" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S W2 S C3-5 S.auto b/src/main/deploy/pathplanner/autos/S W2 S C3-5 S.auto new file mode 100644 index 00000000..d5c2d830 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S W2 S C3-5 S.auto @@ -0,0 +1,120 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.36, + "y": 5.572415049000463 + }, + "rotation": -179.42706130231647 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P CA2" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady2" + } + }, + { + "type": "path", + "data": { + "pathName": "CA2 W2" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady1" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "W2 C3" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C3toC5nonOBJ" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S C1-5 S DEF.auto b/src/main/deploy/pathplanner/autos/S W3-1 S C1-5 S DEF.auto new file mode 100644 index 00000000..b92cda53 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S W3-1 S C1-5 S DEF.auto @@ -0,0 +1,101 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.29235458470917, + "y": 4.778983272252259 + }, + "rotation": 151.53108100340572 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "FullPowerPreload" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S W3-1 Bayou" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "W1 C1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C1toC5default" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S C1-3 S.auto b/src/main/deploy/pathplanner/autos/S W3-1 S C1-5 S.auto similarity index 88% rename from src/main/deploy/pathplanner/autos/S W3-1 S C1-3 S.auto rename to src/main/deploy/pathplanner/autos/S W3-1 S C1-5 S.auto index 52239e54..cb77a592 100644 --- a/src/main/deploy/pathplanner/autos/S W3-1 S C1-3 S.auto +++ b/src/main/deploy/pathplanner/autos/S W3-1 S C1-5 S.auto @@ -11,6 +11,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "FullPowerPreload" + } + }, { "type": "deadline", "data": { @@ -19,22 +25,10 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "ShootWhenReady" - } - }, { "type": "path", "data": { - "pathName": "S W3-1" - } - }, - { - "type": "named", - "data": { - "name": "EnableLimelight" + "pathName": "S W3-1 Bayou" } }, { @@ -88,6 +82,12 @@ "name": "StopAll" } }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S C2-5 S 2.auto b/src/main/deploy/pathplanner/autos/S W3-1 S C2-3 S.auto similarity index 88% rename from src/main/deploy/pathplanner/autos/S W3-1 S C2-5 S 2.auto rename to src/main/deploy/pathplanner/autos/S W3-1 S C2-3 S.auto index 94dd8b6d..9ef44bd8 100644 --- a/src/main/deploy/pathplanner/autos/S W3-1 S C2-5 S 2.auto +++ b/src/main/deploy/pathplanner/autos/S W3-1 S C2-3 S.auto @@ -2,15 +2,21 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.29, - "y": 5.55 + "x": 1.29235458470917, + "y": 4.778983272252259 }, - "rotation": 179.22577983507193 + "rotation": 151.33 }, "command": { "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "FullPowerPreload" + } + }, { "type": "deadline", "data": { @@ -19,16 +25,10 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "ShootWhenReady" - } - }, { "type": "path", "data": { - "pathName": "S W3-1 2" + "pathName": "S W3-1 Bayou" } }, { @@ -109,7 +109,7 @@ { "type": "named", "data": { - "name": "C2toC5nonOBJ" + "name": "C2toC3nonOBJ" } } ] diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S C2-5 S DEF.auto b/src/main/deploy/pathplanner/autos/S W3-1 S C2-5 S DEF.auto new file mode 100644 index 00000000..4d2be2a9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/S W3-1 S C2-5 S DEF.auto @@ -0,0 +1,101 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.29235458470917, + "y": 4.778983272252259 + }, + "rotation": 151.53108100340572 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "FullPowerPreload" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S W3-1 Bayou" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "W1 C2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C2toC5default" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S C2-5 S.auto b/src/main/deploy/pathplanner/autos/S W3-1 S C2-5 S.auto index 84e635bb..4939ca12 100644 --- a/src/main/deploy/pathplanner/autos/S W3-1 S C2-5 S.auto +++ b/src/main/deploy/pathplanner/autos/S W3-1 S C2-5 S.auto @@ -11,6 +11,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "FullPowerPreload" + } + }, { "type": "deadline", "data": { @@ -19,16 +25,10 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "ShootWhenReady" - } - }, { "type": "path", "data": { - "pathName": "S W3-1" + "pathName": "S W3-1 Bayou" } }, { @@ -47,9 +47,16 @@ } }, { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareSWD" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S C1-2 S.auto b/src/main/deploy/pathplanner/autos/S W3-1 S C3-5 S.auto similarity index 88% rename from src/main/deploy/pathplanner/autos/S W3-1 S C1-2 S.auto rename to src/main/deploy/pathplanner/autos/S W3-1 S C3-5 S.auto index 7c6efccd..80f14d3d 100644 --- a/src/main/deploy/pathplanner/autos/S W3-1 S C1-2 S.auto +++ b/src/main/deploy/pathplanner/autos/S W3-1 S C3-5 S.auto @@ -11,6 +11,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "FullPowerPreload" + } + }, { "type": "deadline", "data": { @@ -19,16 +25,10 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "ShootWhenReady" - } - }, { "type": "path", "data": { - "pathName": "S W3-1" + "pathName": "S W3-1 Bayou" } }, { @@ -47,9 +47,16 @@ } }, { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareSWD" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] } } ] @@ -66,7 +73,7 @@ { "type": "path", "data": { - "pathName": "W1 C1" + "pathName": "W1 C3" } }, { @@ -109,13 +116,7 @@ { "type": "named", "data": { - "name": "C1toC2nonOBJ" - } - }, - { - "type": "path", - "data": { - "pathName": "L C2" + "name": "C3toC5nonOBJ" } } ] diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S Old.auto b/src/main/deploy/pathplanner/autos/S W3-1 S Old.auto deleted file mode 100644 index 0f2721ca..00000000 --- a/src/main/deploy/pathplanner/autos/S W3-1 S Old.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.31, - "y": 4.397 - }, - "rotation": 142.0 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "S W3" - } - }, - { - "type": "path", - "data": { - "pathName": "W3 W2" - } - }, - { - "type": "path", - "data": { - "pathName": "W2 W1" - } - } - ] - } - }, - "folder": "Normal", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/S W3-1 S.auto b/src/main/deploy/pathplanner/autos/S W3-1 S.auto index ba32f05b..6ac39b8a 100644 --- a/src/main/deploy/pathplanner/autos/S W3-1 S.auto +++ b/src/main/deploy/pathplanner/autos/S W3-1 S.auto @@ -11,6 +11,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "FullPowerPreload" + } + }, { "type": "deadline", "data": { @@ -19,16 +25,10 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "ShootWhenReady" - } - }, { "type": "path", "data": { - "pathName": "S W3-1" + "pathName": "S W3-1 Bayou" } }, { @@ -47,9 +47,16 @@ } }, { - "type": "named", + "type": "sequential", "data": { - "name": "PrepareSWD" + "commands": [ + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/X-MEN.auto b/src/main/deploy/pathplanner/autos/X-MEN.auto new file mode 100644 index 00000000..cd248ac7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/X-MEN.auto @@ -0,0 +1,215 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3575481986811355, + "y": 5.572415049000463 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P CA2" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady2" + } + }, + { + "type": "path", + "data": { + "pathName": "CA2 W2" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady1" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "W2 C1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "C1 CA1" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady1" + } + }, + { + "type": "path", + "data": { + "pathName": "CA1 W1" + } + }, + { + "type": "named", + "data": { + "name": "ShootInstantlyWhenReady2" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "PrepareSWD" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "W1 C2" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopAll" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "ToIndexer" + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "C2toC5nonOBJ" + } + } + ] + } + }, + "folder": "Normal", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index edc403d0..bab0da93 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A C1 N.path b/src/main/deploy/pathplanner/paths/A C1 N.path index c086182b..01a117fa 100644 --- a/src/main/deploy/pathplanner/paths/A C1 N.path +++ b/src/main/deploy/pathplanner/paths/A C1 N.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/A C1.path b/src/main/deploy/pathplanner/paths/A C1.path index 963febfc..2d4a37d3 100644 --- a/src/main/deploy/pathplanner/paths/A C1.path +++ b/src/main/deploy/pathplanner/paths/A C1.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 8.26566006995396, - "y": 7.4370893646602525 + "x": 7.917787299441183, + "y": 7.552716871956289 }, "prevControl": { - "x": 7.57611633858086, - "y": 6.750126570328318 + "x": 7.228243568068083, + "y": 6.865754077624354 }, "nextControl": null, "isLocked": false, @@ -111,7 +111,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/A C2.path b/src/main/deploy/pathplanner/paths/A C2.path index f078fc39..986c3375 100644 --- a/src/main/deploy/pathplanner/paths/A C2.path +++ b/src/main/deploy/pathplanner/paths/A C2.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 8.26566006995396, - "y": 5.77776349281702 + "x": 8.265148290642474, + "y": 6.0100254110329105 }, "prevControl": { - "x": 8.136958894298528, - "y": 5.584711729333875 + "x": 8.136447114987043, + "y": 5.816973647549765 }, "nextControl": null, "isLocked": false, @@ -88,7 +88,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/A C3.path b/src/main/deploy/pathplanner/paths/A C3.path index 328c36c7..dfbe9089 100644 --- a/src/main/deploy/pathplanner/paths/A C3.path +++ b/src/main/deploy/pathplanner/paths/A C3.path @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/A W1.path b/src/main/deploy/pathplanner/paths/A W1.path index c623be20..35df5213 100644 --- a/src/main/deploy/pathplanner/paths/A W1.path +++ b/src/main/deploy/pathplanner/paths/A W1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": { - "x": 1.844887968272587, - "y": 5.788662487005561 + "x": 1.8357737113864512, + "y": 5.786282288984219 }, "nextControl": null, "isLocked": false, @@ -56,7 +56,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/A W2.path b/src/main/deploy/pathplanner/paths/A W2.path index 6f047834..0f03106e 100644 --- a/src/main/deploy/pathplanner/paths/A W2.path +++ b/src/main/deploy/pathplanner/paths/A W2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.901578927457967, - "y": 5.538747023742648 + "x": 2.8726623557949864, + "y": 5.572415049000463 }, "prevControl": { - "x": 1.431505407685358, - "y": 4.7974124710295385 + "x": 1.4025888360223773, + "y": 4.8310804962873535 }, "nextControl": null, "isLocked": false, @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/A W3.path b/src/main/deploy/pathplanner/paths/A W3.path index 37933b4c..b2d8cdda 100644 --- a/src/main/deploy/pathplanner/paths/A W3.path +++ b/src/main/deploy/pathplanner/paths/A W3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.506282459373429, - "y": 4.359369286114458 + "x": 2.370694368507382, + "y": 4.369015382098561 }, "prevControl": { - "x": 1.5253773100168002, - "y": 4.071228398490948 + "x": 1.3897892191507528, + "y": 4.080874494475051 }, "nextControl": null, "isLocked": false, @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/C1 A.path b/src/main/deploy/pathplanner/paths/C1 A.path index 6ce13677..b84510b2 100644 --- a/src/main/deploy/pathplanner/paths/C1 A.path +++ b/src/main/deploy/pathplanner/paths/C1 A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.26566006995396, - "y": 7.4370893646602525 + "x": 7.917787299441183, + "y": 7.552716871956289 }, "prevControl": null, "nextControl": { - "x": 7.466582075305519, - "y": 6.606866214482267 + "x": 7.118709304792742, + "y": 6.722493721778303 }, "isLocked": false, "linkedName": "C1" @@ -83,7 +83,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/C1 C2.path b/src/main/deploy/pathplanner/paths/C1 C2.path index 3907a318..8780658d 100644 --- a/src/main/deploy/pathplanner/paths/C1 C2.path +++ b/src/main/deploy/pathplanner/paths/C1 C2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.26566006995396, - "y": 7.4370893646602525 + "x": 7.917787299441183, + "y": 7.552716871956289 }, "prevControl": null, "nextControl": { - "x": 8.477097715673596, - "y": 6.931477603156774 + "x": 8.129224945160818, + "y": 7.047105110452811 }, "isLocked": false, "linkedName": "C1" }, { "anchor": { - "x": 8.26566006995396, - "y": 5.77776349281702 + "x": 8.265148290642474, + "y": 6.0100254110329105 }, "prevControl": { - "x": 8.495483597910086, - "y": 6.2833752543204975 + "x": 8.4949718185986, + "y": 6.515637172536388 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C1 CA1.path b/src/main/deploy/pathplanner/paths/C1 CA1.path new file mode 100644 index 00000000..d91b6ec3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C1 CA1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.917787299441183, + "y": 7.552716871956289 + }, + "prevControl": null, + "nextControl": { + "x": 5.40204964326284, + "y": 6.263494567892624 + }, + "isLocked": false, + "linkedName": "C1" + }, + { + "anchor": { + "x": 2.0635131712488803, + "y": 6.792285196553275 + }, + "prevControl": { + "x": 2.012523752499856, + "y": 5.648665376039454 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CA1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + }, + "goalEndState": { + "velocity": 0, + "rotation": -145.2651404602137, + "rotateFast": false + }, + "reversed": false, + "folder": "Center Line", + "previewStartingState": { + "rotation": -179.10033357975664, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C1 L.path b/src/main/deploy/pathplanner/paths/C1 L.path index 90b99d1f..bad58543 100644 --- a/src/main/deploy/pathplanner/paths/C1 L.path +++ b/src/main/deploy/pathplanner/paths/C1 L.path @@ -3,33 +3,51 @@ "waypoints": [ { "anchor": { - "x": 8.26566006995396, - "y": 7.4370893646602525 + "x": 7.917787299441183, + "y": 7.552716871956289 }, "prevControl": null, "nextControl": { - "x": 6.3719141995954764, - "y": 7.156704660553778 + "x": 5.849565701396976, + "y": 7.423189511863697 }, "isLocked": false, "linkedName": "C1" }, { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.831849489661888, + "y": 6.499514282908736 }, "prevControl": { - "x": 5.919544888735168, - "y": 6.892024564190972 + "x": 5.412812423342392, + "y": 7.06163443710625 }, "nextControl": null, "isLocked": false, "linkedName": "L" } ], - "rotationTargets": [], - "constraintZones": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.7, + "rotationDegrees": -167.01, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "GrabVision", + "minWaypointRelativePos": 0.65, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + } + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, @@ -39,13 +57,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -173.5955883034978, + "rotation": -167.0053832080836, "rotateFast": false }, "reversed": false, "folder": "Center Line", "previewStartingState": { - "rotation": -179.2648316228425, + "rotation": -169.71979428827126, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/C2 A.path b/src/main/deploy/pathplanner/paths/C2 A.path index 2958f2f1..2c2a6977 100644 --- a/src/main/deploy/pathplanner/paths/C2 A.path +++ b/src/main/deploy/pathplanner/paths/C2 A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.26566006995396, - "y": 5.77776349281702 + "x": 8.265148290642474, + "y": 6.0100254110329105 }, "prevControl": null, "nextControl": { - "x": 2.8722220244251524, - "y": 7.384549802459921 + "x": 2.8717102451136665, + "y": 7.616811720675812 }, "isLocked": false, "linkedName": "C2" @@ -67,7 +67,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/C2 C1.path b/src/main/deploy/pathplanner/paths/C2 C1.path index 73c7fa47..86a5f003 100644 --- a/src/main/deploy/pathplanner/paths/C2 C1.path +++ b/src/main/deploy/pathplanner/paths/C2 C1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.26566006995396, - "y": 5.77776349281702 + "x": 8.265148290642474, + "y": 6.0100254110329105 }, "prevControl": null, "nextControl": { - "x": 8.454115362877982, - "y": 6.278778783761375 + "x": 8.453603583566496, + "y": 6.511040701977265 }, "isLocked": false, "linkedName": "C2" }, { "anchor": { - "x": 8.26566006995396, - "y": 7.4370893646602525 + "x": 7.917787299441183, + "y": 7.552716871956289 }, "prevControl": { - "x": 8.490887127350964, - "y": 6.890109368124671 + "x": 8.143014356838187, + "y": 7.005736875420707 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C2 C3.path b/src/main/deploy/pathplanner/paths/C2 C3.path index 97794160..a42e7010 100644 --- a/src/main/deploy/pathplanner/paths/C2 C3.path +++ b/src/main/deploy/pathplanner/paths/C2 C3.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.26566006995396, - "y": 5.77776349281702 + "x": 8.265148290642474, + "y": 6.0100254110329105 }, "prevControl": null, "nextControl": { - "x": 8.39397820639613, - "y": 5.349142613178843 + "x": 8.393466427084643, + "y": 5.581404531394734 }, "isLocked": false, "linkedName": "C2" }, { "anchor": { - "x": 8.26566006995396, - "y": 4.100051738737296 + "x": 8.2, + "y": 3.8886140930176603 }, "prevControl": { - "x": 8.50314438217529, - "y": 4.534035167361115 + "x": 8.316948681889095, + "y": 4.271135995402875 }, "nextControl": null, "isLocked": false, - "linkedName": "C3" + "linkedName": null } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0.7, - "rotation": 77.72123462903704, + "rotation": 71.97575179161221, "rotateFast": true }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C2 CA1.path b/src/main/deploy/pathplanner/paths/C2 CA1.path new file mode 100644 index 00000000..9cca3e72 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/C2 CA1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.265148290642474, + "y": 6.0100254110329105 + }, + "prevControl": null, + "nextControl": { + "x": 5.839144588590704, + "y": 8.354446043086238 + }, + "isLocked": false, + "linkedName": "C2" + }, + { + "anchor": { + "x": 2.0635131712488803, + "y": 6.792285196553275 + }, + "prevControl": { + "x": 2.795939750565216, + "y": 4.818175218164729 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CA1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + }, + "goalEndState": { + "velocity": 0, + "rotation": -142.40773798947316, + "rotateFast": false + }, + "reversed": false, + "folder": "Center Line", + "previewStartingState": { + "rotation": -178.07027630837132, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/C2 L.path b/src/main/deploy/pathplanner/paths/C2 L.path index 62958f68..2b9bc4f5 100644 --- a/src/main/deploy/pathplanner/paths/C2 L.path +++ b/src/main/deploy/pathplanner/paths/C2 L.path @@ -3,33 +3,51 @@ "waypoints": [ { "anchor": { - "x": 8.26566006995396, - "y": 5.77776349281702 + "x": 8.265148290642474, + "y": 6.0100254110329105 }, "prevControl": null, "nextControl": { - "x": 6.654597138981512, - "y": 7.154023386060957 + "x": 6.254180001081159, + "y": 6.916214910138103 }, "isLocked": false, "linkedName": "C2" }, { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.831849489661888, + "y": 6.499514282908736 }, "prevControl": { - "x": 4.653217249696911, - "y": 6.87019132903514 + "x": 5.272284531787999, + "y": 6.593167959384961 }, "nextControl": null, "isLocked": false, "linkedName": "L" } ], - "rotationTargets": [], - "constraintZones": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.7, + "rotationDegrees": -168.27, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "GrabVision", + "minWaypointRelativePos": 0.65, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + } + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, @@ -39,13 +57,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -174.47545216616192, + "rotation": -168.27488798483498, "rotateFast": false }, "reversed": false, "folder": "Center Line", "previewStartingState": { - "rotation": 179.83225523650648, + "rotation": 157.34507210798859, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/C3 C2.path b/src/main/deploy/pathplanner/paths/C3 C2.path index 744b3c7a..f60aa20f 100644 --- a/src/main/deploy/pathplanner/paths/C3 C2.path +++ b/src/main/deploy/pathplanner/paths/C3 C2.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 8.26566006995396, - "y": 5.77776349281702 + "x": 8.2, + "y": 5.970815256300165 }, "prevControl": { - "x": 8.490887127350964, - "y": 5.221590555163193 + "x": 8.326509069384601, + "y": 5.5598463336753 }, "nextControl": null, "isLocked": false, - "linkedName": "C2" + "linkedName": null } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0.7, - "rotation": -81.68418491677046, + "rotation": -70.69121045523642, "rotateFast": true }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C3 C4.path b/src/main/deploy/pathplanner/paths/C3 C4.path index be2a8c8a..4b2bf064 100644 --- a/src/main/deploy/pathplanner/paths/C3 C4.path +++ b/src/main/deploy/pathplanner/paths/C3 C4.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 8.288642422749572, - "y": 2.436296745792339 + "x": 8.2, + "y": 2.219712240842921 }, "prevControl": { - "x": 8.46790477455535, - "y": 2.909565863924561 + "x": 8.573063706037212, + "y": 2.6643122527792795 }, "nextControl": null, "isLocked": false, - "linkedName": "C4" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/C3 M.path b/src/main/deploy/pathplanner/paths/C3 M.path index defca4f1..70507198 100644 --- a/src/main/deploy/pathplanner/paths/C3 M.path +++ b/src/main/deploy/pathplanner/paths/C3 M.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 6.746909589377223, - "y": 3.9453038965801706 + "y": 3.94530389658017 }, "isLocked": false, "linkedName": "C3" @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": { - "x": 4.543668034704492, - "y": 4.530204775228513 + "x": 4.772671935242799, + "y": 4.554072325635369 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,19 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "GrabVision", + "minWaypointRelativePos": 1.4, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + } + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, @@ -55,7 +67,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 172.85862277789553, + "rotation": 173.27446529773997, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C4 C3.path b/src/main/deploy/pathplanner/paths/C4 C3.path index dcb9f9aa..8dffa57d 100644 --- a/src/main/deploy/pathplanner/paths/C4 C3.path +++ b/src/main/deploy/pathplanner/paths/C4 C3.path @@ -12,20 +12,20 @@ "y": 2.950934098956663 }, "isLocked": false, - "linkedName": "C4" + "linkedName": null }, { "anchor": { - "x": 8.26566006995396, - "y": 4.095455268178173 + "x": 8.182923599889754, + "y": 4.25 }, "prevControl": { - "x": 8.527658891823943, - "y": 3.5806505655564504 + "x": 8.48131114701946, + "y": 3.6823358859483677 }, "nextControl": null, "isLocked": false, - "linkedName": "C3" + "linkedName": null } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0.7, - "rotation": -78.62159336734791, + "rotation": -70.94229548987173, "rotateFast": true }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C4 C5.path b/src/main/deploy/pathplanner/paths/C4 C5.path index 0871575b..e75e886b 100644 --- a/src/main/deploy/pathplanner/paths/C4 C5.path +++ b/src/main/deploy/pathplanner/paths/C4 C5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.288642422749572, - "y": 2.436296745792339 + "x": 8.275153080797576, + "y": 2.6307895836632413 }, "prevControl": null, "nextControl": { - "x": 8.48629065679184, - "y": 1.9397105759497077 + "x": 8.472801314839845, + "y": 2.13420341382061 }, "isLocked": false, "linkedName": "C4" }, { "anchor": { - "x": 8.281967377058862, - "y": 0.7612043754757987 + "x": 8.277540743191514, + "y": 0.6118730249882861 }, "prevControl": { - "x": 8.5598341857378, - "y": 1.3008011682316762 + "x": 8.555407551870452, + "y": 1.1514698177441631 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C4 M.path b/src/main/deploy/pathplanner/paths/C4 M.path index c77d2409..1e39b500 100644 --- a/src/main/deploy/pathplanner/paths/C4 M.path +++ b/src/main/deploy/pathplanner/paths/C4 M.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.288642422749572, - "y": 2.436296745792339 + "x": 8.275153080797576, + "y": 2.6307895836632413 }, "prevControl": null, "nextControl": { - "x": 6.6890706681749315, - "y": 3.8012811523943313 + "x": 6.675581326222936, + "y": 3.9957739902652336 }, "isLocked": false, "linkedName": "C4" }, { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": { - "x": 4.400411368945171, - "y": 4.2735685023441725 + "x": 4.629415269483479, + "y": 4.297436052751029 }, "nextControl": null, "isLocked": false, @@ -29,7 +29,19 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "GrabVision", + "minWaypointRelativePos": 0.65, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + } + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.1, @@ -39,7 +51,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 172.18236916823508, + "rotation": 175.33314162856104, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C4 R.path b/src/main/deploy/pathplanner/paths/C4 R.path index e661bc21..ac9f605d 100644 --- a/src/main/deploy/pathplanner/paths/C4 R.path +++ b/src/main/deploy/pathplanner/paths/C4 R.path @@ -3,33 +3,51 @@ "waypoints": [ { "anchor": { - "x": 8.288642422749572, - "y": 2.436296745792339 + "x": 8.275153080797576, + "y": 2.6307895836632413 }, "prevControl": null, "nextControl": { - "x": 6.459247140218805, - "y": 0.551576467094703 + "x": 6.804623466884177, + "y": 1.1196489249876405 }, "isLocked": false, "linkedName": "C4" }, { "anchor": { - "x": 2.739170301035638, - "y": 3.1367081173878657 + "x": 3.0706451141860898, + "y": 3.2439017231814775 }, "prevControl": { - "x": 3.382676179312792, - "y": 2.1025022415852965 + "x": 3.6796086145667286, + "y": 1.7097821356840999 }, "nextControl": null, "isLocked": false, - "linkedName": "R" + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.75, + "rotationDegrees": 143.91, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "GrabVision", + "minWaypointRelativePos": 0.75, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + } } ], - "rotationTargets": [], - "constraintZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, @@ -39,7 +57,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 139.86766847541324, + "rotation": 143.91328920967564, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C5 C4.path b/src/main/deploy/pathplanner/paths/C5 C4.path index 80fa48a1..489a8e17 100644 --- a/src/main/deploy/pathplanner/paths/C5 C4.path +++ b/src/main/deploy/pathplanner/paths/C5 C4.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 8.281967377058862, - "y": 0.7612043754757987 + "x": 8.277540743191514, + "y": 0.6118730249882861 }, "prevControl": null, "nextControl": { - "x": 8.596605950210781, - "y": 1.2916082271134313 + "x": 8.592179316343433, + "y": 1.142276876625918 }, "isLocked": false, "linkedName": "C5" }, { "anchor": { - "x": 8.288642422749572, - "y": 2.436296745792339 + "x": 8.2, + "y": 2.6 }, "prevControl": { - "x": 8.415811441551963, - "y": 2.023213124440433 + "x": 8.32716901880239, + "y": 2.1869163786480943 }, "nextControl": null, "isLocked": false, - "linkedName": "C4" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/C5 R.path b/src/main/deploy/pathplanner/paths/C5 R.path index 5e381a06..d8d247e3 100644 --- a/src/main/deploy/pathplanner/paths/C5 R.path +++ b/src/main/deploy/pathplanner/paths/C5 R.path @@ -3,43 +3,61 @@ "waypoints": [ { "anchor": { - "x": 8.281967377058862, - "y": 0.7612043754757987 + "x": 8.277540743191514, + "y": 0.6118730249882861 }, "prevControl": null, "nextControl": { - "x": 6.261598906176536, - "y": 0.8963117590288929 + "x": 6.257172272309187, + "y": 0.7469804085413804 }, "isLocked": false, "linkedName": "C5" }, { "anchor": { - "x": 2.739170301035638, - "y": 3.1367081173878657 + "x": 3.4805, + "y": 3.061 }, "prevControl": { - "x": 3.8928844113753933, - "y": 2.461026945196853 + "x": 4.2885561417820925, + "y": 2.2248176881413615 }, "nextControl": null, "isLocked": false, "linkedName": "R" } ], - "rotationTargets": [], - "constraintZones": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.7, + "rotationDegrees": 144.48, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "GrabVision", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + } + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, - "maxAcceleration": 6.3, + "maxAcceleration": 6.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 }, "goalEndState": { "velocity": 0, - "rotation": 140.0146331538522, + "rotation": 144.48, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/CA1 W1.path b/src/main/deploy/pathplanner/paths/CA1 W1.path new file mode 100644 index 00000000..a7bec672 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CA1 W1.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0635131712488803, + "y": 6.792285196553275 + }, + "prevControl": null, + "nextControl": { + "x": 2.3914165945307624, + "y": 6.698598504187022 + }, + "isLocked": false, + "linkedName": "CA1" + }, + { + "anchor": { + "x": 2.883271729453586, + "y": 6.979658581285779 + }, + "prevControl": { + "x": 2.6373441619921745, + "y": 6.8039960330990565 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "W1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -155.409882833804, + "rotateFast": false + }, + "reversed": false, + "folder": "Wings", + "previewStartingState": { + "rotation": -150.61939213871082, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CA2 W2.path b/src/main/deploy/pathplanner/paths/CA2 W2.path new file mode 100644 index 00000000..3703eafd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CA2 W2.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.051802334703099, + "y": 5.572415049000463 + }, + "prevControl": null, + "nextControl": { + "x": 2.306749428448218, + "y": 5.56513084632203 + }, + "isLocked": false, + "linkedName": "CA2" + }, + { + "anchor": { + "x": 2.8726623557949864, + "y": 5.572415049000463 + }, + "prevControl": { + "x": 2.6543565144927745, + "y": 5.576946263544549 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "W2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -179.2747757009407, + "rotateFast": false + }, + "reversed": false, + "folder": "Wings", + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Calibration Spots.path b/src/main/deploy/pathplanner/paths/Calibration Spots.path deleted file mode 100644 index 696ea02b..00000000 --- a/src/main/deploy/pathplanner/paths/Calibration Spots.path +++ /dev/null @@ -1,148 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.92, - "y": 5.547755 - }, - "prevControl": null, - "nextControl": { - "x": 0.9894471748641465, - "y": 5.552536435416063 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.0652889390885163, - "y": 6.993912994918189 - }, - "prevControl": { - "x": 1.0169143949829513, - "y": 6.995232200075032 - }, - "nextControl": { - "x": 1.1136634831940824, - "y": 6.9925937897613455 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.29, - "y": 5.55 - }, - "prevControl": { - "x": 1.170409526669925, - "y": 5.54793996485694 - }, - "nextControl": { - "x": 1.392260483928399, - "y": 5.551761513143755 - }, - "isLocked": false, - "linkedName": "Sub" - }, - { - "anchor": { - "x": 3.12, - "y": 5.55 - }, - "prevControl": { - "x": 3.014261108140666, - "y": 5.55 - }, - "nextControl": { - "x": 3.225749245914595, - "y": 5.55 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.74, - "y": 0.37 - }, - "prevControl": { - "x": -0.2599999999999999, - "y": 0.3700000000000001 - }, - "nextControl": { - "x": 1.7399999999999998, - "y": 0.3699999999999999 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.19, - "y": 4.104648209292467 - }, - "prevControl": { - "x": 7.622999947947807, - "y": 2.239622339925795 - }, - "nextControl": { - "x": 8.757000052052199, - "y": 5.969674078659139 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.746794203555965, - "y": 4.104648209292467 - }, - "prevControl": { - "x": 5.474121291928236, - "y": 5.073932908448783 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 2.0, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 3.0, - "rotationDegrees": 0, - "rotateFast": false - }, - { - "waypointRelativePos": 5.0, - "rotationDegrees": 0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.5, - "maxAngularVelocity": 1137.21, - "maxAngularAcceleration": 1492.9 - }, - "goalEndState": { - "velocity": 0, - "rotation": 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/deploy/pathplanner/paths/L C1.path b/src/main/deploy/pathplanner/paths/L C1.path index aa642858..33f893ad 100644 --- a/src/main/deploy/pathplanner/paths/L C1.path +++ b/src/main/deploy/pathplanner/paths/L C1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.831849489661888, + "y": 6.499514282908736 }, "prevControl": null, "nextControl": { - "x": 4.6022730343333045, - "y": 6.4856199589218875 + "x": 5.061487326962089, + "y": 6.75715268691018 }, "isLocked": false, "linkedName": "L" }, { "anchor": { - "x": 8.26566006995396, - "y": 7.4370893646602525 + "x": 7.917787299441183, + "y": 7.552716871956289 }, "prevControl": { - "x": 7.243142356235671, - "y": 7.275218955439162 + "x": 6.563926730689648, + "y": 7.563719550413075 }, "nextControl": null, "isLocked": false, @@ -32,20 +32,20 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 }, "goalEndState": { "velocity": 0, - "rotation": -168.54675746234412, + "rotation": -179.92722381291108, "rotateFast": false }, "reversed": false, "folder": "Stage", "previewStartingState": { - "rotation": -174.16583293497462, + "rotation": -149.26451229791667, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/L C2.path b/src/main/deploy/pathplanner/paths/L C2.path index e7f17704..23b2e8c5 100644 --- a/src/main/deploy/pathplanner/paths/L C2.path +++ b/src/main/deploy/pathplanner/paths/L C2.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.831849489661888, + "y": 6.499514282908736 }, "prevControl": null, "nextControl": { - "x": 6.23248792596876, - "y": 7.146745641009012 + "x": 5.658739990803804, + "y": 6.651755158003897 }, "isLocked": false, "linkedName": "L" }, { "anchor": { - "x": 7.047595371786488, - "y": 5.77776349281702 + "x": 8.265148290642474, + "y": 6.0100254110329105 }, "prevControl": { - "x": 6.626048946393878, - "y": 5.801182738672167 - }, - "nextControl": { - "x": 7.295804781979105, - "y": 5.763974081139652 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.26566006995396, - "y": 5.77776349281702 - }, - "prevControl": { - "x": 7.544014192171723, - "y": 5.740991728344039 + "x": 6.987083716501011, + "y": 6.602966957933249 }, "nextControl": null, "isLocked": false, @@ -45,19 +29,7 @@ } ], "rotationTargets": [], - "constraintZones": [ - { - "name": "SpeedyIntake", - "minWaypointRelativePos": 1.2, - "maxWaypointRelativePos": 2.0, - "constraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 1137.21, - "maxAngularAcceleration": 1492.9 - } - } - ], + "constraintZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, @@ -67,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0, - "rotateFast": true + "rotation": 159.36579128418958, + "rotateFast": false }, "reversed": false, "folder": "Stage", "previewStartingState": { - "rotation": -165.35696772976468, + "rotation": -146.30993247402023, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/L C3.path b/src/main/deploy/pathplanner/paths/L C3.path index 3537c5ce..01a0b5ad 100644 --- a/src/main/deploy/pathplanner/paths/L C3.path +++ b/src/main/deploy/pathplanner/paths/L C3.path @@ -3,45 +3,45 @@ "waypoints": [ { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.831849489661888, + "y": 6.499514282908736 }, "prevControl": null, "nextControl": { - "x": 4.781420474375104, - "y": 4.703945890374583 + "x": 5.1311342303209635, + "y": 6.440922679234469 }, "isLocked": false, "linkedName": "L" }, { "anchor": { - "x": 6.293774200090394, - "y": 4.099779112962734 + "x": 7.003557980478892, + "y": 5.692610896875513 }, "prevControl": { - "x": 5.32391891211554, - "y": 4.109244679855541 + "x": 6.076481498135029, + "y": 6.546255578637684 }, "nextControl": { - "x": 6.763265771309702, - "y": 4.095196982073635 + "x": 7.715418788444165, + "y": 5.037135103402538 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.26566006995396, - "y": 4.095455268178173 + "x": 8.279435515585794, + "y": 4.123006159441842 }, "prevControl": { - "x": 7.570775138202772, - "y": 4.081938482275368 + "x": 8.26477605769545, + "y": 4.1267393133145385 }, "nextControl": null, "isLocked": false, - "linkedName": "C3" + "linkedName": null } ], "rotationTargets": [], @@ -55,13 +55,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -179.0899839495599, + "rotation": 108.29014687761794, "rotateFast": false }, "reversed": false, "folder": "Stage", "previewStartingState": { - "rotation": -173.86919389160926, + "rotation": -149.63709882658625, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/M C2.path b/src/main/deploy/pathplanner/paths/M C2.path index 62aed37f..6985bc6f 100644 --- a/src/main/deploy/pathplanner/paths/M C2.path +++ b/src/main/deploy/pathplanner/paths/M C2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": null, "nextControl": { - "x": 5.066516560804678, - "y": 7.345159953477801 + "x": 4.663318884412376, + "y": 7.483224552754384 }, "isLocked": false, "linkedName": "M" }, { "anchor": { - "x": 8.26566006995396, - "y": 5.77776349281702 + "x": 8.265148290642474, + "y": 6.0100254110329105 }, "prevControl": { - "x": 7.144121253528062, - "y": 6.292568195438743 + "x": 6.88656517455336, + "y": 6.58831148685622 }, "nextControl": null, "isLocked": false, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/M C3.path b/src/main/deploy/pathplanner/paths/M C3.path index 96c2f66f..28ad1320 100644 --- a/src/main/deploy/pathplanner/paths/M C3.path +++ b/src/main/deploy/pathplanner/paths/M C3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": null, "nextControl": { - "x": 4.965394208503983, - "y": 3.8104740935125765 + "x": 5.167762965936587, + "y": 3.902710757696766 }, "isLocked": false, "linkedName": "M" @@ -20,8 +20,8 @@ "y": 4.095455268178173 }, "prevControl": { - "x": 7.686504779504522, - "y": 4.068149070598 + "x": 6.3243138250982405, + "y": 4.1046482092964185 }, "nextControl": null, "isLocked": false, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 @@ -40,7 +40,7 @@ "goalEndState": { "velocity": 0, "rotation": 179.96700654552058, - "rotateFast": false + "rotateFast": true }, "reversed": false, "folder": "Stage", diff --git a/src/main/deploy/pathplanner/paths/M C4.path b/src/main/deploy/pathplanner/paths/M C4.path index 500e0646..84641a6e 100644 --- a/src/main/deploy/pathplanner/paths/M C4.path +++ b/src/main/deploy/pathplanner/paths/M C4.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": null, "nextControl": { - "x": 5.664057733490608, - "y": 4.348261148929913 + "x": 5.553342461891771, + "y": 3.7708893677416455 }, "isLocked": false, "linkedName": "M" }, { "anchor": { - "x": 8.288642422749572, - "y": 2.436296745792339 + "x": 8.275153080797576, + "y": 2.6307895836632413 }, "prevControl": { - "x": 7.300401252538228, - "y": 2.7624788060326404 + "x": 7.249635255623875, + "y": 3.6960329650595725 }, "nextControl": null, "isLocked": false, @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 }, "goalEndState": { "velocity": 0, - "rotation": 156.61913319105417, + "rotation": 130.8620125526686, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/M W3.path b/src/main/deploy/pathplanner/paths/M W3.path index bfc6fea8..f3503706 100644 --- a/src/main/deploy/pathplanner/paths/M W3.path +++ b/src/main/deploy/pathplanner/paths/M W3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": null, "nextControl": { - "x": 2.5614401060829, - "y": 4.904434086583739 + "x": 2.790444006621207, + "y": 4.928301636990595 }, "isLocked": false, "linkedName": "M" }, { "anchor": { - "x": 2.506282459373429, - "y": 4.359369286114458 + "x": 2.370694368507382, + "y": 4.369015382098561 }, "prevControl": { - "x": 1.5869883475489233, - "y": 4.658139872457423 + "x": 1.4514002566828759, + "y": 4.6677859684415255 }, "nextControl": null, "isLocked": false, @@ -56,7 +56,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/P A.path b/src/main/deploy/pathplanner/paths/P A.path index 719d6bdf..77140e47 100644 --- a/src/main/deploy/pathplanner/paths/P A.path +++ b/src/main/deploy/pathplanner/paths/P A.path @@ -73,13 +73,13 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 }, "goalEndState": { - "velocity": 0, + "velocity": 0.0, "rotation": -90.0, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/P CA1.path b/src/main/deploy/pathplanner/paths/P CA1.path new file mode 100644 index 00000000..a37d1ed4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/P CA1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3140196323188638, + "y": 7.14361029292672 + }, + "prevControl": null, + "nextControl": { + "x": 1.712188074875435, + "y": 6.979658581285779 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0635131712488803, + "y": 6.792285196553275 + }, + "prevControl": { + "x": 1.6536338921465277, + "y": 6.98093669388128 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CA1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -145.56101069119634, + "rotateFast": false + }, + "reversed": false, + "folder": "Preloads", + "previewStartingState": { + "rotation": -124.99202019855865, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P CA2.path b/src/main/deploy/pathplanner/paths/P CA2.path new file mode 100644 index 00000000..11f7bcce --- /dev/null +++ b/src/main/deploy/pathplanner/paths/P CA2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3575481986811355, + "y": 5.572415049000463 + }, + "prevControl": null, + "nextControl": { + "x": 2.0367568870730968, + "y": 5.5609866699959385 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.051802334703099, + "y": 5.572415049000463 + }, + "prevControl": { + "x": 1.753184238887901, + "y": 5.564105245857552 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CA2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -179.29528514946807, + "rotateFast": false + }, + "reversed": false, + "folder": "Preloads", + "previewStartingState": { + "rotation": 180.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P R.path b/src/main/deploy/pathplanner/paths/P R.path index b96f790b..3f6da691 100644 --- a/src/main/deploy/pathplanner/paths/P R.path +++ b/src/main/deploy/pathplanner/paths/P R.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.739170301035638, - "y": 3.1367081173878657 + "x": 3.4805, + "y": 3.061 }, "prevControl": { - "x": 2.1692079517044442, - "y": 3.1137257645922536 + "x": 2.9105380266769925, + "y": 3.0380083240954763 }, "nextControl": null, "isLocked": false, @@ -32,14 +32,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 }, "goalEndState": { - "velocity": 0, - "rotation": 141.88331200332212, + "velocity": 0.0, + "rotation": 146.31, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/R C3.path b/src/main/deploy/pathplanner/paths/R C3.path index 4216229b..3211c50b 100644 --- a/src/main/deploy/pathplanner/paths/R C3.path +++ b/src/main/deploy/pathplanner/paths/R C3.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 2.739170301035638, - "y": 3.1367081173878657 + "x": 3.4805, + "y": 3.061 }, "prevControl": null, "nextControl": { - "x": 3.7044291184513694, - "y": 2.929866942227352 + "x": 4.846474445103242, + "y": 3.3333649108130463 }, "isLocked": false, "linkedName": "R" }, { "anchor": { - "x": 4.726377739429611, - "y": 3.6358082122659208 + "x": 5.782754295808259, + "y": 4.095455268178173 }, "prevControl": { - "x": 4.207493325080779, - "y": 3.336214234933559 + "x": 5.1546629358152325, + "y": 4.084222285730406 }, "nextControl": { - "x": 5.498584793362196, - "y": 4.081665856500806 + "x": 6.296776899880105, + "y": 4.104648209296419 }, "isLocked": false, "linkedName": null @@ -36,19 +36,25 @@ "y": 4.095455268178173 }, "prevControl": { - "x": 6.188055377230576, - "y": 4.178464364016941 + "x": 6.250882024516547, + "y": 4.1046482092964185 }, "nextControl": null, "isLocked": false, "linkedName": "C3" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": -179.8337009350905, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 @@ -56,7 +62,7 @@ "goalEndState": { "velocity": 0, "rotation": 179.12721881667025, - "rotateFast": false + "rotateFast": true }, "reversed": false, "folder": "Stage", diff --git a/src/main/deploy/pathplanner/paths/R C4.path b/src/main/deploy/pathplanner/paths/R C4.path index 9a757e65..24053ad7 100644 --- a/src/main/deploy/pathplanner/paths/R C4.path +++ b/src/main/deploy/pathplanner/paths/R C4.path @@ -3,65 +3,71 @@ "waypoints": [ { "anchor": { - "x": 2.739170301035638, - "y": 3.1367081173878657 + "x": 3.4805, + "y": 3.061 }, "prevControl": null, "nextControl": { - "x": 3.860709117461535, - "y": 2.4840092979924666 + "x": 3.960644298377043, + "y": 2.3653395898722596 }, "isLocked": false, "linkedName": "R" }, { "anchor": { - "x": 5.645671851254118, - "y": 1.5627999901016596 + "x": 5.904667558265215, + "y": 1.6043846067720653 }, "prevControl": { - "x": 4.971234434826018, - "y": 1.5665677968973468 + "x": 5.042107625440227, + "y": 1.5810721761551738 }, "nextControl": { - "x": 6.468440081337051, - "y": 1.5582035195425377 + "x": 6.7712694626530485, + "y": 1.6278062798636284 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.288642422749572, - "y": 2.436296745792339 + "x": 8.275153080797576, + "y": 2.6307895836632413 }, "prevControl": { - "x": 7.401523604838924, - "y": 2.4499188080123084 + "x": 7.687568246658104, + "y": 1.945273943833855 }, "nextControl": null, "isLocked": false, "linkedName": "C4" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.8, + "rotationDegrees": -164.89810826889706, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 }, "goalEndState": { "velocity": 0, - "rotation": -169.22190068678276, + "rotation": -139.63546342690256, "rotateFast": false }, "reversed": false, "folder": "Stage", "previewStartingState": { - "rotation": 147.60140166316665, + "rotation": 146.65929265352307, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/R C5 N.path b/src/main/deploy/pathplanner/paths/R C5 N.path index 7bb21a9a..0e08878f 100644 --- a/src/main/deploy/pathplanner/paths/R C5 N.path +++ b/src/main/deploy/pathplanner/paths/R C5 N.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.739170301035638, - "y": 3.1367081173878657 + "x": 3.4805, + "y": 3.061 }, "prevControl": null, "nextControl": { - "x": 3.3367114737215666, - "y": 2.768990472658063 + "x": 4.07804117268593, + "y": 2.693282355270197 }, "isLocked": false, "linkedName": "R" @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/R C5.path b/src/main/deploy/pathplanner/paths/R C5.path index 06016c05..aa5c4f39 100644 --- a/src/main/deploy/pathplanner/paths/R C5.path +++ b/src/main/deploy/pathplanner/paths/R C5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.739170301035638, - "y": 3.1367081173878657 + "x": 3.4805, + "y": 3.061 }, "prevControl": null, "nextControl": { - "x": 4.223830291632215, - "y": 1.8541254819352813 + "x": 5.250383821813703, + "y": 1.7149071807946856 }, "isLocked": false, "linkedName": "R" }, { "anchor": { - "x": 8.281967377058862, - "y": 0.7612043754757987 + "x": 8.277540743191514, + "y": 0.6118730249882861 }, "prevControl": { - "x": 7.640540073913295, - "y": 1.512238813951312 + "x": 6.154867970436146, + "y": 1.1553604017531358 }, "nextControl": null, "isLocked": false, @@ -32,20 +32,20 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 }, "goalEndState": { "velocity": 0, - "rotation": 147.21190301355887, - "rotateFast": true + "rotation": 170.62702561571825, + "rotateFast": false }, "reversed": false, "folder": "Stage", "previewStartingState": { - "rotation": 140.2190475453689, + "rotation": 145.31792694138463, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/R W3.path b/src/main/deploy/pathplanner/paths/R W3.path index 355cdf9d..6cd48b25 100644 --- a/src/main/deploy/pathplanner/paths/R W3.path +++ b/src/main/deploy/pathplanner/paths/R W3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.739170301035638, - "y": 3.1367081173878657 + "x": 3.4805, + "y": 3.061 }, "prevControl": null, "nextControl": { - "x": 1.7187538369095925, - "y": 3.426285762614044 + "x": 2.460083535873955, + "y": 3.350577645226178 }, "isLocked": false, "linkedName": "R" }, { "anchor": { - "x": 2.506282459373429, - "y": 4.359369286114458 + "x": 2.370694368507382, + "y": 4.369015382098561 }, "prevControl": { - "x": 1.0471860497054433, - "y": 4.218364170894443 + "x": 0.9115979588393959, + "y": 4.228010266878545 }, "nextControl": null, "isLocked": false, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S C1 N Over W1.path b/src/main/deploy/pathplanner/paths/S C1 N Over W1.path index 04ba4326..68026dd4 100644 --- a/src/main/deploy/pathplanner/paths/S C1 N Over W1.path +++ b/src/main/deploy/pathplanner/paths/S C1 N Over W1.path @@ -54,7 +54,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S C1.path b/src/main/deploy/pathplanner/paths/S C1.path index 1f3c6fde..84dc546d 100644 --- a/src/main/deploy/pathplanner/paths/S C1.path +++ b/src/main/deploy/pathplanner/paths/S C1.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 2.4970895182551844, - "y": 7.754245833239707 + "x": 2.465111215936718, + "y": 7.666896476650097 }, "prevControl": { - "x": 1.8857589338918883, - "y": 7.7772281860353205 + "x": 1.8539138583638446, + "y": 7.640608418259865 }, "nextControl": { - "x": 3.8555200573576824, - "y": 7.703177015980211 + "x": 3.828440799738743, + "y": 7.725534308211474 }, "isLocked": false, "linkedName": null @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 8.26566006995396, - "y": 7.4370893646602525 + "x": 7.917787299441183, + "y": 7.552716871956289 }, "prevControl": { - "x": 7.208471841355778, - "y": 7.161301131112902 + "x": 6.860599070843001, + "y": 7.276928638408938 }, "nextControl": null, "isLocked": false, @@ -87,7 +87,7 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S C2 N Thru Stage.path b/src/main/deploy/pathplanner/paths/S C2 N Thru Stage.path index 5b97406e..4f740002 100644 --- a/src/main/deploy/pathplanner/paths/S C2 N Thru Stage.path +++ b/src/main/deploy/pathplanner/paths/S C2 N Thru Stage.path @@ -48,7 +48,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S C2 N.path b/src/main/deploy/pathplanner/paths/S C2 N.path index 30ac3564..267cbff4 100644 --- a/src/main/deploy/pathplanner/paths/S C2 N.path +++ b/src/main/deploy/pathplanner/paths/S C2 N.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S C2.path b/src/main/deploy/pathplanner/paths/S C2.path index c42c3b27..b277e0ab 100644 --- a/src/main/deploy/pathplanner/paths/S C2.path +++ b/src/main/deploy/pathplanner/paths/S C2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 1.5134448186008644, + "x": 1.3216735004898588, "y": 6.301761136557157 }, "prevControl": null, "nextControl": { - "x": 2.5134448186008536, + "x": 2.3216735004898483, "y": 6.301761136557157 }, "isLocked": false, @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 8.26566006995396, - "y": 5.77776349281702 + "x": 8.265148290642474, + "y": 6.0100254110329105 }, "prevControl": { - "x": 7.32338360533384, - "y": 6.329339959911723 + "x": 7.322871826022354, + "y": 6.561601878127613 }, "nextControl": null, "isLocked": false, @@ -57,44 +57,9 @@ } ], "constraintZones": [], - "eventMarkers": [ - { - "name": "Shoot", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Shoot" - } - } - ] - } - } - }, - { - "name": "ToIndexer", - "waypointRelativePos": 1.75, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ToIndexer" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 @@ -107,7 +72,7 @@ "reversed": false, "folder": "Preloads", "previewStartingState": { - "rotation": -158.8242605828954, + "rotation": -149.46361927097314, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S C3 N Over Stage.path b/src/main/deploy/pathplanner/paths/S C3 N Over Stage.path index cdb7591a..246a7c38 100644 --- a/src/main/deploy/pathplanner/paths/S C3 N Over Stage.path +++ b/src/main/deploy/pathplanner/paths/S C3 N Over Stage.path @@ -48,7 +48,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S C3 N Thru Stage.path b/src/main/deploy/pathplanner/paths/S C3 N Thru Stage.path index 522f1897..89d52ff1 100644 --- a/src/main/deploy/pathplanner/paths/S C3 N Thru Stage.path +++ b/src/main/deploy/pathplanner/paths/S C3 N Thru Stage.path @@ -48,7 +48,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S C3 N Under Stage.path b/src/main/deploy/pathplanner/paths/S C3 N Under Stage.path index 2f12218d..86b8851d 100644 --- a/src/main/deploy/pathplanner/paths/S C3 N Under Stage.path +++ b/src/main/deploy/pathplanner/paths/S C3 N Under Stage.path @@ -48,7 +48,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S C3.path b/src/main/deploy/pathplanner/paths/S C3.path index 1bc70b62..dbaa82dd 100644 --- a/src/main/deploy/pathplanner/paths/S C3.path +++ b/src/main/deploy/pathplanner/paths/S C3.path @@ -105,7 +105,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S C4 N.path b/src/main/deploy/pathplanner/paths/S C4 N.path index 64ed7e56..2de24f58 100644 --- a/src/main/deploy/pathplanner/paths/S C4 N.path +++ b/src/main/deploy/pathplanner/paths/S C4 N.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S C4.path b/src/main/deploy/pathplanner/paths/S C4.path index 27da0ba3..30ffab70 100644 --- a/src/main/deploy/pathplanner/paths/S C4.path +++ b/src/main/deploy/pathplanner/paths/S C4.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 8.288642422749572, - "y": 2.436296745792339 + "x": 8.275153080797576, + "y": 2.6307895836632413 }, "prevControl": { - "x": 6.426619411970311, - "y": 3.9001513140150275 + "x": 6.413130070018315, + "y": 4.09464415188593 }, "nextControl": null, "isLocked": false, @@ -83,7 +83,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S C5 2.path b/src/main/deploy/pathplanner/paths/S C5 2.path new file mode 100644 index 00000000..dfa4543a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S C5 2.path @@ -0,0 +1,91 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.31804312991342, + "y": 3.844752380862786 + }, + "prevControl": null, + "nextControl": { + "x": 1.9028087438890928, + "y": 2.3317873796241413 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.5481769975885635, + "y": 1.310768053634873 + }, + "prevControl": { + "x": 2.928897464735801, + "y": 1.7167031431379065 + }, + "nextControl": { + "x": 6.216695810550042, + "y": 0.8924892327547225 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.277540743191514, + "y": 0.6118730249882861 + }, + "prevControl": { + "x": 7.885268157867914, + "y": 1.931842194575626 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "C5" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": 138.43334613355205, + "rotateFast": false + }, + { + "waypointRelativePos": 1.6, + "rotationDegrees": 152.28026956324393, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.2, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 97.94362415836568, + "rotateFast": false + }, + "reversed": false, + "folder": "Preloads", + "previewStartingState": { + "rotation": 119.7448812969422, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/S C5 N.path b/src/main/deploy/pathplanner/paths/S C5 N.path index 2d6f2351..6805a924 100644 --- a/src/main/deploy/pathplanner/paths/S C5 N.path +++ b/src/main/deploy/pathplanner/paths/S C5 N.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S C5.path b/src/main/deploy/pathplanner/paths/S C5.path index 70958c26..aecfcfd8 100644 --- a/src/main/deploy/pathplanner/paths/S C5.path +++ b/src/main/deploy/pathplanner/paths/S C5.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 8.281967377058862, - "y": 0.7612043754757987 + "x": 8.277540743191514, + "y": 0.6118730249882861 }, "prevControl": { - "x": 7.6144496939493145, - "y": 1.346746202764876 + "x": 7.610023060081966, + "y": 1.197414852277364 }, "nextControl": null, "isLocked": false, @@ -54,7 +54,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S W1.path b/src/main/deploy/pathplanner/paths/S W1.path index 2e8eee34..1242bdc3 100644 --- a/src/main/deploy/pathplanner/paths/S W1.path +++ b/src/main/deploy/pathplanner/paths/S W1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": { - "x": 2.2185338517641693, - "y": 6.762260696421776 + "x": 2.2094195948780335, + "y": 6.759880498400434 }, "nextControl": null, "isLocked": false, @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S W2.path b/src/main/deploy/pathplanner/paths/S W2.path index cb1614bb..2c580098 100644 --- a/src/main/deploy/pathplanner/paths/S W2.path +++ b/src/main/deploy/pathplanner/paths/S W2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.3793978662827602, - "y": 5.631181847982673 + "x": 1.3642439036290506, + "y": 5.572415049000463 }, "prevControl": null, "nextControl": { - "x": 1.8146745263093393, - "y": 5.612789876434728 + "x": 1.7995205636556297, + "y": 5.554023077452517 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.901578927457967, - "y": 5.538747023742648 + "x": 2.8726623557949864, + "y": 5.572415049000463 }, "prevControl": { - "x": 2.4675864716928726, - "y": 5.545352647416459 + "x": 2.2454678129695247, + "y": 5.570262711146891 }, "nextControl": null, "isLocked": false, @@ -50,20 +50,20 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 }, "goalEndState": { "velocity": 0, - "rotation": 179.31337875990656, + "rotation": -179.21039048519975, "rotateFast": false }, "reversed": false, "folder": "Preloads", "previewStartingState": { - "rotation": -179.1656544244532, + "rotation": -178.888482285124, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/S W3 2.path b/src/main/deploy/pathplanner/paths/S W3 2.path index ac7807bd..05a61bca 100644 --- a/src/main/deploy/pathplanner/paths/S W3 2.path +++ b/src/main/deploy/pathplanner/paths/S W3 2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.506282459373429, - "y": 4.359369286114458 + "x": 2.370694368507382, + "y": 4.369015382098561 }, "prevControl": { - "x": 2.062340011204845, - "y": 4.887407243833441 + "x": 1.9267519203387975, + "y": 4.8970533398175435 }, "nextControl": null, "isLocked": false, @@ -56,7 +56,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/S W3 New.path b/src/main/deploy/pathplanner/paths/S W3 New.path new file mode 100644 index 00000000..79b75b87 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S W3 New.path @@ -0,0 +1,92 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.6743212881722518, + "y": 4.43559408955324 + }, + "prevControl": null, + "nextControl": { + "x": 1.9822848156334612, + "y": 4.178191738242379 + }, + "isLocked": false, + "linkedName": "8044 P End" + }, + { + "anchor": { + "x": 2.428325992410636, + "y": 4.23940595884741 + }, + "prevControl": { + "x": 2.2666705412135384, + "y": 3.934056773252893 + }, + "nextControl": { + "x": 2.5524306975069444, + "y": 4.473825957362659 + }, + "isLocked": false, + "linkedName": "W3 Pickup" + }, + { + "anchor": { + "x": 2.7223165756521888, + "y": 4.778983272252259 + }, + "prevControl": { + "x": 2.5430542238464104, + "y": 4.486155265703589 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Houston?" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.15, + "rotationDegrees": 156.0780441292195, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.2, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + }, + "goalEndState": { + "velocity": 0, + "rotation": 164.04118314714827, + "rotateFast": false + }, + "reversed": false, + "folder": "Preloads", + "previewStartingState": { + "rotation": 153.56913073103783, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/S W3-1 2.path b/src/main/deploy/pathplanner/paths/S W3-1 2.path index 6eaeb888..9a213028 100644 --- a/src/main/deploy/pathplanner/paths/S W3-1 2.path +++ b/src/main/deploy/pathplanner/paths/S W3-1 2.path @@ -3,57 +3,57 @@ "waypoints": [ { "anchor": { - "x": 1.29, + "x": 1.3272877609585005, "y": 5.55 }, "prevControl": null, "nextControl": { - "x": 1.6949490562586944, - "y": 5.018155524376958 + "x": 1.6766195234518129, + "y": 5.127890786987223 }, "isLocked": false, - "linkedName": "Sub" + "linkedName": null }, { "anchor": { - "x": 2.5982118705558803, - "y": 4.421804677875873 + "x": 2.484449224217597, + "y": 4.359369286114458 }, "prevControl": { - "x": 2.5438107087286053, - "y": 4.008355847988578 + "x": 2.3680053033881596, + "y": 4.250203110333227 }, "nextControl": { - "x": 2.621194223351493, - "y": 4.59647055912253 + "x": 2.8828287256619016, + "y": 4.732850068730926 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.377581283717999, - "y": 5.285941142990909 + "x": 2.35, + "y": 5.35 }, "prevControl": { - "x": 1.518041289162086, - "y": 5.1526434967763555 + "x": 1.4549929876325924, + "y": 5.205127154345743 }, "nextControl": { - "x": 3.046269152359308, - "y": 5.389641400694642 + "x": 3.154001768996446, + "y": 5.480142024113835 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": { - "x": 1.4291761916786625, - "y": 5.8294737866069 + "x": 1.2239458576103528, + "y": 6.048107214636947 }, "nextControl": null, "isLocked": false, @@ -63,25 +63,25 @@ "rotationTargets": [ { "waypointRelativePos": 0.7, - "rotationDegrees": 160.04500243607646, + "rotationDegrees": 154.4860899706085, "rotateFast": false }, { - "waypointRelativePos": 1.7999999999999998, + "waypointRelativePos": 1.85, "rotationDegrees": -177.54423533374379, "rotateFast": false }, { - "waypointRelativePos": 2.55, - "rotationDegrees": -159.93962834871448, + "waypointRelativePos": 2.7, + "rotationDegrees": -160.30395935169378, "rotateFast": false } ], "constraintZones": [ { "name": "Slow w2", - "minWaypointRelativePos": 2.3000000000000003, - "maxWaypointRelativePos": 2.65, + "minWaypointRelativePos": 2.05, + "maxWaypointRelativePos": 2.7, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 4.2, @@ -89,17 +89,6 @@ "maxAngularAcceleration": 1492.9 } }, - { - "name": "Slow w3", - "minWaypointRelativePos": 0.0, - "maxWaypointRelativePos": 0.95, - "constraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 1137.21, - "maxAngularAcceleration": 1492.9 - } - }, { "name": "Capped accel", "minWaypointRelativePos": 1.0999999999999999, @@ -112,25 +101,7 @@ } } ], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 4.2, @@ -139,13 +110,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -151.62646231180472, + "rotation": -149.36, "rotateFast": false }, "reversed": false, "folder": "Preloads", "previewStartingState": { - "rotation": -179.9080895145809, + "rotation": -179.15747573949946, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/S W3-1 Bayou.path b/src/main/deploy/pathplanner/paths/S W3-1 Bayou.path new file mode 100644 index 00000000..c77c573d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S W3-1 Bayou.path @@ -0,0 +1,163 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.29235458470917, + "y": 4.778983272252259 + }, + "prevControl": null, + "nextControl": { + "x": 1.6973036409678643, + "y": 4.247138796629217 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.428325992410636, + "y": 4.23940595884741 + }, + "prevControl": { + "x": 2.180976262035649, + "y": 3.997876222128304 + }, + "nextControl": { + "x": 2.819025989936051, + "y": 4.6209130152545805 + }, + "isLocked": false, + "linkedName": "W3 Pickup" + }, + { + "anchor": { + "x": 2.049547453132173, + "y": 5.237341725792591 + }, + "prevControl": { + "x": 1.2243689644069953, + "y": 4.9622822295508655 + }, + "nextControl": { + "x": 3.2514408950734497, + "y": 5.637972873106349 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.883271729453586, + "y": 6.979658581285779 + }, + "prevControl": { + "x": 1.2331387987285984, + "y": 6.023592704988293 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "W1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7, + "rotationDegrees": 158.25924699388764, + "rotateFast": false + }, + { + "waypointRelativePos": 1.7999999999999998, + "rotationDegrees": 169.22011509237518, + "rotateFast": false + }, + { + "waypointRelativePos": 2.55, + "rotationDegrees": -154.92639793146472, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "Slow w2 shot", + "minWaypointRelativePos": 2.3000000000000003, + "maxWaypointRelativePos": 2.65, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + } + }, + { + "name": "Slow w3 lineup", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.5499999999999999, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 2.7, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + } + }, + { + "name": "Slow w2 lineup", + "minWaypointRelativePos": 1.2000000000000002, + "maxWaypointRelativePos": 1.75, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + } + }, + { + "name": "Capped accel", + "minWaypointRelativePos": 1.0999999999999999, + "maxWaypointRelativePos": 2.8499999999999996, + "constraints": { + "maxVelocity": 4.5, + "maxAcceleration": 2.87, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + } + } + ], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.2, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + }, + "goalEndState": { + "velocity": 0, + "rotation": -154.19161832495357, + "rotateFast": false + }, + "reversed": false, + "folder": "Preloads", + "previewStartingState": { + "rotation": 153.56913073103783, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/S W3-1.path b/src/main/deploy/pathplanner/paths/S W3-1.path index 728f16b2..416fff9c 100644 --- a/src/main/deploy/pathplanner/paths/S W3-1.path +++ b/src/main/deploy/pathplanner/paths/S W3-1.path @@ -8,52 +8,52 @@ }, "prevControl": null, "nextControl": { - "x": 1.6973036409678643, - "y": 4.247138796629217 + "x": 1.7930634442845832, + "y": 4.534035167359043 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.4787036360186945, - "y": 4.168998797124134 + "x": 2.484449224217597, + "y": 4.359369286114458 }, "prevControl": { - "x": 2.2313539056437075, - "y": 3.9274690604050284 + "x": 2.3680053033881596, + "y": 4.250203110333227 }, "nextControl": { - "x": 2.8694036335441093, - "y": 4.5505058535313045 + "x": 2.8828287256619016, + "y": 4.732850068730926 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.377581283717999, - "y": 5.285941142990909 + "x": 2.484449224217597, + "y": 5.298198397813163 }, "prevControl": { - "x": 1.518041289162086, - "y": 5.1526434967763555 + "x": 1.5820088377782069, + "y": 5.210865457189835 }, "nextControl": { - "x": 3.046269152359308, - "y": 5.389641400694642 + "x": 3.040722540967769, + "y": 5.3520312994342465 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": { - "x": 1.2422530556147342, - "y": 6.025972903009635 + "x": 1.2239458576103528, + "y": 6.048107214636947 }, "nextControl": null, "isLocked": false, @@ -63,39 +63,39 @@ "rotationTargets": [ { "waypointRelativePos": 0.7, - "rotationDegrees": 179.78434028883294, + "rotationDegrees": 147.9046762148673, "rotateFast": false }, { - "waypointRelativePos": 1.7999999999999998, + "waypointRelativePos": 1.85, "rotationDegrees": -177.54423533374379, "rotateFast": false }, { - "waypointRelativePos": 2.55, - "rotationDegrees": -159.93962834871448, + "waypointRelativePos": 2.7, + "rotationDegrees": -160.30395935169378, "rotateFast": false } ], "constraintZones": [ { - "name": "Slow w2", - "minWaypointRelativePos": 2.3000000000000003, - "maxWaypointRelativePos": 2.65, + "name": "Slow Preload", + "minWaypointRelativePos": 0.44999999999999996, + "maxWaypointRelativePos": 0.5499999999999999, "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 4.2, + "maxVelocity": 0.25, + "maxAcceleration": 4.0, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 } }, { - "name": "Slow w3", - "minWaypointRelativePos": 0, - "maxWaypointRelativePos": 0.5499999999999999, + "name": "Slow w2", + "minWaypointRelativePos": 2.05, + "maxWaypointRelativePos": 2.7, "constraints": { - "maxVelocity": 5.0, - "maxAcceleration": 3.0, + "maxVelocity": 1.0, + "maxAcceleration": 4.2, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 } @@ -112,25 +112,7 @@ } } ], - "eventMarkers": [ - { - "name": "Intake", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Intake" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, "maxAcceleration": 4.2, @@ -139,13 +121,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -149.36485747779972, + "rotation": -151.7935229185301, "rotateFast": false }, "reversed": false, "folder": "Preloads", "previewStartingState": { - "rotation": 153.56913073103783, + "rotation": 146.4872454687277, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/S W3.path b/src/main/deploy/pathplanner/paths/S W3.path index 6210e732..f2188f8c 100644 --- a/src/main/deploy/pathplanner/paths/S W3.path +++ b/src/main/deploy/pathplanner/paths/S W3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.506282459373429, - "y": 4.359369286114458 + "x": 2.370694368507382, + "y": 4.369015382098561 }, "prevControl": { - "x": 2.055062266152901, - "y": 4.512201932205282 + "x": 1.9194741752868536, + "y": 4.5218480281893845 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/S2 to W3 Prep.path b/src/main/deploy/pathplanner/paths/S2 to W3 Prep.path index 5d5491c3..ec060f15 100644 --- a/src/main/deploy/pathplanner/paths/S2 to W3 Prep.path +++ b/src/main/deploy/pathplanner/paths/S2 to W3 Prep.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/Shot Spots.path b/src/main/deploy/pathplanner/paths/Shot Spots.path index 87b1d1aa..ee0f717e 100644 --- a/src/main/deploy/pathplanner/paths/Shot Spots.path +++ b/src/main/deploy/pathplanner/paths/Shot Spots.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.831849489661888, + "y": 6.499514282908736 }, "prevControl": null, "nextControl": { - "x": 4.774908333691249, - "y": 5.915657609590695 + "x": 4.83184948966189, + "y": 6.499514282908736 }, "isLocked": false, "linkedName": "L" }, { "anchor": { - "x": 3.954170685497026, - "y": 5.046924673916537 + "x": 4.183174586035333, + "y": 5.070792224323394 }, "prevControl": { - "x": 2.954170685497026, - "y": 5.046924673916537 + "x": 3.1831745860353333, + "y": 5.070792224323394 }, "nextControl": null, "isLocked": false, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/W1 A.path b/src/main/deploy/pathplanner/paths/W1 A.path index 5f072084..295574c7 100644 --- a/src/main/deploy/pathplanner/paths/W1 A.path +++ b/src/main/deploy/pathplanner/paths/W1 A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": null, "nextControl": { - "x": 2.448666263038041, - "y": 7.238220481000093 + "x": 2.439552006151905, + "y": 7.235840282978751 }, "isLocked": false, "linkedName": "W1" @@ -84,7 +84,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/W1 C1 N.path b/src/main/deploy/pathplanner/paths/W1 C1 N.path index 30209f06..fd830499 100644 --- a/src/main/deploy/pathplanner/paths/W1 C1 N.path +++ b/src/main/deploy/pathplanner/paths/W1 C1 N.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": null, "nextControl": { - "x": 3.862241274314576, - "y": 6.756811721910118 + "x": 3.85312701742844, + "y": 6.754431523888776 }, "isLocked": false, "linkedName": "W1" @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/W1 C1.path b/src/main/deploy/pathplanner/paths/W1 C1.path index ce1383d2..7ed54706 100644 --- a/src/main/deploy/pathplanner/paths/W1 C1.path +++ b/src/main/deploy/pathplanner/paths/W1 C1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": null, "nextControl": { - "x": 6.151283612757595, - "y": 7.317581130123067 + "x": 5.621228246600958, + "y": 8.144293944005295 }, "isLocked": false, "linkedName": "W1" }, { "anchor": { - "x": 8.26566006995396, - "y": 7.4370893646602525 + "x": 7.917787299441183, + "y": 7.552716871956289 }, "prevControl": { - "x": 6.987276791032128, - "y": 7.470174532900553 + "x": 6.933501042845781, + "y": 6.861400389467445 }, "nextControl": null, "isLocked": false, @@ -38,14 +38,14 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 }, "goalEndState": { "velocity": 0, - "rotation": 179.91283827567477, + "rotation": -157.58884995809322, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/W1 C2 N.path b/src/main/deploy/pathplanner/paths/W1 C2 N.path index 2940a955..ec68a14e 100644 --- a/src/main/deploy/pathplanner/paths/W1 C2 N.path +++ b/src/main/deploy/pathplanner/paths/W1 C2 N.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": null, "nextControl": { - "x": 3.9449777443787815, - "y": 7.211862307263249 + "x": 3.935863487492646, + "y": 7.209482109241907 }, "isLocked": false, "linkedName": "W1" diff --git a/src/main/deploy/pathplanner/paths/W1 C2.path b/src/main/deploy/pathplanner/paths/W1 C2.path index 172fe8bf..7d61fae1 100644 --- a/src/main/deploy/pathplanner/paths/W1 C2.path +++ b/src/main/deploy/pathplanner/paths/W1 C2.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": null, "nextControl": { - "x": 6.119108318843737, - "y": 6.913091720920284 + "x": 6.1099940619576, + "y": 6.910711522898942 }, "isLocked": false, "linkedName": "W1" }, { "anchor": { - "x": 7.075174195141224, - "y": 5.77776349281702 + "x": 7.171317037669537, + "y": 6.2661384897237875 }, "prevControl": { - "x": 6.601737727551603, - "y": 5.809938786730877 + "x": 6.763763314760673, + "y": 6.418971135814612 }, "nextControl": { - "x": 7.419116116401798, - "y": 5.754388799139118 + "x": 7.478795203272495, + "y": 6.150834177622678 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.26566006995396, - "y": 5.77776349281702 + "x": 8.265148290642474, + "y": 6.0100254110329105 }, "prevControl": { - "x": 7.7535366418250575, - "y": 5.480142024113835 + "x": 7.610151235967514, + "y": 6.301135213110671 }, "nextControl": null, "isLocked": false, @@ -55,7 +55,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 165.68895873739373, + "rotation": 159.67686317033701, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/W1 C3.path b/src/main/deploy/pathplanner/paths/W1 C3.path new file mode 100644 index 00000000..b17e9d58 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/W1 C3.path @@ -0,0 +1,86 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.883271729453586, + "y": 6.979658581285779 + }, + "prevControl": null, + "nextControl": { + "x": 3.530607999863342, + "y": 5.659705452391093 + }, + "isLocked": false, + "linkedName": "W1" + }, + { + "anchor": { + "x": 4.485829113502199, + "y": 4.570423892620835 + }, + "prevControl": { + "x": 3.579300729111895, + "y": 5.089617421862554 + }, + "nextControl": { + "x": 5.28638106921604, + "y": 4.111925954348362 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.26566006995396, + "y": 4.095455268178173 + }, + "prevControl": { + "x": 7.171317037669537, + "y": 4.060981738984754 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "C3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.55, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "Easy there cowboy", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.85, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 2.7, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + } + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.5, + "maxAngularVelocity": 1137.21, + "maxAngularAcceleration": 1492.9 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Wings", + "previewStartingState": { + "rotation": -148.47050160124863, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/W1 W2.path b/src/main/deploy/pathplanner/paths/W1 W2.path index f4184801..b39bfe99 100644 --- a/src/main/deploy/pathplanner/paths/W1 W2.path +++ b/src/main/deploy/pathplanner/paths/W1 W2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": null, "nextControl": { - "x": 2.3512739869758232, - "y": 6.889078258418361 + "x": 2.3421597300896875, + "y": 6.886698060397019 }, "isLocked": false, "linkedName": "W1" @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 2.901578927457967, - "y": 5.538747023742648 + "x": 2.8726623557949864, + "y": 5.572415049000463 }, "prevControl": { - "x": 2.302037772089848, - "y": 5.537525660272609 + "x": 2.2731212004268673, + "y": 5.571193685530424 }, "nextControl": null, "isLocked": false, @@ -83,7 +83,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/W1 W3.path b/src/main/deploy/pathplanner/paths/W1 W3.path index fa558a34..fa68ffde 100644 --- a/src/main/deploy/pathplanner/paths/W1 W3.path +++ b/src/main/deploy/pathplanner/paths/W1 W3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": null, "nextControl": { - "x": 2.117670775281163, - "y": 7.208241836977058 + "x": 2.1085565183950274, + "y": 7.2058616389557155 }, "isLocked": false, "linkedName": "W1" }, { "anchor": { - "x": 2.506282459373429, - "y": 4.359369286114458 + "x": 2.370694368507382, + "y": 4.369015382098561 }, "prevControl": { - "x": 1.1043589388410582, - "y": 4.134142228717455 + "x": 0.9687708479750108, + "y": 4.143788324701558 }, "nextControl": null, "isLocked": false, @@ -73,14 +73,14 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 }, "goalEndState": { "velocity": 0, - "rotation": -179.31793960682745, + "rotation": 153.81819246855673, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/W2 A.path b/src/main/deploy/pathplanner/paths/W2 A.path index d31e63b0..3e0364ed 100644 --- a/src/main/deploy/pathplanner/paths/W2 A.path +++ b/src/main/deploy/pathplanner/paths/W2 A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.901578927457967, - "y": 5.538747023742648 + "x": 2.8726623557949864, + "y": 5.572415049000463 }, "prevControl": null, "nextControl": { - "x": 1.3579375214836105, - "y": 4.932286929066075 + "x": 1.3290209498206298, + "y": 4.96595495432389 }, "isLocked": false, "linkedName": "W2" @@ -84,7 +84,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/W2 C1.path b/src/main/deploy/pathplanner/paths/W2 C1.path index c9f1b83b..f12575e6 100644 --- a/src/main/deploy/pathplanner/paths/W2 C1.path +++ b/src/main/deploy/pathplanner/paths/W2 C1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.901578927457967, - "y": 5.538747023742648 + "x": 2.8726623557949864, + "y": 5.572415049000463 }, "prevControl": null, "nextControl": { - "x": 3.901578927457967, - "y": 5.538747023742648 + "x": 4.0454189870225346, + "y": 5.958477790957356 }, "isLocked": false, "linkedName": "W2" }, { "anchor": { - "x": 8.26566006995396, - "y": 7.4370893646602525 + "x": 7.917787299441183, + "y": 7.552716871956289 }, "prevControl": { - "x": 7.2656600699539595, - "y": 7.4370893646602525 + "x": 6.909879597698259, + "y": 7.1813041053750535 }, "nextControl": null, "isLocked": false, @@ -30,51 +30,16 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopIntake", - "waypointRelativePos": 0.05, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopIntake" - } - } - ] - } - } - }, - { - "name": "Intake", - "waypointRelativePos": 0.75, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ToIndexer" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 }, "goalEndState": { "velocity": 0, - "rotation": 178.2100893917539, + "rotation": -161.43737350441384, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/W2 C2.path b/src/main/deploy/pathplanner/paths/W2 C2.path index 341f7097..2fb11577 100644 --- a/src/main/deploy/pathplanner/paths/W2 C2.path +++ b/src/main/deploy/pathplanner/paths/W2 C2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.901578927457967, - "y": 5.538747023742648 + "x": 2.8726623557949864, + "y": 5.572415049000463 }, "prevControl": null, "nextControl": { - "x": 5.899470290179165, - "y": 7.328898921318496 + "x": 5.8705537185161845, + "y": 7.362566946576311 }, "isLocked": false, "linkedName": "W2" }, { "anchor": { - "x": 8.26566006995396, - "y": 5.77776349281702 + "x": 8.265148290642474, + "y": 6.0100254110329105 }, "prevControl": { - "x": 6.77640360879826, - "y": 5.938639962386308 + "x": 6.775891829486774, + "y": 6.170901880602198 }, "nextControl": null, "isLocked": false, @@ -60,7 +60,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/W2 C3.path b/src/main/deploy/pathplanner/paths/W2 C3.path index ebcce444..4d38be21 100644 --- a/src/main/deploy/pathplanner/paths/W2 C3.path +++ b/src/main/deploy/pathplanner/paths/W2 C3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.901578927457967, - "y": 5.538747023742648 + "x": 2.8726623557949864, + "y": 5.572415049000463 }, "prevControl": null, "nextControl": { - "x": 4.968894602552702, - "y": 4.938417586249554 + "x": 5.7875591928074, + "y": 3.7006243484669565 }, "isLocked": false, "linkedName": "W2" @@ -20,8 +20,8 @@ "y": 4.095455268178173 }, "prevControl": { - "x": 5.28994536133258, - "y": 3.4688666746135275 + "x": 6.79469113574461, + "y": 4.251033666118688 }, "nextControl": null, "isLocked": false, @@ -30,51 +30,16 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [ - { - "name": "StopIntake", - "waypointRelativePos": 0, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "StopIntake" - } - } - ] - } - } - }, - { - "name": "ToIndexer", - "waypointRelativePos": 0.85, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "ToIndexer" - } - } - ] - } - } - } - ], + "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 }, "goalEndState": { - "velocity": 0, - "rotation": 179.09166216609205, + "velocity": 0.0, + "rotation": 179.70097830393772, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/W2 W1.path b/src/main/deploy/pathplanner/paths/W2 W1.path index 4cc5afe3..3a97be77 100644 --- a/src/main/deploy/pathplanner/paths/W2 W1.path +++ b/src/main/deploy/pathplanner/paths/W2 W1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.901578927457967, - "y": 5.538747023742648 + "x": 2.8726623557949864, + "y": 5.572415049000463 }, "prevControl": null, "nextControl": { - "x": 2.248880108062568, - "y": 5.593904670452118 + "x": 2.2199635363995873, + "y": 5.627572695709933 }, "isLocked": false, "linkedName": "W2" @@ -32,28 +32,28 @@ }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": { - "x": 2.547650694405532, - "y": 6.913091720920284 + "x": 2.5385364375193964, + "y": 6.910711522898942 }, "nextControl": { - "x": 3.3373232041458003, - "y": 7.071026222868336 + "x": 3.3282089472596645, + "y": 7.068646024846994 }, "isLocked": false, "linkedName": "W1" }, { "anchor": { - "x": 3.774908333691248, - "y": 5.915657609590695 + "x": 3.831849489661888, + "y": 6.499514282908736 }, "prevControl": { - "x": 3.650803628594939, - "y": 6.356918783266457 + "x": 3.7077447845655787, + "y": 6.940775456584499 }, "nextControl": null, "isLocked": false, @@ -117,7 +117,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/W2 W3.path b/src/main/deploy/pathplanner/paths/W2 W3.path index 94d83be2..64423cf8 100644 --- a/src/main/deploy/pathplanner/paths/W2 W3.path +++ b/src/main/deploy/pathplanner/paths/W2 W3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.901578927457967, - "y": 5.538747023742648 + "x": 2.8726623557949864, + "y": 5.572415049000463 }, "prevControl": null, "nextControl": { - "x": 1.7993448386940938, - "y": 5.312387674441769 + "x": 1.7704282670311131, + "y": 5.346055699699584 }, "isLocked": false, "linkedName": "W2" }, { "anchor": { - "x": 2.506282459373429, - "y": 4.359369286114458 + "x": 2.370694368507382, + "y": 4.369015382098561 }, "prevControl": { - "x": 1.2985429942280795, - "y": 4.365499943297937 + "x": 1.162954903362032, + "y": 4.37514603928204 }, "nextControl": null, "isLocked": false, @@ -73,7 +73,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/W3 A.path b/src/main/deploy/pathplanner/paths/W3 A.path index 56ef385e..d386dc64 100644 --- a/src/main/deploy/pathplanner/paths/W3 A.path +++ b/src/main/deploy/pathplanner/paths/W3 A.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.506282459373429, - "y": 4.359369286114458 + "x": 2.370694368507382, + "y": 4.369015382098561 }, "prevControl": null, "nextControl": { - "x": 1.7031663683376892, - "y": 4.193841542160527 + "x": 1.5675782774716418, + "y": 4.20348763814463 }, "isLocked": false, "linkedName": "W3" @@ -84,7 +84,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/W3 C4.path b/src/main/deploy/pathplanner/paths/W3 C4.path index c2461a0b..09fda67f 100644 --- a/src/main/deploy/pathplanner/paths/W3 C4.path +++ b/src/main/deploy/pathplanner/paths/W3 C4.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.506282459373429, - "y": 4.359369286114458 + "x": 2.370694368507382, + "y": 4.369015382098561 }, "prevControl": null, "nextControl": { - "x": 1.9729152844118105, - "y": 2.7960517043302815 + "x": 1.8373271935457631, + "y": 2.805697800314384 }, "isLocked": false, "linkedName": "W3" @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 8.288642422749572, - "y": 2.436296745792339 + "x": 8.275153080797576, + "y": 2.6307895836632413 }, "prevControl": { - "x": 6.860205388313975, - "y": 2.6762050855487773 + "x": 6.84671604636198, + "y": 2.8706979234196797 }, "nextControl": null, "isLocked": false, @@ -83,7 +83,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/W3 C5.path b/src/main/deploy/pathplanner/paths/W3 C5.path index 52156a51..13d22b97 100644 --- a/src/main/deploy/pathplanner/paths/W3 C5.path +++ b/src/main/deploy/pathplanner/paths/W3 C5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.506282459373429, - "y": 4.359369286114458 + "x": 2.370694368507382, + "y": 4.369015382098561 }, "prevControl": null, "nextControl": { - "x": 1.9054780553924942, - "y": 1.6496188110167695 + "x": 1.7698899645264468, + "y": 1.659264907000872 }, "isLocked": false, "linkedName": "W3" }, { "anchor": { - "x": 8.281967377058862, - "y": 0.7612043754757987 + "x": 8.277540743191514, + "y": 0.6118730249882861 }, "prevControl": { - "x": 7.938650574784042, - "y": 0.7673350326592779 + "x": 7.934223940916693, + "y": 0.6180036821717655 }, "nextControl": null, "isLocked": false, @@ -67,7 +67,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/W3 W1.path b/src/main/deploy/pathplanner/paths/W3 W1.path index dd2e7de7..8be3b2f9 100644 --- a/src/main/deploy/pathplanner/paths/W3 W1.path +++ b/src/main/deploy/pathplanner/paths/W3 W1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.506282459373429, - "y": 4.359369286114458 + "x": 2.370694368507382, + "y": 4.369015382098561 }, "prevControl": null, "nextControl": { - "x": 1.7315672483148705, - "y": 4.585572343784395 + "x": 1.5959791574488231, + "y": 4.595218439768497 }, "isLocked": false, "linkedName": "W3" }, { "anchor": { - "x": 2.892385986339722, - "y": 6.982038779307121 + "x": 2.883271729453586, + "y": 6.979658581285779 }, "prevControl": { - "x": 1.4904624658073509, - "y": 6.756811721910118 + "x": 1.481348208921215, + "y": 6.754431523888776 }, "nextControl": null, "isLocked": false, @@ -67,7 +67,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/deploy/pathplanner/paths/W3 W2.path b/src/main/deploy/pathplanner/paths/W3 W2.path index 22363ada..ca4e31c2 100644 --- a/src/main/deploy/pathplanner/paths/W3 W2.path +++ b/src/main/deploy/pathplanner/paths/W3 W2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.506282459373429, - "y": 4.359369286114458 + "x": 2.370694368507382, + "y": 4.369015382098561 }, "prevControl": null, "nextControl": { - "x": 2.069617756256789, - "y": 4.644350460780055 + "x": 1.9340296653907414, + "y": 4.653996556764158 }, "isLocked": false, "linkedName": "W3" @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 2.901578927457967, - "y": 5.538747023742648 + "x": 2.8726623557949864, + "y": 5.572415049000463 }, "prevControl": { - "x": 2.648773046706228, - "y": 5.54334349430177 + "x": 2.6198564750432474, + "y": 5.577011519559585 }, "nextControl": null, "isLocked": false, @@ -84,7 +84,7 @@ } ], "globalConstraints": { - "maxVelocity": 5.0, + "maxVelocity": 4.5, "maxAcceleration": 3.5, "maxAngularVelocity": 1137.21, "maxAngularAcceleration": 1492.9 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 191ca518..79c783c9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,22 +2,28 @@ import java.util.Optional; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.littletonrobotics.urcl.URCL; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.DriveConstants; +import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.NeoMotorConstants; +import frc.robot.util.Constants.LoggingConstants; import frc.robot.util.rev.Neo; import frc.robot.util.rev.NeoPhysicsSim; -import monologue.Monologue; /** * The VM is configured to automatically run this class, and to call the @@ -28,7 +34,7 @@ * build.gradle file in the * project. */ -public class Robot extends TimedRobot { +public class Robot extends LoggedRobot { private static Optional alliance = Optional.empty(); public static GameMode gameMode = GameMode.DISABLED; @@ -48,15 +54,39 @@ public static enum GameMode { @Override public void robotInit() { + + // Git metadata for tracking version for AKit + Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); + Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + + switch (LoggingConstants.getMode()) { + case REAL: + Logger.addDataReceiver(new WPILOGWriter("/media/sda1/logs")); // Log to a USB stick ("/U/logs") + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables + break; + case REPLAY: + String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) + Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log + Logger.addDataReceiver(new WPILOGWriter(LogFileUtil + .addPathSuffix(logPath, "_sim"))); // Save outputs to a new log + break; + case SIM: + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables + break; + } + + Logger.start(); + robotContainer = new RobotContainer(); - Monologue.setupMonologue(robotContainer, "Robot/Draggables", false, true); DataLogManager.start(); DataLogManager.logNetworkTables(true); DriverStation.startDataLog(DataLogManager.getLog(), true); DriverStation.silenceJoystickConnectionWarning(true); - // Remove if not at comp: - RobotController.setBrownoutVoltage(8.0); + RobotController.setBrownoutVoltage(6.0); } /** @@ -68,26 +98,19 @@ public void robotInit() { * and * SmartDashboard integrated updating. */ - private boolean updatedAlready = false; - private boolean updateTimer = false; private boolean startedURCL = false; + @Override public void robotPeriodic() { // Set the previous to the current timestamp before it updates Robot.previousTimestamp = Robot.currentTimestamp; Robot.currentTimestamp = Timer.getFPGATimestamp(); - if (gameMode != GameMode.DISABLED) { - Monologue.updateAll(); - } - else { - updateTimer = (int) (Robot.currentTimestamp / 10) % 2 == 0; - if (updateTimer && !updatedAlready) { - updatedAlready = true; - Monologue.updateAll(); - } else if (!updateTimer) { - updatedAlready = false; - } - } + + RobotContainer.displayTime = + DriverStation.isFMSAttached() + ? Timer.getMatchTime() // Display time left in current match mode (auto/teleop) + : Robot.currentTimestamp - RobotContainer.gameModeStart; // Display time since mode start + CommandScheduler.getInstance().run(); } @@ -104,6 +127,7 @@ public void disabledPeriodic() { // it needs to be updated. DriverStation.refreshData(); Robot.alliance = DriverStation.getAlliance(); + } @Override @@ -111,13 +135,10 @@ public void disabledExit() { // Shut off NetworkTables broadcasting for most logging calls // if we are at competition RobotContainer.gameModeStart = currentTimestamp; - // Monologue.setFileOnly(DriverStation.isFMSAttached()); } @Override public void autonomousInit() { - // Update "constants" - Monologue.updateAll(); if (!startedURCL) { URCL.start(NeoMotorConstants.CAN_ID_MAP); startedURCL = true; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 78cddc50..6617db7b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,12 +2,14 @@ import java.util.function.BooleanSupplier; +import org.littletonrobotics.junction.AutoLogOutput; + import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; - +import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -22,6 +24,7 @@ import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; @@ -37,12 +40,18 @@ import frc.robot.commands.managers.HDCTuner; import frc.robot.commands.managers.PieceControl; import frc.robot.commands.managers.ShooterCmds; -import frc.robot.leds.Strips.LedStrip; -import frc.robot.leds.Commands.LPI; -import frc.robot.subsystems.*; +import frc.robot.subsystems.ampper.Ampper; +import frc.robot.subsystems.ampper.Elevator; +import frc.robot.subsystems.climb.Climb; +import frc.robot.subsystems.colorsensor.PicoColorSensor; +import frc.robot.subsystems.drive.Swerve; +import frc.robot.subsystems.intake.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.Pivot; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.vision.Limelight; import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.CameraConstants; -import frc.robot.util.Constants.ColorSensorConstants; import frc.robot.util.Constants.DriveConstants; import frc.robot.util.Constants.ElevatorConstants; import frc.robot.util.Constants.FieldConstants; @@ -51,16 +60,13 @@ import frc.robot.util.Constants.ShooterConstants; import frc.robot.util.auto.PathPlannerStorage; import frc.robot.util.calc.LimelightMapping; +import frc.robot.util.calc.PoseCalculations; import frc.robot.util.calc.ShooterCalc; import frc.robot.util.custom.PatriBoxController; import frc.robot.util.custom.ActiveConditionalCommand; import frc.robot.util.rev.Neo; -import monologue.Annotations.IgnoreLogged; -import monologue.Annotations.Log; -import monologue.Logged; -import monologue.Monologue; -public class RobotContainer implements Logged { +public class RobotContainer { private PowerDistribution pdh; @@ -71,78 +77,67 @@ public class RobotContainer implements Logged { private final PatriBoxController driver; private final PatriBoxController operator; - @IgnoreLogged private Swerve swerve; - @IgnoreLogged private final Intake intake; - @IgnoreLogged private Limelight limelight3g; - @IgnoreLogged private Limelight limelight3; - @IgnoreLogged private LimelightMapping limelightMapper; - @IgnoreLogged private final Climb climb; - @IgnoreLogged private Pivot pivot; - @IgnoreLogged private Shooter shooter; - @IgnoreLogged private Elevator elevator; - @IgnoreLogged private CalibrationControl calibrationControl; - @IgnoreLogged private PathPlannerStorage pathPlannerStorage; - @IgnoreLogged private ShooterCalc shooterCalc; - @IgnoreLogged public static HDCTuner HDCTuner; - private final LedStrip ledStrip; - @IgnoreLogged private Indexer indexer; - @IgnoreLogged private Ampper ampper; private ShooterCmds shooterCmds; - @IgnoreLogged - private ColorSensor colorSensor = new ColorSensor(ColorSensorConstants.I2C_PORT); + private PicoColorSensor piPico; - @IgnoreLogged private PieceControl pieceControl; private AlignmentCmds alignmentCmds; + private boolean fieldRelativeToggle = true; private final BooleanSupplier robotRelativeSupplier; - @Log + @AutoLogOutput (key = "Draggables/Components3d") public static Pose3d[] components3d = new Pose3d[5]; - @Log + @AutoLogOutput (key = "Draggables/DesiredComponents3d") public static Pose3d[] desiredComponents3d = new Pose3d[5]; - @Log + @AutoLogOutput (key = "Draggables/NotePose3ds") public static Pose3d[] notePose3ds = new Pose3d[12]; - @Log + @AutoLogOutput (key = "Draggables/HighNotePose3ds") public static Pose3d[] highNotePose3ds = new Pose3d[12]; - @Log + @AutoLogOutput (key = "Draggables/FreshCode") private boolean freshCode = true; - @Log - public static Field2d field2d = new Field2d(); - @Log + @AutoLogOutput (key = "Draggables/RobotPose2d") public static Pose2d robotPose2d = new Pose2d(); - @Log + @AutoLogOutput (key = "Draggables/VisionErrorPose") public static Transform2d visionErrorPose = new Transform2d(); - @Log + @AutoLogOutput (key = "Draggables/DistanceToSpeakerMeters") public static double distanceToSpeakerMeters = 0; - @Log + @AutoLogOutput (key = "Draggables/DistanceToSpeakerFeet") + public static double distanceToSpeakerFeet = 0; + @AutoLogOutput (key = "Draggables/RobotPose3d") public static Pose3d robotPose3d = new Pose3d(); - @Log + @AutoLogOutput (key = "Draggables/SwerveMeasuredStates") public static SwerveModuleState[] swerveMeasuredStates; - @Log + @AutoLogOutput (key = "Draggables/SwerveDesiredStates") public static SwerveModuleState[] swerveDesiredStates; - @Log + @AutoLogOutput (key = "Draggables/GameModeStart") public static double gameModeStart = 0; - @Log + @AutoLogOutput (key = "Draggables/HasPiece") public static boolean hasPiece = true; - @Log + @AutoLogOutput (key = "Draggables/EnableVision") public static boolean enableVision = true; + @AutoLogOutput (key = "Draggables/Timer") + public static double displayTime = 0.0; + @AutoLogOutput (key = "Draggables/AutoStatingPose") + public static Pose2d autoStartingPose = new Pose2d(); + + public static Field2d field2d = new Field2d(); public RobotContainer() { @@ -152,7 +147,9 @@ public RobotContainer() { operator = new PatriBoxController(OIConstants.OPERATOR_CONTROLLER_PORT, OIConstants.OPERATOR_DEADBAND); pdh = new PowerDistribution(30, ModuleType.kRev); - pdh.setSwitchableChannel(false); + pdh.setSwitchableChannel(false); + + piPico = new PicoColorSensor(); intake = new Intake(); climb = new Climb(); @@ -173,9 +170,7 @@ public RobotContainer() { limelight3g.disableLEDS(); } - ledStrip = new LedStrip(swerve::getPose); indexer = new Indexer(); - shooter = new Shooter(); elevator = new Elevator(); ampper = new Ampper(); @@ -201,11 +196,14 @@ public RobotContainer() { elevator, ampper, shooterCmds, - colorSensor); + piPico); calibrationControl = new CalibrationControl(shooterCmds); - robotRelativeSupplier = () -> !driver.getYButton(); + driver.back().toggleOnTrue( + Commands.runOnce(() -> fieldRelativeToggle = !fieldRelativeToggle) + ); + robotRelativeSupplier = () -> fieldRelativeToggle; swerve.setDefaultCommand(new Drive( swerve, @@ -219,21 +217,25 @@ public RobotContainer() { shooter.setDefaultCommand( pieceControl.getAutomaticShooterSpeeds( swerve::getPose, - () -> driver.getLeftTrigger() || (!OIConstants.SINGLE_DRIVER_MODE && operator.getLeftBumper()) + () -> driver.getLeftBumper() + || (OIConstants.OPERATOR_PRESENT + && operator.getLeftBumper()), + () -> OIConstants.OPERATOR_PRESENT + && (operator.getYButton()) ) ); - BooleanSupplier autoDecisionMaker; if (FieldConstants.IS_SIMULATION) { - autoDecisionMaker = driver::getYButton; + pathPlannerStorage = new PathPlannerStorage(operator::getYButton, swerve, limelight3); } else { - autoDecisionMaker = colorSensor::hasNote; + pathPlannerStorage = new PathPlannerStorage(piPico::hasNoteShooter, piPico::hasNoteElevator, swerve, limelight3); } - pathPlannerStorage = new PathPlannerStorage(autoDecisionMaker, swerve, limelight3); initializeComponents(); prepareNamedCommands(); + SmartDashboard.putData(field2d); + pathPlannerStorage.configureAutoChooser(); pathPlannerStorage.getAutoChooser().addOption("WheelRadiusCharacterization", disableVision() @@ -243,10 +245,8 @@ public RobotContainer() { .andThen(enableVision())); configureButtonBindings(); - configureLoggingPaths(); pdh.setSwitchableChannel(false); - Monologue.updateAll(); } @@ -257,13 +257,18 @@ private void configureButtonBindings() { return; } - if (OIConstants.SINGLE_DRIVER_MODE) { - configureSoloDiverBindings(driver); - } else { - configureDriverBindings(driver); - configureOperatorBindings(operator); + switch (OIConstants.DRIVER_MODE) { + case SINGLE: + configureSoloDriverBindings(driver); + break; + case DOUBLE: + configureDriverBindings(driver); + configureOperatorBindings(operator); + break; + case DEV: + configureDevDriverBindings(driver); + break; } - configureTimedEvents(); configureTestBindings(); @@ -274,7 +279,8 @@ private void configureTestBindings() { // See https://docs.wpilib.org/en/stable/docs/software/convenience-features/event-based.html // for more information // configureHDCBindings(driver); - configureCalibrationBindings(driver); + configureCalibrationBindings(operator, testButtonBindingLoop); + // configureCalibrationBindings(operator, CommandScheduler.getInstance().getDefaultButtonLoop()); } private void configureTimedEvents() { @@ -297,21 +303,18 @@ private void configureTimedEvents() { // if shootWhenReady is called at while this condition is true new Trigger(() -> Robot.gameMode == GameMode.TELEOP - && shooter.getAverageSpeed() > 2500 - && shooter.getAverageTargetSpeed() != 2500 - && swerve.getPose().getX() > FieldConstants.CENTERLINE_X ^ Robot.isBlueAlliance() + && shooter.getAverageSpeed() > 1700 + && ((swerve.getPose().getX() < FieldConstants.CENTERLINE_X ^ Robot.isBlueAlliance()) || PoseCalculations.closeToSpeaker()) && limelight3g.getPose2d().getTranslation().getDistance(swerve.getPose().getTranslation()) < Units.inchesToMeters(4)) - .onTrue(Commands.runOnce(() -> - pdh.setSwitchableChannel(true)).alongWith(limelight3g.blinkLeds(() -> 1), driver.setRumble(() -> 1))) - .onFalse(Commands.runOnce(() -> - pdh.setSwitchableChannel(false)).alongWith(driver.setRumble(() -> 0))); + .onTrue(driver.setRumble(() -> 1.0)) + .onFalse(driver.setRumble(() -> 0)); // When our alliance changes, reflect that in the path previewer new Trigger(Robot::isRedAlliance) .onTrue(pathPlannerStorage.updatePathViewerCommand()) .onFalse(pathPlannerStorage.updatePathViewerCommand()); - new Trigger(swerve::isAlignedToAmp).or(shooterCalc.readyToShootSupplier()) + new Trigger(() -> PoseCalculations.isAlignedToAmp(robotPose2d)) .onTrue(driver.setRumble(() -> 0.5)) .onFalse(driver.setRumble(() -> 0)); @@ -319,13 +322,12 @@ private void configureTimedEvents() { // The reason we are checking bumpers // is so this doesn't happen on pieceControl::moveNote new Trigger( - () -> colorSensor.hasNote() + () -> (piPico.hasNoteShooter() || piPico.hasNoteElevator()) && ( // All of these buttons command intake // Whether that be source or not - driver.getLeftTrigger() - || driver.getXButton() - || (!OIConstants.SINGLE_DRIVER_MODE + driver.getLeftBumper() + || (OIConstants.OPERATOR_PRESENT &&(operator.getLeftBumper() || operator.getStartButton() || operator.getBackButton())) @@ -361,6 +363,7 @@ private void configureTimedEvents() { ).onFalse( driver.setRumble(() -> 0) ); + } private void configureFieldCalibrationBindings(PatriBoxController controller) { @@ -404,80 +407,80 @@ private void configureCommonDriverBindings(PatriBoxController controller) { // Upon hitting start or back, // reset the orientation of the robot // to be facing AWAY FROM the driver station - controller.back() - .onTrue(Commands.runOnce(() -> - swerve.resetOdometry(FieldConstants.GET_SUBWOOFER_POSITION()), swerve - )); - - // Upon hitting start button - // reset the orientation of the robot - // to be facing TOWARDS the driver station - // TODO: for testing reset odometry to speaker controller.start() .onTrue(Commands.runOnce(() -> - swerve.resetOdometry(FieldConstants.GET_SUBWOOFER_POSITION().plus(new Transform2d(0,0, Rotation2d.fromDegrees(180)))), swerve - )); - + swerve.resetOdometry(FieldConstants.GET_SUBWOOFER_POSITION()), swerve)); // Speaker / Source / Chain rotational alignment controller.rightStick() + .onTrue(swerve.resetHDCCommand()) .toggleOnTrue( Commands.sequence( - elevator.toBottomCommand(), - swerve.resetHDCCommand(), + pieceControl.elevatorToBottomSafe() + .unless(() -> elevator.getDesiredPosition() == ElevatorConstants.BOTTOM_POS), limelight3g.setLEDState(() -> true), new ActiveConditionalCommand( // This runs SWD on heading control // and shooting-while-still on shooter alignmentCmds.wingRotationalAlignment(controller::getLeftX, controller::getLeftY, robotRelativeSupplier), alignmentCmds.preparePassCommand(controller::getLeftX, controller::getLeftY, robotRelativeSupplier), - alignmentCmds.alignmentCalc::closeToSpeaker) + () -> PoseCalculations.inSpeakerShotZone(robotPose2d.getTranslation()) || climb.getHooksUp()) ).finallyDo( () -> - limelight3g.setLEDState(() -> false) - .andThen(shooterCmds.raisePivot() - .alongWith(shooterCmds.stopShooter()) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf)) - .schedule() - ) - ); - - - // Climbing controls - controller.povUp() - .onTrue(climb.povUpCommand()); - - controller.povDown() - .onTrue(climb.toBottomCommand().alongWith(shooterCmds.stowPivot())); + Commands.parallel( + swerve.resetDesiredHDCPoseCommand(), + limelight3g.setLEDState(() -> false), + Commands.parallel( + shooterCmds.raisePivot(), + shooterCmds.stopShooter()) + .withInterruptBehavior(InterruptionBehavior.kCancelSelf)) + .schedule())); // Note to target will either place amp or shoot, // depending on if the elevator is up or not controller.rightTrigger() - .onTrue(pieceControl.noteToTarget(swerve::getPose, swerve::getRobotRelativeVelocity, swerve::atHDCAngle, () -> controller.getLeftBumper()) - .alongWith(driver.setRumble(() -> 0.5, 0.3)) + .onTrue( + pieceControl.noteToTarget( + swerve::getPose, + swerve::getRobotRelativeVelocity, + swerve::atHDCAngle, + operator::getLeftBumper) + .alongWith(driver.setRumble(() -> 0.5, 0.3)) .andThen(Commands.runOnce(() -> RobotContainer.hasPiece = false))); - - - // Intake controls - // The warning of dead code only applies if we are using single driver mode - controller.leftTrigger() - .whileTrue(pieceControl.intakeNoteDriver(swerve::getPose, swerve::getRobotRelativeVelocity)) - .negate().and(() -> OIConstants.SINGLE_DRIVER_MODE || !operator.getLeftBumper()) - .onTrue(pieceControl.stopIntakeAndIndexer()); controller.rightBumper() .onTrue(pieceControl.ejectNote()) - .onFalse(pieceControl.stopEjecting()); + .negate().and(() -> !OIConstants.OPERATOR_PRESENT || !operator.getRightBumper()) + .onTrue(pieceControl.stopEjecting()); + + // Climbing controls + controller.povUp() + .onTrue(climb.povUpCommand()); + + controller.povDown() + .onTrue( + climb.toBottomCommand() + .alongWith(shooterCmds.stowPivot())); // POV left and right are uncommonly used but needed incase of emergency controller.povLeft() - .onTrue(pieceControl.stopAllMotors().andThen(shooterCmds.raisePivot())); - + .onTrue( + Commands.sequence( + pieceControl.stopAllMotors(), + Commands.waitUntil(elevator::atDesiredPosition), + shooterCmds.raisePivot() + )); + controller.povRight() - .onTrue(Commands.runOnce(() -> pdh.setSwitchableChannel(false))); + .onTrue( + Commands.sequence( + pieceControl.stopAllMotors(), + shooterCmds.stowPivot() + )); + } - private void configureSoloDiverBindings(PatriBoxController controller) { + private void configureSoloDriverBindings(PatriBoxController controller) { configureCommonDriverBindings(controller); @@ -485,17 +488,21 @@ private void configureSoloDiverBindings(PatriBoxController controller) { .whileTrue(pieceControl.blepNote()) .onFalse(pieceControl.stopIntakeAndIndexer().alongWith(shooterCmds.raisePivot())); - controller.x() + controller.leftBumper() .whileTrue(pieceControl.sourceShooterIntake()) .onFalse(pieceControl.stopIntakeAndIndexer().alongWith(shooterCmds.raisePivot())); controller.b() - .onTrue(pieceControl.noteToTrap().andThen(elevator.toTopCommand()).andThen(pieceControl.prepPiece())); + .onTrue(pieceControl.noteToTrap3() + .andThen( + pieceControl.elevatorUpWhileIntaking(), + pieceControl.prepPiece())); // If this is nice to work with, then we keep it. If not... bye bye! new Trigger(() -> elevator.getDesiredPosition() == ElevatorConstants.TRAP_PLACE_POS && swerve.getPose().getY() > FieldConstants.FIELD_HEIGHT_METERS/2.0) .onTrue(swerve.resetHDCCommand()) + .onFalse(swerve.resetDesiredHDCPoseCommand()) .whileTrue(alignmentCmds.ampRotationalAlignmentCommand(driver::getLeftX, driver::getLeftY)); // Subwoofer preset incase something goes south @@ -503,18 +510,17 @@ private void configureSoloDiverBindings(PatriBoxController controller) { .toggleOnTrue(shooterCmds.prepareSubwooferCommand().withInterruptBehavior(InterruptionBehavior.kCancelSelf)); // Quick uppies for double amping - controller.leftBumper().and(() -> true) + controller.y() .onTrue(shooterCmds.raisePivot().alongWith(elevator.toNoteFixCommand().alongWith(pieceControl.intakeForDoubleAmp()))) - .onFalse(pieceControl.stopIntakeAndIndexer().andThen(pieceControl.doubleAmpElevatorEnd())); + .onFalse(ampper.outtakeSlow(.3).andThen(pieceControl.stopIntakeAndIndexer(),pieceControl.doubleAmpElevatorEnd())); // controller.leftBumper() // .onTrue(elevator.toTopIshButNotFullCommand()) // .onFalse(elevator.toBottomCommand()); - // controller.leftBumper() - // .whileTrue(pieceControl.intakeAuto()) - // .negate().and(driver.leftTrigger().negate()) - // .onTrue(pieceControl.stopIntakeAndIndexer()); + controller.leftBumper() + .whileTrue(pieceControl.intakeAuto()) + .onFalse(pieceControl.stopIntakeAndIndexer()); } @@ -522,30 +528,45 @@ private void configureDriverBindings(PatriBoxController controller) { configureCommonDriverBindings(controller); + controller.leftStick() + .toggleOnTrue( + Commands.sequence( + pieceControl.elevatorToBottomSafe(), + shooterCmds.prepareSubwooferCommand() + ).finallyDo( + () -> + Commands.parallel( + shooterCmds.raisePivot(), + shooterCmds.stopShooter()) + .withInterruptBehavior(InterruptionBehavior.kCancelSelf) + .schedule())); + + controller.a() .onTrue(swerve.resetHDCCommand()) .whileTrue( - Commands.sequence( - Commands.either( - alignmentCmds.trapAlignmentCommand(controller::getLeftX, controller::getLeftY), - alignmentCmds.ampAlignmentCommand(controller::getLeftX), - climb::getHooksUp)) - .alongWith( - limelight3g.setLEDState(() -> true))) + alignmentCmds.ampRotationalAlignmentCommand(controller::getLeftX, controller::getLeftY) + .alongWith(limelight3g.setLEDState(() -> true))) .onFalse( - limelight3g.setLEDState(() -> false)); - - controller.x() - .toggleOnTrue( - alignmentCmds.preparePresetPose(driver::getLeftX, driver::getLeftY, true)); + Commands.parallel( + limelight3g.setLEDState(() -> false), + swerve.resetHDCCommand(), + swerve.resetDesiredHDCPoseCommand())); + controller.leftBumper() + .whileTrue(pieceControl.intakeNoteDriver(swerve::getPose, swerve::getRobotRelativeVelocity)) + .negate().and(() -> !OIConstants.OPERATOR_PRESENT || !operator.getLeftBumper()) + .onTrue(pieceControl.stopIntakeAndIndexer()); - controller.b() - .toggleOnTrue( - alignmentCmds.preparePresetPose(driver::getLeftX, driver::getLeftY, false)); + controller.y() + .onTrue( + Commands.either( + climb.toTopCommand().alongWith(climb.setToggleMode(true)), + climb.toBottomCommand().alongWith(shooterCmds.stowPivot()), + () -> !climb.getHooksUp())); - controller.leftBumper() - .toggleOnTrue(shooterCmds.preparePassCommand(swerve::getPose) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf)); + controller.leftTrigger() + .onTrue(pieceControl.blepNote()); + } private void configureOperatorBindings(PatriBoxController controller) { @@ -559,20 +580,24 @@ private void configureOperatorBindings(PatriBoxController controller) { .onTrue(ampper.toggleSpeed()); controller.povDown() - .onTrue(pieceControl.elevatorToBottom()); + .onTrue(pieceControl.elevatorToBottomSafe()); controller.leftBumper() .whileTrue(pieceControl.intakeUntilNote()) - .negate().and(driver.leftTrigger().negate()) + .negate().and(driver.leftBumper().negate()) .onTrue(pieceControl.stopIntakeAndIndexer()); controller.rightBumper() .onTrue(pieceControl.ejectNote()) - .onFalse(pieceControl.stopEjecting()); + .negate().and(driver.rightBumper().negate()) + .onTrue(pieceControl.stopEjecting()); controller.rightTrigger() .onTrue(pieceControl - .noteToTarget(swerve::getPose, swerve::getRobotRelativeVelocity, swerve::atHDCAngle, + .noteToTarget( + swerve::getPose, + swerve::getRobotRelativeVelocity, + () -> swerve.atHDCAngle(), () -> controller.getLeftBumper()) .alongWith(driver.setRumble(() -> 0.5, 0.3)) .andThen(Commands.runOnce(() -> RobotContainer.hasPiece = false))); @@ -587,8 +612,10 @@ private void configureOperatorBindings(PatriBoxController controller) { .onTrue(pieceControl.panicEjectNote()) .onFalse(pieceControl.stopPanicEject()); - controller.start().or(controller.back()).or(controller.y()) - .whileTrue(pieceControl.sourceShooterIntake()) + controller.start().or(controller.back()) + .whileTrue( + Commands.waitUntil(elevator::atDesiredPosition) + .andThen(pieceControl.sourceShooterIntake())) .onFalse(pieceControl.stopIntakeAndIndexer()); controller.rightStick().and(controller.leftStick()) @@ -599,46 +626,70 @@ private void configureOperatorBindings(PatriBoxController controller) { } - private void configureCalibrationBindings(PatriBoxController controller) { - controller.leftBumper(testButtonBindingLoop) + private void configureDevDriverBindings(PatriBoxController controller) { + + configureCommonDriverBindings(controller); + + controller.x() + .onTrue( + swerve.resetOdometryCommand(FieldConstants::GET_SAMPLE_CENTER_PASS_POSITION)); + + controller.b() + .onTrue( + swerve.resetOdometryCommand(FieldConstants::GET_SAMPLE_SOURCE_PASS_POSITION)); + + controller.leftTrigger() + .whileTrue(pieceControl.sourceShooterIntake()) + .onFalse(pieceControl.stopIntakeAndIndexer()); + + controller.y() + .whileTrue(pieceControl.intakeNoteDriver(swerve::getPose, swerve::getRobotRelativeVelocity)) + .negate().and(() -> !OIConstants.OPERATOR_PRESENT || !operator.getLeftBumper()) + .onTrue(pieceControl.stopIntakeAndIndexer()); + + + } + + private void configureCalibrationBindings(PatriBoxController controller, EventLoop eventLoop) { + controller.leftBumper(eventLoop) .onTrue(pieceControl.stopAllMotors().andThen(shooterCmds.raisePivot())); - controller.rightBumper(testButtonBindingLoop).onTrue(calibrationControl.updateMotorsCommand()); - controller.rightTrigger(0.5, testButtonBindingLoop) + controller.rightBumper(eventLoop).onTrue(calibrationControl.updateMotorsCommand()); + controller.rightTrigger(0.5, eventLoop) .onTrue(pieceControl.shootWhenReady(swerve::getPose, swerve::getRobotRelativeVelocity, () -> true)); - controller.leftY(0.3, testButtonBindingLoop) + controller.leftY(0.3, eventLoop) .whileTrue(calibrationControl.incrementSpeeds(() -> (int) (controller.getLeftY() * 5))); - controller.rightY(0.3, testButtonBindingLoop) + controller.rightY(0.3, eventLoop) .whileTrue(calibrationControl.incrementAngle(() -> controller.getRightY())); - controller.leftX(0.3, testButtonBindingLoop) + controller.leftX(0.3, eventLoop) .whileTrue(calibrationControl.incrementLeftSpeed(() -> (int) (controller.getLeftX() * 5))); - controller.rightX(0.3, testButtonBindingLoop) + controller.rightX(0.3, eventLoop) .whileTrue(calibrationControl.incrementRightSpeed(() -> (int) (controller.getRightX() * 5))); - controller.back(testButtonBindingLoop).onTrue(calibrationControl.incrementDistance(-1)); - controller.start(testButtonBindingLoop).onTrue(calibrationControl.incrementDistance(1)); + controller.back(eventLoop).onTrue(calibrationControl.incrementDistance(-0.5)); + controller.start(eventLoop).onTrue(calibrationControl.incrementDistance(0.5)); - controller.a(testButtonBindingLoop).onTrue(calibrationControl.logTriplet()); + controller.a(eventLoop).onTrue(calibrationControl.logTriplet()); - controller.x(testButtonBindingLoop).onTrue(calibrationControl.toggleLeftLock()); - controller.b(testButtonBindingLoop).onTrue(calibrationControl.toggleRightLock()); - controller.y(testButtonBindingLoop).onTrue(calibrationControl.togglePivotLock()); + // controller.x(eventLoop).onTrue(calibrationControl.toggleLeftLock()); + // controller.b(eventLoop).onTrue(calibrationControl.toggleRightLock()); + // controller.y(eventLoop).onTrue(calibrationControl.togglePivotLock()); - controller.pov(0, 270, testButtonBindingLoop) + controller.pov(0, 270, eventLoop) .whileTrue(pieceControl.intakeNoteDriver(swerve::getPose, swerve::getRobotRelativeVelocity) .alongWith(Commands.run(swerve::setWheelsX))) .onFalse(pieceControl.stopIntakeAndIndexer()); - controller.pov(0, 90, testButtonBindingLoop) + controller.pov(0, 90, eventLoop) .onTrue(pieceControl.ejectNote()); - controller.pov(0, 0, testButtonBindingLoop) + controller.pov(0, 0, eventLoop) .whileTrue(pieceControl.sourceShooterIntake()) .onFalse(pieceControl.stopIntakeAndIndexer()); - controller.pov(0, 180, testButtonBindingLoop) - .onTrue(calibrationControl.copyCalcTriplet()); + controller.pov(0, 180, eventLoop) + .onTrue(calibrationControl.copyCalcTriplet(swerve::getPose)); } private void configureHDCBindings(PatriBoxController controller) { @@ -693,9 +744,7 @@ public void onDisabled() { } public void onEnabled() { - if (Robot.gameMode == GameMode.TELEOP && !DriverStation.isFMSAttached()) { - new LPI(ledStrip, swerve::getPose, driver, swerve::setDesiredPose).schedule(); - } else if (Robot.gameMode == GameMode.TEST) { + if (Robot.gameMode == GameMode.TEST) { CommandScheduler.getInstance().setActiveButtonLoop(testButtonBindingLoop); } gameModeStart = Robot.currentTimestamp; @@ -704,47 +753,61 @@ public void onEnabled() { } public void updateNTGains() { - double P = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/0P").getDouble(-1); - double I = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/1I").getDouble(-1); - double D = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/2D").getDouble(-1); - - double P2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/0P").getDouble(-1); - double I2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/1I").getDouble(-1); - double D2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/2D").getDouble(-1); - - if (P == -1 || I == -1 || D == -1 || P2 == -1 || I2 == -1 || D2 == -1) { + double HPFCP = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/0P").getDouble(-1); + double HPFCI = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/1I").getDouble(-1); + double HPFCD = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/2D").getDouble(-1); + double HPFCP2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/0P").getDouble(-1); + double HPFCI2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/1I").getDouble(-1); + double HPFCD2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/2D").getDouble(-1); + + double HDCP = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Translation/0P").getDouble(-1); + double HDCI = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Translation/1I").getDouble(-1); + double HDCD = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Translation/2D").getDouble(-1); + double HDCP2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Rotation/0P").getDouble(-1); + double HDCI2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Rotation/1I").getDouble(-1); + double HDCD2 = NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Rotation/2D").getDouble(-1); + + if (HPFCP == -1 || HPFCI == -1 || HPFCD == -1 || HPFCP2 == -1 || HPFCI2 == -1 || HPFCD2 == -1 || + HDCP == -1 || HDCI == -1 || HDCD == -1 || HDCP2 == -1 || HDCI2 == -1 || HDCD2 == -1) { NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/0P").setDouble(AutoConstants.XY_CORRECTION_P); NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/1I").setDouble(AutoConstants.XY_CORRECTION_I); NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Translation/2D").setDouble(AutoConstants.XY_CORRECTION_D); NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/0P").setDouble(AutoConstants.ROTATION_CORRECTION_P); NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/1I").setDouble(AutoConstants.ROTATION_CORRECTION_I); NetworkTableInstance.getDefault().getTable("Calibration").getEntry("Auto/Rotation/2D").setDouble(AutoConstants.ROTATION_CORRECTION_D); + + NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Translation/0P").setDouble(AutoConstants.XY_CORRECTION_P); + NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Translation/1I").setDouble(AutoConstants.XY_CORRECTION_I); + NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Translation/2D").setDouble(AutoConstants.XY_CORRECTION_D); + NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Rotation/0P").setDouble(AutoConstants.ROTATION_CORRECTION_P_TELE); + NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Rotation/1I").setDouble(AutoConstants.ROTATION_CORRECTION_I); + NetworkTableInstance.getDefault().getTable("Calibration").getEntry("HDC/Rotation/2D").setDouble(AutoConstants.ROTATION_CORRECTION_D); return; - } - - if (!(MathUtil.isNear(AutoConstants.HPFC.translationConstants.kP, P, 0.01) - && MathUtil.isNear(AutoConstants.HPFC.translationConstants.kI, I, 0.01) - && MathUtil.isNear(AutoConstants.HPFC.translationConstants.kD, D, 0.01) - && MathUtil.isNear(AutoConstants.HPFC.rotationConstants.kP, P2, 0.01) - && MathUtil.isNear(AutoConstants.HPFC.rotationConstants.kI, I2, 0.01) - && MathUtil.isNear(AutoConstants.HPFC.rotationConstants.kD, D2, 0.01))) - { + } else { AutoConstants.HPFC = new HolonomicPathFollowerConfig( new PIDConstants( - P, - I, - D), + HPFCP, + HPFCI, + HPFCD), new PIDConstants( - P2, - I2, - D2), + HPFCP2, + HPFCI2, + HPFCD2), DriveConstants.MAX_SPEED_METERS_PER_SECOND, Math.hypot(DriveConstants.WHEEL_BASE, DriveConstants.TRACK_WIDTH) / 2.0, new ReplanningConfig()); + + AutoConstants.XY_PID.setP(HDCP); + AutoConstants.XY_PID.setI(HDCI); + AutoConstants.XY_PID.setD(HDCD); + AutoConstants.THETA_PID.setP(HDCP2); + AutoConstants.THETA_PID.setI(HDCI2); + AutoConstants.THETA_PID.setD(HDCD2); + - swerve.reconfigureAutoBuilder(); - fixPathPlannerCommands(); - System.out.println("Reconfigured HPFC"); + // swerve.reconfigureAutoBuilder(); + // fixPathPlannerCommands(); + // System.out.println("Reconfigured HPFC"); } } @@ -754,7 +817,7 @@ private void prepareNamedCommands() { NamedCommands.registerCommand("ToIndexer", Commands.either( pieceControl.intakeUntilNote(), - Commands.waitUntil(driver::getYButton), + Commands.waitUntil(operator::getYButton), () -> !FieldConstants.IS_SIMULATION ) ); @@ -765,7 +828,7 @@ private void prepareNamedCommands() { NamedCommands.registerCommand("ShootInstantly", Commands.either( pieceControl.noteToShootUsingSensor(swerve::getPose, swerve::getRobotRelativeVelocity), - Commands.waitUntil(() -> !driver.getYButton()), + Commands.waitUntil(() -> !operator.getYButton()), () -> !FieldConstants.IS_SIMULATION ) ); @@ -775,11 +838,18 @@ private void prepareNamedCommands() { .andThen( Commands.either( pieceControl.noteToShootUsingSensorWhenReady(swerve::getPose, swerve::getRobotRelativeVelocity), - Commands.waitUntil(() -> !driver.getYButton()), + Commands.waitUntil(() -> !operator.getYButton()), () -> !FieldConstants.IS_SIMULATION - ) + ).withTimeout(1.5) ) ); + NamedCommands.registerCommand("ShootInstantlyWhenReady2", + Commands.either( + pieceControl.noteToShootUsingSensorWhenReady(swerve::getPose, swerve::getRobotRelativeVelocity), + Commands.waitUntil(() -> !operator.getYButton()), + () -> !FieldConstants.IS_SIMULATION + ).withTimeout(1.5) + ); NamedCommands.registerCommand("ShootWhenReady", pieceControl.shootPreload()); NamedCommands.registerCommand("RaiseElevator", elevator.toTopCommand()); NamedCommands.registerCommand("LowerElevator", elevator.toBottomCommand()); @@ -792,6 +862,39 @@ private void prepareNamedCommands() { NamedCommands.registerCommand("PrepareSWD", shooterCmds.prepareSWDCommandAuto(swerve::getPose, swerve::getRobotRelativeVelocity)); NamedCommands.registerCommand("DisableLimelight", disableVision()); NamedCommands.registerCommand("EnableLimelight", enableVision()); + NamedCommands.registerCommand("FullPowerPreload", + shooter.fullPower(2000) // Say that again? + .alongWith(Commands.waitUntil(pivot::getAtDesiredAngle)) + .andThen(pieceControl.intakeAuto() + .alongWith(shooterCmds.getNoteTrajectoryCommand(swerve::getPose, swerve::getRobotRelativeVelocity))) + .deadlineWith(shooterCmds.preparePivotCommandAuto(swerve::getPose, swerve::getRobotRelativeVelocity))); + + NamedCommands.registerCommand("FullPowerPreload2", + shooter.fullPower(2700) + .alongWith(Commands.waitUntil(pivot::getAtDesiredAngle)) + .andThen( + Commands.either( + pieceControl.noteToShootUsingSensor(swerve::getPose, swerve::getRobotRelativeVelocity), + Commands.waitUntil(() -> !operator.getYButton()), + () -> !FieldConstants.IS_SIMULATION + ) + ) + .deadlineWith(shooterCmds.preparePivotCommandAuto(swerve::getPose, swerve::getRobotRelativeVelocity))); + + NamedCommands.registerCommand("FullPowerPreload3", + shooter.fullPower(1678) // Say that again? + .alongWith(Commands.waitUntil(pivot::getAtDesiredAngle)) + .andThen( + Commands.either( + pieceControl.noteToShootUsingSensor(swerve::getPose, swerve::getRobotRelativeVelocity), + Commands.waitUntil(() -> !operator.getYButton()), + () -> !FieldConstants.IS_SIMULATION + ) + ) + .deadlineWith(shooterCmds.preparePivotCommandAuto(swerve::getPose, swerve::getRobotRelativeVelocity))); + NamedCommands.registerCommand("BoostShooterR", + shooter.fullPower(2500) + .raceWith(shooterCmds.preparePivotCommandAuto(swerve::getPose, swerve::getRobotRelativeVelocity))); registerPathToPathCommands(); } @@ -803,6 +906,10 @@ private Command enableVision() { return Commands.runOnce(() -> enableVision = true).ignoringDisable(true); } + public void set3GPriorityTagID(int id) { + limelight3g.setPriorityTagID(id); + } + private void registerPathToPathCommands() { for (int i = 1; i <= FieldConstants.CENTER_NOTE_COUNT; i++) { for (int j = 1; j <= FieldConstants.CENTER_NOTE_COUNT; j++) { @@ -811,36 +918,11 @@ private void registerPathToPathCommands() { } NamedCommands.registerCommand("C" + i + "toC" + j, pathPlannerStorage.generateCenterLogicOBJ(i, j, swerve, limelight3)); NamedCommands.registerCommand("C" + i + "toC" + j + "nonOBJ", pathPlannerStorage.generateCenterLogicNonOBJ(i, j, swerve)); + NamedCommands.registerCommand("C" + i + "toC" + j + "default", pathPlannerStorage.generateCenterLogicDefault(i, j, swerve)); } } } - private void configureLoggingPaths() { - Monologue.logObj(shooterCalc, "Robot/Math/shooterCalc"); - Monologue.logObj(calibrationControl, "Robot/Math/calibrationControl"); - Monologue.logObj(HDCTuner, "Robot/Math/HDCTuner"); - Monologue.logObj(pieceControl, "Robot/Math/PieceControl"); - - Monologue.logObj(swerve, "Robot/Swerve"); - - Monologue.logObj(intake, "Robot/Subsystems/intake"); - Monologue.logObj(climb, "Robot/Subsystems/climb"); - if (CameraConstants.FIELD_CALIBRATION_MODE) { - Monologue.logObj(limelightMapper, "Robot/Limelights/limelightMapper"); - } else { - Monologue.logObj(limelight3, "Robot/Limelights/limelight3"); - Monologue.logObj(limelight3g, "Robot/Limelights/limelight3g"); - } - Monologue.logObj(colorSensor, "Robot/ColorSensors/colorSensor"); - Monologue.logObj(shooter, "Robot/Subsystems/shooter"); - Monologue.logObj(elevator, "Robot/Subsystems/elevator"); - Monologue.logObj(pivot, "Robot/Subsystems/pivot"); - Monologue.logObj(ampper, "Robot/Subsystems/ampper"); - Monologue.logObj(pieceControl, "Robot/Managers/pieceControl"); - - Monologue.logObj(pathPlannerStorage, "Robot"); - } - /** * This is explained in detail in discussion #180 of the 2024 repository of 4738 * https://github.com/Patribots4738/Crescendo2024/discussions/180 diff --git a/src/main/java/frc/robot/commands/drive/AlignmentCmds.java b/src/main/java/frc/robot/commands/drive/AlignmentCmds.java index 075fcc70..561460b2 100644 --- a/src/main/java/frc/robot/commands/drive/AlignmentCmds.java +++ b/src/main/java/frc/robot/commands/drive/AlignmentCmds.java @@ -12,21 +12,20 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Robot; import frc.robot.commands.managers.ShooterCmds; -import frc.robot.subsystems.Climb; -import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.climb.Climb; +import frc.robot.subsystems.drive.Swerve; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.calc.AlignmentCalc; -import monologue.Annotations.IgnoreLogged; +import frc.robot.util.calc.PoseCalculations; public class AlignmentCmds { - - @IgnoreLogged + private Climb climb; - @IgnoreLogged + private Swerve swerve; private ShooterCmds shooterCmds; - @IgnoreLogged + public AlignmentCalc alignmentCalc; public AlignmentCmds(Swerve swerve, Climb climb, ShooterCmds shooterCmds) { @@ -75,6 +74,32 @@ public Command ampAlignmentCommand(DoubleSupplier driverX) { ); } + + public Command moveToPresetCommand() { + + return + getAutoAlignmentCommand( + alignmentCalc.getPresetAlignmentSpeedsSupplier(), + () -> + ChassisSpeeds.fromFieldRelativeSpeeds( + 0, + 0, + 0, + swerve.getPose().getRotation() + ) + ); + } + + + public Command moveAndPreparePresetCommand() { + return Commands.parallel( + moveToPresetCommand(), + shooterCmds.prepareFireCommand( + () ->PoseCalculations.getClosestShootingPose(swerve.getPose()).getTranslation(), + Robot::isRedAlliance) + ); + } + public Command ampRotationalAlignmentCommand(DoubleSupplier driverX, DoubleSupplier driverY) { return swerve.getDriveCommand( @@ -168,21 +193,32 @@ public Command wingRotationalAlignment(DoubleSupplier driverX, DoubleSupplier dr climb::getHooksUp); } + public Command preparePassCommand(DoubleSupplier driverX, DoubleSupplier driverY, BooleanSupplier robotRelativeSupplier, BooleanSupplier lowPass) { + return + passRotationalAlignment(driverX, driverY, shooterCmds) + .alongWith(shooterCmds.preparePassCommand(swerve::getPose, lowPass)); + } + public Command preparePassCommand(DoubleSupplier driverX, DoubleSupplier driverY, BooleanSupplier robotRelativeSupplier) { + return preparePassCommand(driverX, driverY, robotRelativeSupplier, () -> PoseCalculations.inLowPassZone(swerve.getPose())); + } + + public Command prepareSlidePassCommand(DoubleSupplier driverX, DoubleSupplier driverY, BooleanSupplier robotRelativeSupplier) { return passRotationalAlignment(driverX, driverY, shooterCmds) - .alongWith(shooterCmds.preparePassCommand(swerve::getPose)); + .alongWith(shooterCmds.prepareSlidePassCommand()); + } public Command preparePresetPose(DoubleSupplier driverX, DoubleSupplier driverY, boolean xButtonPressed) { // The true condition represents the pose closer to C1 // Which is to the left of the driver return Commands.either( - prepareFireAndRotateCommand(driverX, driverY, FieldConstants.ORBIT_POSE), - prepareFireAndRotateCommand(driverX, driverY, FieldConstants.PODIUM_SHOT_SPOT), + prepareFireAndRotateCommand(driverX, driverY, FieldConstants.ORBIT_POSE.getTranslation()), + prepareFireAndRotateCommand(driverX, driverY, FieldConstants.PODIUM_SHOT_SPOT.getTranslation()), // No way I just found my first XOR use case :D () -> Robot.isRedAlliance() ^ xButtonPressed - ); + ); } private Command prepareFireAndRotateCommand(DoubleSupplier driverX, DoubleSupplier driverY, Translation2d pose) { diff --git a/src/main/java/frc/robot/commands/drive/ChasePose.java b/src/main/java/frc/robot/commands/drive/ChasePose.java index cab8e9a3..77cdd22d 100644 --- a/src/main/java/frc/robot/commands/drive/ChasePose.java +++ b/src/main/java/frc/robot/commands/drive/ChasePose.java @@ -10,7 +10,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.drive.Swerve; import frc.robot.util.Constants.AutoConstants; public class ChasePose extends Command { diff --git a/src/main/java/frc/robot/commands/drive/Drive.java b/src/main/java/frc/robot/commands/drive/Drive.java index a953a148..58117593 100644 --- a/src/main/java/frc/robot/commands/drive/Drive.java +++ b/src/main/java/frc/robot/commands/drive/Drive.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Robot; import frc.robot.Robot.GameMode; -import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.drive.Swerve; import frc.robot.util.Constants.DriveConstants; public class Drive extends Command { diff --git a/src/main/java/frc/robot/commands/drive/DriveHDC.java b/src/main/java/frc/robot/commands/drive/DriveHDC.java index a0cc7320..cf257ba6 100644 --- a/src/main/java/frc/robot/commands/drive/DriveHDC.java +++ b/src/main/java/frc/robot/commands/drive/DriveHDC.java @@ -10,7 +10,7 @@ //import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.commands.managers.HDCTuner; -import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.drive.Swerve; import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.DriveConstants; diff --git a/src/main/java/frc/robot/commands/drive/WheelRadiusCharacterization.java b/src/main/java/frc/robot/commands/drive/WheelRadiusCharacterization.java index b2f91d01..42189fc0 100644 --- a/src/main/java/frc/robot/commands/drive/WheelRadiusCharacterization.java +++ b/src/main/java/frc/robot/commands/drive/WheelRadiusCharacterization.java @@ -8,21 +8,17 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.drive.Swerve; import frc.robot.util.Constants.DriveConstants; -import monologue.Logged; -import monologue.Annotations.Log; import java.util.function.DoubleSupplier; // thanks 6328 :D <3 -public class WheelRadiusCharacterization extends Command implements Logged { +public class WheelRadiusCharacterization extends Command { // wheel radius (meters) = // gyro delta (radians) * drive base radius (meters) / wheel position delta // (radians) - @Log private double gyroDelta = 0.0, wheelPosDelta = 0.0, currentEffectiveWheelRadius = 0.0; - @Log private double lastGyroYawRads = 0.0, accumGyroYawRads = 0.0; private double[] startWheelPositions; diff --git a/src/main/java/frc/robot/commands/logging/NoteTrajectory.java b/src/main/java/frc/robot/commands/logging/NoteTrajectory.java index 658899d5..d2db21df 100644 --- a/src/main/java/frc/robot/commands/logging/NoteTrajectory.java +++ b/src/main/java/frc/robot/commands/logging/NoteTrajectory.java @@ -18,7 +18,6 @@ import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.NTConstants; import frc.robot.util.Constants.ShooterConstants; -import monologue.Logged; /** * This command represents a trajectory of a note on the field. @@ -26,7 +25,7 @@ * * We can use the resulting pose to display the note on the field, in order to create a more accurate representation of the field. */ -public class NoteTrajectory extends Command implements Logged { +public class NoteTrajectory extends Command { double x, x0, vx0, ax, z, z0, vz0, az, y, y0, vy0, ay; ChassisSpeeds initalSpeeds; diff --git a/src/main/java/frc/robot/commands/managers/CalibrationControl.java b/src/main/java/frc/robot/commands/managers/CalibrationControl.java index f8cfdae0..3ce4d3e7 100644 --- a/src/main/java/frc/robot/commands/managers/CalibrationControl.java +++ b/src/main/java/frc/robot/commands/managers/CalibrationControl.java @@ -1,25 +1,41 @@ package frc.robot.commands.managers; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import org.littletonrobotics.junction.AutoLogOutput; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.util.Constants.FieldConstants; import frc.robot.util.custom.SpeedAngleTriplet; -import monologue.Logged; -import monologue.Annotations.Log; -public class CalibrationControl implements Logged { +public class CalibrationControl { private SpeedAngleTriplet desiredTriplet = SpeedAngleTriplet.of(3311.0, 3034.0, 30.3); - @Log - private double leftSpeed = 3311, rightSpeed = 3034, angle = 30; + @AutoLogOutput (key = "Managers/CalibrationControl/LeftSpeed") + private double leftSpeed = 3311; + + @AutoLogOutput (key = "Managers/CalibrationControl/RightSpeed") + private double rightSpeed = 3034; + + @AutoLogOutput (key = "Managers/CalibrationControl/Angle") + private double angle = 30; - @Log - private boolean pivotLock, leftLock, rightLock; + @AutoLogOutput (key = "Managers/CalibrationControl/PivotLock") + private boolean pivotLock; + + @AutoLogOutput (key = "Managers/CalibrationControl/LeftLock") + private boolean leftLock; + + @AutoLogOutput (key = "Managers/CalibrationControl/RightLock") + private boolean rightLock; - @Log + @AutoLogOutput (key = "Managers/CalibrationControl/Distance") private double distance = 0; private ShooterCmds shooterCmds; @@ -28,19 +44,20 @@ public CalibrationControl(ShooterCmds shooterCmds) { this.shooterCmds = shooterCmds; } - public Command copyCalcTriplet() { + public Command copyCalcTriplet(Supplier poseSupplier) { return Commands.runOnce(() -> { - SpeedAngleTriplet triplet = shooterCmds.getTriplet(); + SpeedAngleTriplet triplet = shooterCmds.shooterCalc.calculateSpeakerTriplet(poseSupplier.get().getTranslation()); desiredTriplet = triplet; leftSpeed = triplet.getLeftSpeed(); rightSpeed = triplet.getRightSpeed(); angle = triplet.getAngle(); + System.out.println("Copied triplet for " + Math.round(Units.metersToFeet(poseSupplier.get().getTranslation().getDistance(FieldConstants.GET_SPEAKER_TRANSLATION()))) + " ft"); logSpeeds(); logAngle(); }); } - public Command incrementDistance(int increment) { + public Command incrementDistance(double increment) { return Commands.runOnce(() -> { distance += increment; logDistance(); @@ -119,11 +136,11 @@ public Command updateMotorsCommand() { public Command logTriplet() { return Commands.runOnce( - () -> System.out.println("put(" + (int) distance + ", SpeedAngleTriplet.of("+Math.round(desiredTriplet.getLeftSpeed())+".0, "+Math.round(desiredTriplet.getRightSpeed())+".0, "+desiredTriplet.getAngle()+"));")); + () -> System.out.println("put(" + (int)(distance * 10) / 10.0 + ", SpeedAngleTriplet.of("+Math.round(desiredTriplet.getLeftSpeed())+".0, "+Math.round(desiredTriplet.getRightSpeed())+".0, "+desiredTriplet.getAngle()+"));")); } public void logDistance() { - System.out.println("Distance: " + (int) distance + "ft"); + System.out.println("Distance: " + (int)(distance * 10) / 10.0 + "ft"); } public void logSpeeds() { diff --git a/src/main/java/frc/robot/commands/managers/HDCTuner.java b/src/main/java/frc/robot/commands/managers/HDCTuner.java index e6e2ead4..f5c3b942 100644 --- a/src/main/java/frc/robot/commands/managers/HDCTuner.java +++ b/src/main/java/frc/robot/commands/managers/HDCTuner.java @@ -1,5 +1,7 @@ package frc.robot.commands.managers; +import org.littletonrobotics.junction.AutoLogOutput; + import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -7,13 +9,13 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.Constants.AutoConstants; -import monologue.Annotations.Log; -import monologue.Logged; -public class HDCTuner extends SubsystemBase implements Logged { +public class HDCTuner extends SubsystemBase { - @Log - public int PIDControllerIndex = 0, PIDIndex = 0; + @AutoLogOutput (key = "Managers/HDCTuner/PIDControllerIndex") + public int PIDControllerIndex = 0; + @AutoLogOutput (key = "Managers/HDCTuner/PIDIndex") + public int PIDIndex = 0; public PIDController XYController = AutoConstants.HDC.getXController(); diff --git a/src/main/java/frc/robot/commands/managers/PieceControl.java b/src/main/java/frc/robot/commands/managers/PieceControl.java index c9d09e94..582a1536 100644 --- a/src/main/java/frc/robot/commands/managers/PieceControl.java +++ b/src/main/java/frc/robot/commands/managers/PieceControl.java @@ -4,10 +4,13 @@ import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; @@ -17,47 +20,44 @@ import frc.robot.Robot.GameMode; import frc.robot.RobotContainer; import frc.robot.commands.logging.NT; -import frc.robot.subsystems.ColorSensor; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Intake; -import frc.robot.subsystems.Ampper; +import frc.robot.subsystems.colorsensor.PicoColorSensor; +import frc.robot.subsystems.ampper.Elevator; +import frc.robot.subsystems.intake.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.ampper.Ampper; import frc.robot.util.Constants.FieldConstants; -import frc.robot.util.Constants.ShooterConstants; +import frc.robot.util.calc.PoseCalculations; +import frc.robot.util.Constants.DriveConstants; import frc.robot.util.Constants.ElevatorConstants; import frc.robot.util.custom.ActiveConditionalCommand; +import frc.robot.util.custom.SelectiveConditionalCommand; import frc.robot.util.custom.SpeedAngleTriplet; -import monologue.Annotations.IgnoreLogged; -import monologue.Annotations.Log; -import monologue.Logged; -public class PieceControl implements Logged { +public class PieceControl { - @IgnoreLogged private Intake intake; - @IgnoreLogged - private Indexer indexer; - @IgnoreLogged + private Indexer indexer; + private Elevator elevator; - @IgnoreLogged + private Ampper ampper; - @IgnoreLogged + private ShooterCmds shooterCmds; - - private ColorSensor colorSensor; + private PicoColorSensor piPico; - @Log + @AutoLogOutput (key = "Managers/PieceControl/ShooterMode") private boolean shooterMode = false; // State representing if we are trying to unstuck the elevator - @Log + @AutoLogOutput (key = "Managers/PieceControl/ElevatorDislodging") private boolean elevatorDislodging = false; - @Log + + @AutoLogOutput (key = "Managers/PieceControl/MovingNote") private boolean currentlyMovingNote = false; - @Log + @AutoLogOutput (key = "Managers/PieceControl/HasPiece") private boolean hasPiece = false; public PieceControl( @@ -66,13 +66,13 @@ public PieceControl( Elevator elevator, Ampper ampper, ShooterCmds shooterCmds, - ColorSensor colorSensor) { + PicoColorSensor piPico) { this.intake = intake; this.indexer = indexer; this.elevator = elevator; this.ampper = ampper; this.shooterCmds = shooterCmds; - this.colorSensor = colorSensor; + this.piPico = piPico; } public Command stopAllMotors() { @@ -99,8 +99,18 @@ public Command brakeIntakeAndIndexer() { // TODO: only run angle reset when we are not using prepareSWDCommand public Command shootWhenReady(Supplier poseSupplier, Supplier speedSupplier, BooleanSupplier atDesiredAngle) { - return Commands.waitUntil(() -> shooterCmds.shooterCalc.readyToShootSupplier().getAsBoolean() && atDesiredAngle.getAsBoolean()) - .andThen(noteToShoot(poseSupplier, speedSupplier)); + return + Commands.waitUntil(() -> + (PoseCalculations.inSpeakerShotZone(poseSupplier.get().getTranslation()) + ? shooterCmds.shooterCalc.readyToShootSupplier().getAsBoolean() + : shooterCmds.shooterCalc.readyToPassSupplier().getAsBoolean()) + && atDesiredAngle.getAsBoolean() + && Math.hypot( + speedSupplier.get().vxMetersPerSecond, + speedSupplier.get().vyMetersPerSecond) < DriveConstants.PASS_ROBOT_VELOCITY_THRESHOLD_METERS_PER_SECOND + ) + .andThen(noteToShoot(poseSupplier, speedSupplier)); + } public Command shootPreload() { @@ -113,6 +123,7 @@ public Command noteToShoot(Supplier poseSupplier, Supplier poseSupplier, Supplier poseSupplier, Supplier speedSupplier) { + public Command noteToShootUsingSensorWhenReady(Supplier poseSupplier, Supplier speedSupplier) { // this should be ran while we are aiming with pivot and shooter already // start running indexer so it gets up to speed and wait until shooter is at desired // rotation and speed before sending note from ampper into indexer and then into // shooter before stopping ampper and indexer return Commands.sequence( - intake.inCommand(), - ampper.intake(), - indexer.toShooter(), - shooterCmds.getNoteTrajectoryCommand(poseSupplier, speedSupplier), - Commands.waitUntil(() -> !colorSensor.hasNote()), - stopIntakeAndIndexer()); + Commands.waitUntil(shooterCmds.shooterCalc.readyToShootSupplier()), + noteToShootUsingSensor(poseSupplier, speedSupplier)); } - public Command noteToShootUsingSensorWhenReady(Supplier poseSupplier, Supplier speedSupplier) { + public Command noteToShootUsingSensor(Supplier poseSupplier, Supplier speedSupplier) { // this should be ran while we are aiming with pivot and shooter already // start running indexer so it gets up to speed and wait until shooter is at desired // rotation and speed before sending note from ampper into indexer and then into // shooter before stopping ampper and indexer return Commands.sequence( - Commands.waitUntil(shooterCmds.shooterCalc.readyToShootSupplier()), intake.inCommand(), ampper.intake(), indexer.toShooter(), shooterCmds.getNoteTrajectoryCommand(poseSupplier, speedSupplier), - Commands.waitUntil(() -> !colorSensor.hasNote() || FieldConstants.IS_SIMULATION), + Commands.waitUntil(() -> !piPico.hasNoteShooter() || FieldConstants.IS_SIMULATION), stopIntakeAndIndexer()); } @@ -161,7 +167,7 @@ public Command intakeUntilNote() { intake.inCommand(), ampper.intake(), indexer.toShooterSlow(), - Commands.waitUntil(colorSensor::hasNote), + Commands.waitUntil(piPico::hasNoteShooter), stopIntakeAndIndexer(), indexer.toElevatorSlow(), ampper.outtakeSlow(), @@ -174,15 +180,19 @@ public Command intakeUntilNote() { public Command intakeForDoubleAmp() { return Commands.sequence( - intake.inCommandSlow(.45), + intake.inCommandSlow(.85), ampper.intakeSlow(), - indexer.toShooterSlow(), + indexer.stopCommand(), Commands.runOnce(this::restartDoubleAmpTimer) ); } public Command doubleAmpElevatorEnd() { - return Commands.either(elevatorToTop(), elevatorToBottom(), this::doubleAmpTimerReady); + return Commands.either(elevatorUpWhileIntaking(), elevatorToBottom(), this::doubleAmpTimerReady); + } + + public Command elevatorUpWhileIntaking() { + return elevator.toTopCommand().deadlineWith(intake.inCommandFor(.5)).andThen(intake.stopCommand()); } public Command blepNote() { @@ -192,7 +202,7 @@ public Command blepNote() { indexer.toShooter(), shooterCmds.setTripletCommand(new SpeedAngleTriplet(500, 500, 0)) ).andThen( - Commands.waitUntil(() -> !colorSensor.hasNote()), + Commands.waitUntil(() -> !piPico.hasNoteShooter()), stopAllMotors() ); } @@ -207,7 +217,7 @@ public Command intakeNoteDriver(Supplier poseSupplier, Supplier !colorSensor.hasNote()), + Commands.waitUntil(() -> !piPico.hasNoteShooter()), () -> FieldConstants.IS_SIMULATION), NT.getWaitCommand("noteToTrap1"), // 0.2 stopIntakeAndIndexer(), @@ -238,6 +248,29 @@ public Command noteToTrap() { ); } + public Command noteToTrap2() { + return Commands.sequence( + ampper.setPercentCommand(-.7), + shooterCmds.stowPivot(), + indexer.setPercentCommand(.7), + intake.inCommandSlow(), + Commands.either( + Commands.waitSeconds(.1), + Commands.waitUntil(() -> !piPico.hasNoteShooter()), + () -> FieldConstants.IS_SIMULATION), + Commands.waitSeconds(.1), + Commands.either( + Commands.waitSeconds(.1), + Commands.waitUntil(() -> piPico.fallingEdgeHasNoteElevator()), + () -> FieldConstants.IS_SIMULATION).withTimeout(.254), + stopIntakeAndIndexer() + ); + } + + public Command noteToTrap3() { + return Commands.either(noteToTrap2(), noteToTrap(), () -> piPico.shooterSensorConnected() && piPico.elevatorSensorConnected()); + } + public Command ejectNote() { //outtakes the note return Commands.sequence( @@ -251,7 +284,7 @@ public Command ejectNote() { public Command stopEjecting() { return Commands.parallel( - elevator.toBottomCommand(), + elevatorToBottomSafe(), stopAllMotors() ); } @@ -281,16 +314,20 @@ public Command noteToTarget(Supplier poseSupplier, Supplier elevator.getDesiredPosition() > 0) + Commands.waitUntil(() -> elevator.getDesiredPosition() > ElevatorConstants.BOTTOM_POS) ), Commands.race( placeWhenReady() - .andThen(setHasPiece(false)), - Commands.waitUntil(() -> elevator.getDesiredPosition() <= 0) - ), - () -> elevator.getDesiredPosition() <= 0 - ).withInterruptBehavior(InterruptionBehavior.kCancelSelf) - .andThen(Commands.defer(() -> intakeUntilNote().onlyIf(operatorWantsToIntake), intakeUntilNote().getRequirements())); + .andThen(setHasPiece(false)) + ).andThen(ampper.stopCommand()), + () -> elevator.getDesiredPosition() <= ElevatorConstants.BOTTOM_POS + ) + .withInterruptBehavior(InterruptionBehavior.kCancelSelf) + .raceWith(Commands.waitSeconds(4)) + .andThen( + Commands.defer( + () -> intakeUntilNote().onlyIf(operatorWantsToIntake), + intakeUntilNote().getRequirements())); } private Command setDislodging(boolean dislodging) { @@ -343,25 +380,27 @@ public Command elevatorToPlacement(boolean povLeftPosition) { Commands.sequence( stopIntakeAndIndexer(), Commands.either( - setElevatorPosition(() -> ElevatorConstants.NOTE_FIX_POS), - setElevatorPosition(() -> ElevatorConstants.TRAP_PLACE_POS), - () -> povLeftPosition), + elevatorToNoteFixSafe(), + elevatorToTopSafe(), + () -> povLeftPosition), placeWhenReady().onlyIf(this::shouldPlaceWhenReady)); } public Command placeWhenReady() { return new SelectiveConditionalCommand( - ampper.outtake(NT.getValue("placeOuttake")) - .andThen( - elevatorToBottom() - .alongWith(shooterCmds.raisePivot()) - ), + Commands.sequence( + ampper.outtake(NT.getValue("placeOuttake")), + ampper.stopCommand(), + setPlaceWhenReadyCommand(false), + elevatorToBottomSafe(), + shooterCmds.raisePivot() + ), setPlaceWhenReadyCommand(true), elevator::atDesiredPosition); } - @Log + @AutoLogOutput (key = "Managers/PieceControl/PlaceWhenReady") private boolean placeWhenReady = false; public boolean shouldPlaceWhenReady() { @@ -377,7 +416,7 @@ public Command sourceShooterIntake() { shooterCmds.sourceIntakeCommand(), indexer.toElevator(), ampper.outtake(), - Commands.waitUntil(colorSensor::hasNote), + Commands.waitUntil(piPico::hasNoteShooter), Commands.waitSeconds(0.1), shooterCmds.stopShooter(), stopIntakeAndIndexer(), @@ -407,7 +446,7 @@ public Command setCurrentlyMovingNote(boolean currentlyMovingNote) { return Commands.runOnce(() -> this.currentlyMovingNote = currentlyMovingNote); } - @Log + @AutoLogOutput (key = "Managers/PieceControl/DesiredSide") private boolean desiredSide = false; public Command moveNote(boolean desiredSide) { @@ -415,7 +454,7 @@ public Command moveNote(boolean desiredSide) { Commands.runOnce(() -> this.desiredSide = desiredSide), setCurrentlyMovingNote(true), Commands.either( - noteToIndexer(), + intakeUntilNote(), noteToTrap(), () -> desiredSide)) .finallyDo(() -> { @@ -442,24 +481,48 @@ public Command setShooterModeCommand(boolean newShooterMode) { ); } + public Command moveNoteThenElevator() { + return Commands.sequence( + moveNote(false), + elevator.toTopCommand(), + prepPiece()); + } + // Within a range of the [red circle](https://www.desmos.com/calculator/cu3ocssv5d) - public Command getAutomaticShooterSpeeds(Supplier robotPose, BooleanSupplier intaking) { - return new ActiveConditionalCommand( - Commands.runOnce( - () -> shooterCmds.setSpeeds(ShooterConstants.DEFAULT_RPM), - shooterCmds.getShooter() - ), - shooterCmds.stopShooter(), - () -> - (((colorSensor.hasNote() - && RobotContainer.distanceToSpeakerMeters < FieldConstants.AUTOMATIC_SHOOTER_DISTANCE_RADIUS) - || (RobotContainer.distanceToSpeakerMeters < 3.4 && intaking.getAsBoolean() && elevator.getDesiredPosition() <= 0.1)) - - || (Robot.currentTimestamp - RobotContainer.gameModeStart < 7 - && Robot.gameMode == GameMode.TELEOP - && DriverStation.isFMSAttached())) - && RobotController.getBatteryVoltage() > 10) - .onlyIf(() -> Robot.gameMode != GameMode.TEST); + public Command getAutomaticShooterSpeeds(Supplier robotPose, BooleanSupplier intaking, BooleanSupplier manualActivation) { + return + new ActiveConditionalCommand( + Commands.run( + () -> + { + SpeedAngleTriplet desiredTriplet = + PoseCalculations.inSpeakerShotZone(robotPose.get().getTranslation()) + && Robot.currentTimestamp - RobotContainer.gameModeStart >= 7 + || Robot.gameMode == GameMode.AUTONOMOUS + ? shooterCmds.shooterCalc.calculateSpeakerTriplet(robotPose.get().getTranslation()) + : shooterCmds.shooterCalc.calculatePassTriplet(robotPose.get()); + Pair dividedSpeeds = + Pair.of(desiredTriplet.getLeftSpeed() * 0.7, desiredTriplet.getRightSpeed() * 0.7); + shooterCmds.setSpeeds(dividedSpeeds); + }, + shooterCmds.getShooter() + ), + shooterCmds.stopShooter(), + () -> + ((((piPico.hasNoteShooter() || piPico.hasNoteElevator() + && RobotContainer.distanceToSpeakerMeters < FieldConstants.AUTOMATIC_SHOOTER_DISTANCE_RADIUS_METERS) + || (RobotContainer.distanceToSpeakerMeters < FieldConstants.SPEAKER_CLEANUP_DISTANCE_METERS + && intaking.getAsBoolean() + && elevator.getDesiredPosition() < ElevatorConstants.NOTE_FIX_POS)) + + && (FieldConstants.IS_SIMULATION || piPico.shooterSensorConnected())) + + || (Robot.currentTimestamp - RobotContainer.gameModeStart < 7 + && Robot.gameMode == GameMode.TELEOP + && DriverStation.isFMSAttached()) + + || manualActivation.getAsBoolean())) + .onlyIf(() -> Robot.gameMode != GameMode.TEST); } private Timer doubleAmpTimer = new Timer(); @@ -470,4 +533,25 @@ public void restartDoubleAmpTimer() { public boolean doubleAmpTimerReady() { return doubleAmpTimer.get() > 0.35; } + + public Command setElevatorPositionSafe(DoubleSupplier position) { + return Commands.sequence( + shooterCmds.stowPivot(), + // shooterCmds.lockPivot(), + elevator.setPositionCommand(position) + // shooterCmds.unlockPivot() + ).withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + } + + public Command elevatorToTopSafe() { + return setElevatorPositionSafe(() -> ElevatorConstants.ELEVATOR_TOP_LIMIT); + } + + public Command elevatorToBottomSafe() { + return setElevatorPositionSafe(() -> ElevatorConstants.BOTTOM_POS); + } + + public Command elevatorToNoteFixSafe() { + return setElevatorPositionSafe(() -> ElevatorConstants.NOTE_FIX_POS); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/managers/ShooterCmds.java b/src/main/java/frc/robot/commands/managers/ShooterCmds.java index 3e31ba84..5b3c7fd4 100644 --- a/src/main/java/frc/robot/commands/managers/ShooterCmds.java +++ b/src/main/java/frc/robot/commands/managers/ShooterCmds.java @@ -12,19 +12,18 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.commands.logging.NoteTrajectory; -import frc.robot.subsystems.Pivot; -import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.shooter.Pivot; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.ShooterConstants; +import frc.robot.util.calc.PoseCalculations; import frc.robot.util.calc.ShooterCalc; import frc.robot.util.custom.SpeedAngleTriplet; -import monologue.Annotations.IgnoreLogged; public class ShooterCmds { - @IgnoreLogged private Pivot pivot; - @IgnoreLogged + private Shooter shooter; private SpeedAngleTriplet desiredTriplet = new SpeedAngleTriplet(); @@ -137,21 +136,41 @@ public Command prepareSWDCommandAuto(Supplier robotPose, Supplier robotPose, Supplier speeds) { + return Commands.run( + () -> { + desiredTriplet = shooterCalc.calculateSWDTripletAuto(robotPose.get(), speeds.get()); + pivot.setAngle(desiredTriplet.getAngle()); + }, pivot); + } + public Command prepareStillSpeakerCommand(Supplier robotPose) { return prepareSWDCommand(robotPose, () -> new ChassisSpeeds()); } - public Command preparePassCommand(Supplier robotPose) { + public Command preparePassCommand(Supplier robotPose, BooleanSupplier lowPass) { return Commands.run( () -> { - desiredTriplet = shooterCalc.calculatePassTriplet(robotPose.get()); + desiredTriplet = shooterCalc.calculatePassTriplet(robotPose.get(), lowPass.getAsBoolean()); pivot.setAngle(desiredTriplet.getAngle()); shooter.setSpeed(desiredTriplet.getSpeeds()); }, pivot, shooter); } + public Command preparePassCommand(Supplier robotPose) { + return preparePassCommand(robotPose, () -> PoseCalculations.inLowPassZone(robotPose.get())); + } + + public Command prepareSlidePassCommand() { + return Commands.run( + () -> { + pivot.setAngle(ShooterConstants.PIVOT_LOWER_LIMIT_DEGREES); + shooter.setSpeed(ShooterConstants.SLIDE_PASS_AVERAGE_RPM); + }, pivot, shooter); + } + public Command prepareSubwooferCommand() { - return setTripletCommand(shooterCalc.calculateSpeakerTriplet(FieldConstants.GET_SUBWOOFER_POSITION().getTranslation())); + return prepareStillSpeakerCommand(FieldConstants::GET_SUBWOOFER_POSITION); } public Command getNoteTrajectoryCommand(Supplier pose, Supplier speeds) { diff --git a/src/main/java/frc/robot/leds/Commands/LPI.java b/src/main/java/frc/robot/leds/Commands/LPI.java index 93f7270d..f0da71db 100644 --- a/src/main/java/frc/robot/leds/Commands/LPI.java +++ b/src/main/java/frc/robot/leds/Commands/LPI.java @@ -6,6 +6,8 @@ import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; + import com.pathplanner.lib.util.GeometryUtil; import edu.wpi.first.math.MathUtil; @@ -21,24 +23,23 @@ import frc.robot.util.Constants.LEDConstants; import frc.robot.util.auto.PathPlannerStorage; import frc.robot.util.custom.PatriBoxController; -import monologue.Logged; import java.util.function.Consumer; -import monologue.Annotations.Log; //https://www.tldraw.com/r/borcubmSklQQYMLikl6YZ?viewport=-1638,-855,3094,1889&page=page:page -public class LPI extends Command implements Logged{ +public class LPI extends Command { /** Creates a new LPI. */ private final LedStrip ledStrip; private final Supplier positionSupplier; private int activeRotationalLED = 0; private PatriBoxController xboxController; + @AutoLogOutput (key = "Draggables/ClosestAutoPose") private Pose2d closestPose; + Pose2d currentRobotPosition; Translation2d currentRobotTranslation; private final Consumer poseConsumer; - - @Log + Pose2d cardinalMagnitude; public LPI(LedStrip ledStrip, Supplier positionSupplier, PatriBoxController xboxController, Consumer consumer) { diff --git a/src/main/java/frc/robot/subsystems/ColorSensor.java b/src/main/java/frc/robot/subsystems/ColorSensor.java deleted file mode 100644 index 8cc4a435..00000000 --- a/src/main/java/frc/robot/subsystems/ColorSensor.java +++ /dev/null @@ -1,59 +0,0 @@ -package frc.robot.subsystems; - -import com.revrobotics.ColorMatchResult; -import com.revrobotics.ColorSensorV3; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Robot; -import frc.robot.util.Constants.ColorSensorConstants; -import frc.robot.util.Constants.FieldConstants; -import monologue.Logged; -import monologue.Annotations.Log; - -public class ColorSensor extends SubsystemBase implements Logged { - - private ColorSensorV3 colorSensor; - - @Log - private boolean hasNote = false; - - private Color detectedColor = new Color(0.0, 0.0, 0.0); - - @Log - private String colorString = ""; - - private ColorMatchResult match = ColorSensorConstants.COLOR_MATCH.matchClosestColor(detectedColor); - - @Log - private String matchString = ""; - - public ColorSensor(I2C.Port i2cPort) { - this.colorSensor = new ColorSensorV3(i2cPort); - } - - @Override - public void periodic() { - this.detectedColor = colorSensor.getColor(); - this.match = ColorSensorConstants.COLOR_MATCH.matchClosestColor(detectedColor); - - colorString = detectedColor.toString(); - if (FieldConstants.IS_SIMULATION) { - hasNote = (int) Robot.currentTimestamp/5 % 2 == 0; - matchString = "Simulation"; - return; - } - if (this.match.color == ColorSensorConstants.NOTE_ORANGE) { - matchString = "Note"; - hasNote = true; - } else { - matchString = "Surely not a note"; - hasNote = false; - } - } - - public boolean hasNote() { - return this.hasNote; - } -} diff --git a/src/main/java/frc/robot/subsystems/Ampper.java b/src/main/java/frc/robot/subsystems/ampper/Ampper.java similarity index 61% rename from src/main/java/frc/robot/subsystems/Ampper.java rename to src/main/java/frc/robot/subsystems/ampper/Ampper.java index 4f6ed32e..559ddfe8 100644 --- a/src/main/java/frc/robot/subsystems/Ampper.java +++ b/src/main/java/frc/robot/subsystems/ampper/Ampper.java @@ -1,4 +1,6 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.ampper; + +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; @@ -6,69 +8,69 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.Constants.ElevatorConstants; import frc.robot.util.rev.Neo; -import monologue.Logged; -import monologue.Annotations.Log; -public class Ampper extends SubsystemBase implements Logged { +public class Ampper extends SubsystemBase implements AmpperIO { private final Neo motor; - - @Log - private double desiredSpeed = 0; + private final AmpperIOInputsAutoLogged inputs = new AmpperIOInputsAutoLogged(); public Ampper() { motor = new Neo(ElevatorConstants.AMPPER_CAN_ID, false, ElevatorConstants.AMPPER_INVERTION); motor.setSmartCurrentLimit(ElevatorConstants.AMPPER_CURRENT_LIMIT); } + @Override + public void periodic() { + updateInputs(inputs); + Logger.processInputs("SubsystemInputs/Ampper", inputs); + } + public Command intakeFromHandoff() { return intake() .andThen(Commands.waitSeconds(ElevatorConstants.INTAKE_TIME)) .andThen(stopCommand()); } - public void setSpeed(double percent) { + public void setPercent(double percent) { percent = MathUtil.clamp( percent, ElevatorConstants.AMPPER_LOWER_PERCENT_LIMIT, ElevatorConstants.AMPPER_UPPER_PERCENT_LIMIT); - if (desiredSpeed != percent) { - desiredSpeed = percent; - + if (inputs.targetPercent != percent) { motor.set(percent); } } public Command outtake() { - return runOnce(() -> setSpeed(ElevatorConstants.AMPPER_OUTTAKE_PERCENT)); + return runOnce(() -> setPercent(ElevatorConstants.AMPPER_OUTTAKE_PERCENT)); } public Command outtakeSlow() { - return runOnce(() -> setSpeed(ElevatorConstants.AMPPER_OUTTAKE_SLOW)); + return runOnce(() -> setPercent(ElevatorConstants.AMPPER_OUTTAKE_SLOW)); } public Command outtakeSlow(double seconds) { return Commands.sequence( - intakeSlow(), + outtakeSlow(), Commands.waitSeconds(seconds), stopCommand()); } public Command intakeSlow() { - return runOnce(() -> setSpeed(0.4)); + return runOnce(() -> setPercent(0.4)); } public Command intakeSlow(double speed) { - return runOnce(() -> setSpeed(speed)); + return runOnce(() -> setPercent(speed)); } public Command stopCommand() { - return runOnce(() -> setSpeed(ElevatorConstants.AMPPER_STOP_PERCENT)); + return runOnce(() -> setPercent(ElevatorConstants.AMPPER_STOP_PERCENT)); } public Command intake() { - return runOnce(() -> setSpeed(ElevatorConstants.AMPPER_INTAKE_PERCENT)); + return runOnce(() -> setPercent(ElevatorConstants.AMPPER_INTAKE_PERCENT)); } public Command intake(double seconds) { @@ -91,7 +93,11 @@ public Command toggleSpeed() { return Commands.either( outtakeSlow(), stopCommand(), - () -> desiredSpeed == 0); + () -> inputs.targetPercent == 0); + } + + public Command setPercentCommand(double percent) { + return runOnce(() -> setPercent(percent)); } public Command setCoastMode() { @@ -101,4 +107,11 @@ public Command setCoastMode() { public Command setBrakeMode() { return Commands.runOnce(() -> motor.setBrakeMode()).ignoringDisable(true); } + + @Override + public void updateInputs(AmpperIOInputs inputs) { + inputs.targetPercent = motor.getTargetPercent(); + inputs.appliedVolts = motor.getAppliedOutput(); + inputs.outputCurrentAmps = motor.getOutputCurrent(); + } } diff --git a/src/main/java/frc/robot/subsystems/ampper/AmpperIO.java b/src/main/java/frc/robot/subsystems/ampper/AmpperIO.java new file mode 100644 index 00000000..b1829431 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ampper/AmpperIO.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.ampper; + +import org.littletonrobotics.junction.AutoLog; + +public interface AmpperIO { + + @AutoLog + class AmpperIOInputs { + + public double targetPercent = 0.0; + public double appliedVolts = 0.0; + public double outputCurrentAmps = 0.0; + + } + + default void updateInputs(AmpperIOInputs inputs) {} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/ampper/Elevator.java similarity index 73% rename from src/main/java/frc/robot/subsystems/Elevator.java rename to src/main/java/frc/robot/subsystems/ampper/Elevator.java index ebc0ef3d..ec564f2b 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/ampper/Elevator.java @@ -1,7 +1,10 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.ampper; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; @@ -13,22 +16,25 @@ import frc.robot.util.Constants.ElevatorConstants; import frc.robot.util.rev.Neo; import frc.robot.Robot; -import monologue.Logged; -import monologue.Annotations.Log; -public class Elevator extends SubsystemBase implements Logged { +public class Elevator extends SubsystemBase implements ElevatorIO { private final Neo motor; + private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + + private double hitGuillotineTimestamp = 0; - @Log - private double position = 0, desiredPos = 0, hitGuillotineTimestamp = 0, desiredOverridePos = 0; + @AutoLogOutput(key = "Subsystems/Elevator/DesiredOverridePosition") + private double desiredOverridePos = 0; - @Log + @AutoLogOutput(key = "Subsystems/Elevator/OverrideMode") private boolean overrideMode = false; - @Log - private boolean atDesiredPos = false, stuckOnGuillotine = false; + @AutoLogOutput(key = "Subsystems/Elevator/AtDesiredPosition") + private boolean atDesiredPos = false; + + private boolean stuckOnGuillotine = false; /** Creates a new Elevator. */ public Elevator() { @@ -44,7 +50,9 @@ public void configMotors() { @Override public void periodic() { - position = motor.getPosition(); + updateInputs(inputs); + Logger.processInputs("SubsystemInputs/Elevator", inputs); + atDesiredPos = atDesiredPosition(); // A value of 0 on hitGuillotineTimestamp indicates that we are not hitting the guillotine @@ -57,21 +65,21 @@ else if (!nearGuillotine() && hitGuillotineTimestamp != 0) updateStuck(); RobotContainer.components3d[NTConstants.AMPPER_INDEX] = new Pose3d( - 0, 0, position * ElevatorConstants.AMPPER_POSITION_MULTIPLIER, + 0, 0, inputs.positionRotations * ElevatorConstants.AMPPER_POSITION_MULTIPLIER, new Rotation3d() ); RobotContainer.components3d[NTConstants.ELEVATOR_INDEX] = new Pose3d( - 0, 0, position, + 0, 0, inputs.positionRotations, new Rotation3d() ); } public double getPosition() { - return position; + return inputs.positionRotations; } public double getDesiredPosition() { - return desiredPos; + return inputs.targetPositionMeters; } public void setPosition(double pos) { @@ -80,8 +88,7 @@ public void setPosition(double pos) { ElevatorConstants.ELEVATOR_BOTTOM_LIMIT, ElevatorConstants.ELEVATOR_TOP_LIMIT); - if (desiredPos != pos) { - desiredPos = pos; + if (inputs.targetPositionMeters != pos) { motor.setTargetPosition(pos); RobotContainer.desiredComponents3d[NTConstants.ELEVATOR_INDEX] = new Pose3d( @@ -139,12 +146,12 @@ public Command toDropCommand() { public boolean atDesiredPosition() { if (overrideMode) - return MathUtil.isNear(desiredOverridePos, position, ElevatorConstants.ELEVATOR_DEADBAND); - return MathUtil.isNear(desiredPos, position, ElevatorConstants.ELEVATOR_DEADBAND); + return MathUtil.isNear(desiredOverridePos, inputs.positionRotations, ElevatorConstants.ELEVATOR_DEADBAND); + return MathUtil.isNear(inputs.targetPositionMeters, inputs.positionRotations, ElevatorConstants.ELEVATOR_DEADBAND); } public boolean atPosition(double position) { - return MathUtil.isNear(position, this.position, ElevatorConstants.ELEVATOR_DEADBAND); + return MathUtil.isNear(position, inputs.positionRotations, ElevatorConstants.ELEVATOR_DEADBAND); } public boolean nearGuillotine() { @@ -155,14 +162,18 @@ public boolean getStuck() { return stuckOnGuillotine; } + public boolean isUp() { + return inputs.targetPositionMeters > 0; + } + // If we are trying to get up but we have been near guillotine for more than or equal to 0.1s then we are stuck public void updateStuck() { stuckOnGuillotine = hitGuillotineTimestamp != 0 && Robot.currentTimestamp - this.hitGuillotineTimestamp >= ElevatorConstants.STUCK_TIME_SECONDS - && desiredPos > position + && inputs.targetPositionMeters > inputs.positionRotations && nearGuillotine() - && motor.getOutputCurrent() > 35.0; + && inputs.outputCurrentAmps > 35.0; } public Command resetEncoder() { @@ -181,4 +192,13 @@ public Command overrideCommand(DoubleSupplier doubleSupplier) { .beforeStarting(Commands.runOnce(() -> overrideMode = true)) .finallyDo(() -> overrideMode = false); } + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + inputs.positionRotations = motor.getPosition(); + inputs.targetPositionMeters = motor.getTargetPosition(); + inputs.appliedVolts = motor.getAppliedOutput(); + inputs.outputCurrentAmps = motor.getOutputCurrent(); + } + } diff --git a/src/main/java/frc/robot/subsystems/ampper/ElevatorIO.java b/src/main/java/frc/robot/subsystems/ampper/ElevatorIO.java new file mode 100644 index 00000000..0561c90f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ampper/ElevatorIO.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.ampper; + +import org.littletonrobotics.junction.AutoLog; + +public interface ElevatorIO { + + @AutoLog + class ElevatorIOInputs { + + public double positionRotations = 0.0; + public double targetPositionMeters = 0.0; + public double appliedVolts = 0.0; + public double outputCurrentAmps = 0.0; + + } + + default void updateInputs(ElevatorIOInputs inputs) {} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java similarity index 57% rename from src/main/java/frc/robot/subsystems/Climb.java rename to src/main/java/frc/robot/subsystems/climb/Climb.java index d37a367d..36bffc2f 100644 --- a/src/main/java/frc/robot/subsystems/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -1,11 +1,12 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.climb; import java.util.function.BooleanSupplier; -import java.util.function.Supplier; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.wpilibj2.command.Command; @@ -15,21 +16,23 @@ import frc.robot.util.Constants.ClimbConstants; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.NTConstants; -import frc.robot.util.calc.PoseCalculations; import frc.robot.util.rev.Neo; -import monologue.Logged; -import monologue.Annotations.Log; -public class Climb extends SubsystemBase implements Logged { +public class Climb extends SubsystemBase implements ClimbIO { private final Neo leftMotor; private final Neo rightMotor; - @Log - private double posLeft = 0, posRight = 0, targetPosRight = 0, targetPosLeft = 0; + private final ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); + + @AutoLogOutput(key = "Subsystems/Climb/AtDesiredPosition") + private boolean atDesiredPos = false; + + @AutoLogOutput(key = "Subsystems/Climb/HooksUp") + private boolean hooksUp = false; - @Log - private boolean atDesiredPos = false, hooksUp = false; + @AutoLogOutput(key = "Subsystems/Climb/ToggleMode") + private boolean toggleMode = false; public Climb() { leftMotor = new Neo(ClimbConstants.LEFT_CLIMB_CAN_ID, true); @@ -52,20 +55,27 @@ private void configureMotors() { @Override public void periodic() { - targetPosLeft = leftMotor.getTargetPosition(); - targetPosRight = rightMotor.getTargetPosition(); - posLeft = leftMotor.getPosition(); - posRight = rightMotor.getPosition(); - atDesiredPos = atDesiredPosition().getAsBoolean(); + updateInputs(inputs); + Logger.processInputs("SubsystemInputs/Climb", inputs); + + atDesiredPos = ( + MathUtil.applyDeadband( + Math.abs( + inputs.leftPositionMeters - inputs.leftTargetPositionMeters), + ClimbConstants.CLIMB_DEADBAND) == 0 && + MathUtil.applyDeadband( + Math.abs( + inputs.rightPositionMeters - inputs.rightTargetPositionMeters), + ClimbConstants.CLIMB_DEADBAND) == 0); hooksUp = getHooksUp(); RobotContainer.components3d[NTConstants.LEFT_CLIMB_INDEX] = new Pose3d( - 0, 0, leftMotor.getPosition(), + 0, 0, inputs.leftPositionMeters, new Rotation3d() ); RobotContainer.components3d[NTConstants.RIGHT_CLIMB_INDEX] = new Pose3d( - 0, 0, rightMotor.getPosition(), + 0, 0, inputs.rightPositionMeters, new Rotation3d() ); } @@ -90,13 +100,13 @@ public void setPosition(double pos1, double pos2) { ClimbConstants.BOTTOM_LIMIT, ClimbConstants.TOP_LIMIT); - if (targetPosLeft != pos1) { + if (inputs.leftTargetPositionMeters != pos1) { leftMotor.setTargetPosition(pos1); } - if (targetPosRight != pos2) - - rightMotor.setTargetPosition(pos2); + if (inputs.rightTargetPositionMeters != pos2) { + rightMotor.setTargetPosition(pos2); + } RobotContainer.desiredComponents3d[NTConstants.LEFT_CLIMB_INDEX] = new Pose3d( 0, 0, pos1, @@ -125,29 +135,59 @@ public Command toMiddleCommand() { } public Command toBottomCommand() { - return runOnce(() -> setPosition(ClimbConstants.BOTTOM_LIMIT)); + return runOnce(() -> setPosition(ClimbConstants.BOTTOM_LIMIT)).alongWith(setToggleMode(false)); } public Command povUpCommand() { return Commands.either( toMiddleCommand(), toTopCommand(), - () -> targetPosLeft == ClimbConstants.TOP_LIMIT || targetPosRight == ClimbConstants.BOTTOM_LIMIT); + () -> inputs.leftTargetPositionMeters == ClimbConstants.TOP_LIMIT || inputs.rightTargetPositionMeters == ClimbConstants.BOTTOM_LIMIT); + } + + public Command toggleCommand() { + return Commands.either( + toTopCommand().alongWith(setToggleMode(true)), + toBottomCommand(), + () -> !hooksUp); } public BooleanSupplier atDesiredPosition() { return () -> ( MathUtil.applyDeadband( Math.abs( - leftMotor.getPosition() - leftMotor.getTargetPosition()), + inputs.leftPositionMeters - inputs.leftTargetPositionMeters), ClimbConstants.CLIMB_DEADBAND) == 0 && MathUtil.applyDeadband( Math.abs( - rightMotor.getPosition() - rightMotor.getTargetPosition()), + inputs.rightPositionMeters - inputs.rightTargetPositionMeters), ClimbConstants.CLIMB_DEADBAND) == 0); } public boolean getHooksUp() { - return (leftMotor.getTargetPosition() > 0 || rightMotor.getTargetPosition() > 0); + return (inputs.leftTargetPositionMeters > 0 || inputs.rightTargetPositionMeters > 0); + } + + public boolean getToggleMode() { + return toggleMode; + } + + public Command setToggleMode(boolean newMode) { + return Commands.runOnce(() -> toggleMode = newMode); + } + + @Override + public void updateInputs(ClimbIOInputs inputs) { + + inputs.leftPositionMeters = leftMotor.getPosition(); + inputs.leftTargetPositionMeters = leftMotor.getTargetPosition(); + inputs.leftAppliedVolts = leftMotor.getAppliedOutput(); + inputs.leftOutputCurrentAmps = leftMotor.getOutputCurrent(); + + inputs.rightPositionMeters = rightMotor.getPosition(); + inputs.rightTargetPositionMeters = rightMotor.getTargetPosition(); + inputs.rightAppliedVolts = rightMotor.getAppliedOutput(); + inputs.rightOutputCurrentAmps = rightMotor.getOutputCurrent(); + } } diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java new file mode 100644 index 00000000..0d12f9d6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.climb; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimbIO { + + @AutoLog + class ClimbIOInputs { + + public double leftPositionMeters = 0.0; + public double leftTargetPositionMeters = 0.0; + public double leftAppliedVolts = 0.0; + public double leftOutputCurrentAmps = 0.0; + + public double rightPositionMeters = 0.0; + public double rightTargetPositionMeters = 0.0; + public double rightAppliedVolts = 0.0; + public double rightOutputCurrentAmps = 0.0; + + } + + default void updateInputs(ClimbIOInputs inputs) {} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/colorsensor/PicoColorSensor.java b/src/main/java/frc/robot/subsystems/colorsensor/PicoColorSensor.java new file mode 100644 index 00000000..024b57c6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/colorsensor/PicoColorSensor.java @@ -0,0 +1,399 @@ +package frc.robot.subsystems.colorsensor; + +import java.nio.charset.StandardCharsets; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.locks.ReentrantLock; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +import com.revrobotics.ColorMatchResult; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.hal.SerialPortJNI; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.wpilibj.util.Color; +import frc.robot.Robot; +import frc.robot.util.Constants.ColorSensorConstants; +import frc.robot.util.custom.TransistorPower; + + +public class PicoColorSensor implements AutoCloseable, PicoColorSensorIO { + public static class RawColor { + public RawColor(int r, int g, int b, int _ir) { + red = r; + green = g; + blue = b; + ir = _ir; + } + + public RawColor() {} + + public int red; + public int green; + public int blue; + public int ir; + } + + private static class SingleCharSequence implements CharSequence { + public byte[] data; + + @Override + public int length() { + return data.length; + } + + @Override + public char charAt(int index) { + return (char) data[index]; + } + + @Override + public CharSequence subSequence(int start, int end) { + return new String(data, start, end, StandardCharsets.UTF_8); + } + + } + + private static class IntRef { int value; } + + int parseIntFromIndex(SingleCharSequence charSeq, int readLen, IntRef lastComma) { + int nextComma = 0; + try { + nextComma = findNextComma(charSeq.data, readLen, lastComma.value); + int value = Integer.parseInt(charSeq, lastComma.value + 1, nextComma, 10); + lastComma.value = nextComma; + return value; + } catch (Exception ex) { + return 0; + } + } + + private int findNextComma(byte[] data, int readLen, int lastComma) { + while (true) { + if (readLen <= lastComma + 1) { + return readLen; + } + lastComma++; + if (data[lastComma] == ',') { + break; + } + } + return lastComma; + } + + private final PicoColorSensorIOInputsAutoLogged inputs = new PicoColorSensorIOInputsAutoLogged(); + + private final AtomicBoolean debugPrints = new AtomicBoolean(); + @AutoLogOutput (key = "Subsystems/ColorSensor/HasColorElevator") + private boolean hasColorElevator; + @AutoLogOutput (key = "Subsystems/ColorSensor/HasColorShooter") + private boolean hasColorShooter; + @AutoLogOutput (key = "Subsystems/ColorSensor/ProximityElevator") + private int proximityElevator; + @AutoLogOutput (key = "Subsystems/ColorSensor/ProximityShooter") + private int proximityShooter; + private final RawColor colorElevator = new RawColor(); + private final RawColor colorShooter = new RawColor(); + private double lastReadTime; + private final ReentrantLock threadLock = new ReentrantLock(); + private final Thread readThread; + private final AtomicBoolean threadRunning = new AtomicBoolean(true); + + private Color detectedColorElevator = new Color(0.0, 0.0, 0.0); + private Color detectedColorShooter = new Color(0.0, 0.0, 0.0); + @AutoLogOutput (key = "Subsystems/ColorSensor/ElevatorColor") + private String elevatorColorString; + @AutoLogOutput (key = "Subsystems/ColorSensor/ShooterColor") + private String shooterColorString; + @AutoLogOutput (key = "Subsystems/ColorSensor/HasNoteElevator") + private boolean hasNoteElevator = false; + @AutoLogOutput (key = "Subsystems/ColorSensor/HasNoteShooter") + private boolean hasNoteShooter = false; + private ColorMatchResult elevatorMatch = ColorSensorConstants.COLOR_MATCH.matchClosestColor(detectedColorElevator); + private ColorMatchResult shooterMatch = ColorSensorConstants.COLOR_MATCH.matchClosestColor(detectedColorShooter); + @AutoLogOutput (key = "Subsystems/ColorSensor/MatchElevator") + private String matchStringElevator; + @AutoLogOutput (key = "Subsystems/ColorSensor/MatchShooter") + private String matchStringShooter; + private TransistorPower powerToPico; + private double disabledTimestamp = 0; + + private void threadMain() { + // Using JNI for a non allocating read + + int port = SerialPortJNI.serialInitializePort((byte) 1); + SerialPortJNI.serialSetBaudRate(port, 115200); + SerialPortJNI.serialSetDataBits(port, (byte) 8); + SerialPortJNI.serialSetParity(port, (byte) 0); + SerialPortJNI.serialSetStopBits(port, (byte) 10); + + SerialPortJNI.serialSetTimeout(port, 1); + SerialPortJNI.serialEnableTermination(port, '\n'); + + HAL.report(tResourceType.kResourceType_SerialPort, 2); + + inputs.buffer = new byte[257]; + SingleCharSequence charSeq = new SingleCharSequence(); + charSeq.data = inputs.buffer; + IntRef lastComma = new IntRef(); + + RawColor colorElevator = new RawColor(); + RawColor colorShooter = new RawColor(); + + while (threadRunning.get()) { + + int read = SerialPortJNI.serialRead(port, inputs.buffer, inputs.buffer.length - 1); + Logger.processInputs("SubsystemInputs/ColorSensor", inputs); + + if (read <= 0) { + try { + threadLock.lock(); + this.hasColorElevator = false; + this.hasColorShooter = false; + } finally { + threadLock.unlock(); + } + continue; + } + if (!threadRunning.get()) { + System.out.println("No thread"); + break; + } + + // Trim trailing newline if exists + if (inputs.buffer[read - 1] == '\n') { + read--; + } + + if (read == 0) { + continue; + } + + if (debugPrints.get()) { + System.out.println(new String(inputs.buffer, 0, read, StandardCharsets.UTF_8)); + } + + lastComma.value = -1; + + boolean hasColorShooter = parseIntFromIndex(charSeq, read, lastComma) != 0; + boolean hasColorElevator = parseIntFromIndex(charSeq, read, lastComma) != 0; + colorShooter.red = parseIntFromIndex(charSeq, read, lastComma); + colorShooter.green = parseIntFromIndex(charSeq, read, lastComma); + colorShooter.blue = parseIntFromIndex(charSeq, read, lastComma); + colorShooter.ir = parseIntFromIndex(charSeq, read, lastComma); + int proxShooter = parseIntFromIndex(charSeq, read, lastComma); + colorElevator.red = parseIntFromIndex(charSeq, read, lastComma); + colorElevator.green = parseIntFromIndex(charSeq, read, lastComma); + colorElevator.blue = parseIntFromIndex(charSeq, read, lastComma); + colorElevator.ir = parseIntFromIndex(charSeq, read, lastComma); + int proxElevator = parseIntFromIndex(charSeq, read, lastComma); + + double ts = Robot.currentTimestamp; + + try { + threadLock.lock(); + + this.lastReadTime = ts; + this.hasColorElevator = hasColorElevator; + this.hasColorShooter = hasColorShooter; + + if (hasColorElevator) { + this.colorElevator.red = colorElevator.red; + this.colorElevator.green = colorElevator.green; + this.colorElevator.blue = colorElevator.blue; + this.colorElevator.ir = colorElevator.ir; + this.proximityElevator = proxElevator; + + this.detectedColorElevator = new Color(this.colorElevator.red, this.colorElevator.green, this.colorElevator.blue); + this.elevatorMatch = ColorSensorConstants.COLOR_MATCH.matchClosestColor(detectedColorElevator); + this.elevatorColorString = detectedColorElevator.toString(); + + if (this.proximityElevator > 150) { + this.matchStringElevator = "Note"; + this.hasNoteElevator = true; + } else { + this.matchStringElevator = "Surely not a note"; + hadNoteElevatorLastLoop = hasNoteElevator; + this.hasNoteElevator = false; + } + } else { + this.hasNoteElevator = false; + this.proximityElevator = 0; + if (Robot.currentTimestamp - disabledTimestamp > 3) { + disabledTimestamp = Robot.currentTimestamp; + powerToPico.getPowerCycleCommand().schedule(); + } + } + if (hasColorShooter) { + this.colorShooter.red = colorShooter.red; + this.colorShooter.green = colorShooter.green; + this.colorShooter.blue = colorShooter.blue; + this.colorShooter.ir = colorShooter.ir; + this.proximityShooter = proxShooter; + + this.detectedColorShooter = new Color(this.colorShooter.red, this.colorShooter.green, this.colorShooter.blue); + this.shooterMatch = ColorSensorConstants.COLOR_MATCH.matchClosestColor(this.detectedColorShooter); + this.shooterColorString = detectedColorShooter.toString(); + + if (this.proximityShooter > 150) { + this.matchStringShooter = "Note"; + this.hasNoteShooter = true; + } else { + this.matchStringShooter = "Surely not a note"; + hadNoteShooterLastLoop = hasNoteShooter; + this.hasNoteShooter = false; + } + + } else { + this.hasNoteShooter = false; + this.proximityShooter = 0; + if (Robot.currentTimestamp - disabledTimestamp > 3) { + disabledTimestamp = Robot.currentTimestamp; + powerToPico.getPowerCycleCommand().schedule(); + } + } + } finally { + threadLock.unlock(); + } + } + + SerialPortJNI.serialClose(port); + } + + public PicoColorSensor() { + powerToPico = new TransistorPower(0); + powerToPico.set(true); + readThread = new Thread(this::threadMain); + readThread.setName("PicoColorSensorThread"); + readThread.start(); + } + + @AutoLogOutput (key = "Subsystems/ColorSensor/ElevatorSensorConnected") + public boolean elevatorSensorConnected() { + try { + threadLock.lock(); + return hasColorElevator; + } finally { + threadLock.unlock(); + } + } + + @AutoLogOutput (key = "Subsystems/ColorSensor/ShooterSensorConnected") + public boolean shooterSensorConnected() { + try { + threadLock.lock(); + return hasColorShooter; + } finally { + threadLock.unlock(); + } + } + + public RawColor getRawColorElevator() { + try { + threadLock.lock(); + return new RawColor(colorElevator.red, colorElevator.green, colorElevator.blue, colorElevator.ir); + } finally { + threadLock.unlock(); + } + } + + public void getRawColorElevator(RawColor rawColor) { + try { + threadLock.lock(); + rawColor.red = colorElevator.red; + rawColor.green = colorElevator.green; + rawColor.blue = colorElevator.blue; + rawColor.ir = colorElevator.ir; + } finally { + threadLock.unlock(); + } + } + + public int getProximityElevator() { + try { + threadLock.lock(); + return proximityElevator; + } finally { + threadLock.unlock(); + } + } + + public boolean hasNoteElevator() { + try { + threadLock.lock(); + return this.hasNoteElevator; + } finally { + threadLock.unlock(); + } + } + + private boolean hadNoteElevatorLastLoop = false; + private boolean hadNoteShooterLastLoop = false; + public boolean fallingEdgeHasNoteElevator() { + return hadNoteElevatorLastLoop; + } + + public boolean fallingEdgeHasNoteShooter() { + return hadNoteShooterLastLoop; + } + + public RawColor getRawColorShooter() { + try { + threadLock.lock(); + return new RawColor(colorShooter.red, colorShooter.green, colorShooter.blue, colorShooter.ir); + } finally { + threadLock.unlock(); + } + } + + public void getRawColorShooter(RawColor rawColor) { + try { + threadLock.lock(); + rawColor.red = colorShooter.red; + rawColor.green = colorShooter.green; + rawColor.blue = colorShooter.blue; + rawColor.ir = colorShooter.ir; + } finally { + threadLock.unlock(); + } + } + + public int getProximityShooter() { + try { + threadLock.lock(); + return proximityShooter; + } finally { + threadLock.unlock(); + } + } + + public boolean hasNoteShooter() { + try { + threadLock.lock(); + return this.hasNoteShooter; + } finally { + threadLock.unlock(); + } + } + + public double getLastReadTimestampSeconds() { + try { + threadLock.lock(); + return lastReadTime; + } finally { + threadLock.unlock(); + } + } + + void setDebugPrints(boolean debug) { + debugPrints.set(debug); + } + + @Override + public void close() throws Exception { + threadRunning.set(false); + readThread.join(); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/colorsensor/PicoColorSensorIO.java b/src/main/java/frc/robot/subsystems/colorsensor/PicoColorSensorIO.java new file mode 100644 index 00000000..190bb2d3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/colorsensor/PicoColorSensorIO.java @@ -0,0 +1,14 @@ +package frc.robot.subsystems.colorsensor; + +import org.littletonrobotics.junction.AutoLog; + +public interface PicoColorSensorIO { + + @AutoLog + class PicoColorSensorIOInputs { + + public byte[] buffer = new byte[257]; + + } + +} diff --git a/src/main/java/frc/robot/subsystems/drive/Gyro.java b/src/main/java/frc/robot/subsystems/drive/Gyro.java new file mode 100644 index 00000000..b47877d3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/Gyro.java @@ -0,0 +1,59 @@ +package frc.robot.subsystems.drive; + +import org.littletonrobotics.junction.Logger; + +import com.ctre.phoenix6.hardware.Pigeon2; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; + +public class Gyro implements GyroIO { + private final Pigeon2 pigeon; + private final GyroIOInputsAutoLogged inputs = new GyroIOInputsAutoLogged(); + + + public Gyro(int gyroCANId) { + pigeon = new Pigeon2(gyroCANId); + } + + + public Rotation2d getYawRotation2D() { + return inputs.yawPosition; + } + + public double getYawRadians() { + return inputs.yawPosition.getRadians(); + } + + public double getYawDegrees() { + return inputs.yawPosition.getDegrees(); + } + + public double getYawVelocity() { + return inputs.yawVelocityRadsPerSec; + } + + public void setYaw(double value) { + pigeon.setYaw(value); + } + + // could have pitch/roll as inputs but no need realistically + public double getPitch() { + return pigeon.getPitch().refresh().getValueAsDouble(); + } + + public double getRoll() { + return pigeon.getRoll().refresh().getValueAsDouble(); + } + + public void processInputs() { + Logger.processInputs("SubsystemInputs/Gyro", inputs); + } + + @Override + public void updateInputs() { + inputs.yawPosition = pigeon.getRotation2d(); + inputs.yawVelocityRadsPerSec = + Units.degreesToRadians(pigeon.getAngularVelocityZWorld().refresh().getValue()); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java new file mode 100644 index 00000000..e9537899 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.drive; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Rotation2d; + + +public interface GyroIO { + @AutoLog + class GyroIOInputs { + public Rotation2d yawPosition = new Rotation2d(); + public double yawVelocityRadsPerSec = 0.0; + } + + default void updateInputs() {} +} diff --git a/src/main/java/frc/robot/util/rev/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java similarity index 76% rename from src/main/java/frc/robot/util/rev/MAXSwerveModule.java rename to src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java index 8d8ff420..cadd11a5 100644 --- a/src/main/java/frc/robot/util/rev/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/drive/MAXSwerveModule.java @@ -2,7 +2,9 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.util.rev; +package frc.robot.subsystems.drive; + +import org.littletonrobotics.junction.Logger; import com.revrobotics.SparkPIDController; @@ -11,25 +13,25 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.ModuleConstants; -import monologue.Logged; -import monologue.Annotations.Log; +import frc.robot.util.rev.Neo; -public class MAXSwerveModule implements Logged{ +public class MAXSwerveModule implements MAXSwerveModuleIO { private final Neo driveMotor; private final Neo turnMotor; private double chassisAngularOffset = 0; - @Log - private SwerveModuleState desiredState = new SwerveModuleState(0.0, new Rotation2d()); + private final int index; + private SwerveModuleState desiredState = new SwerveModuleState(0.0, new Rotation2d()); + private final MAXSwerveModuleIOInputsAutoLogged inputs = new MAXSwerveModuleIOInputsAutoLogged(); /** * Constructs a MAXSwerveModule and configures the driving and turning motor, * encoder, and PID controller. This configuration is specific to the REV * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore * Encoder. */ - public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset) { + public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngularOffset, int index) { driveMotor = new Neo(drivingCANId, true); // Invert the turning encoder, since the output shaft rotates in the opposite @@ -38,7 +40,7 @@ public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngular turnMotor = new Neo(turningCANId, false, ModuleConstants.TURNING_ENCODER_INVERTED, true); this.chassisAngularOffset = chassisAngularOffset; - + this.index = index; resetEncoders(); configMotors(); desiredState.angle = new Rotation2d(turnMotor.getPosition()); @@ -52,8 +54,7 @@ public MAXSwerveModule(int drivingCANId, int turningCANId, double chassisAngular public SwerveModuleState getState() { // Apply chassis angular offset to the encoder position to get the position // relative to the chassis. - return new SwerveModuleState(driveMotor.getVelocity(), - new Rotation2d(turnMotor.getPosition() - chassisAngularOffset)); + return inputs.state; } public SparkPIDController getDrivingPIDController() { @@ -72,15 +73,13 @@ public SparkPIDController getTurningPIDController() { public SwerveModulePosition getPosition() { // Apply chassis angular offset to the encoder position to get the position // relative to the chassis. - return new SwerveModulePosition( - driveMotor.getPosition(), - new Rotation2d(turnMotor.getPosition() - chassisAngularOffset)); + return inputs.position; } // gets the rotations of the wheel converted to radians // We need to undo the position conversion factor public double getDrivePositionRadians() { - return (driveMotor.getPosition() / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS) * 2 * Math.PI; + return inputs.drivePositionMeters; } /** @@ -97,7 +96,7 @@ public void setDesiredState(SwerveModuleState desiredState) { // Optimize the reference state to avoid spinning further than 90 degrees. if (!FieldConstants.IS_SIMULATION) { correctedDesiredState = SwerveModuleState.optimize(correctedDesiredState, - new Rotation2d(turnMotor.getPosition())); + new Rotation2d(inputs.turnPositionRads)); } // Command driving and turning SPARKS MAX towards their respective setpoints. @@ -107,6 +106,10 @@ public void setDesiredState(SwerveModuleState desiredState) { this.desiredState = correctedDesiredState; } + public SwerveModuleState getDesiredState() { + return desiredState; + } + /** * Zeroes this driving motor's encoder. */ @@ -159,4 +162,33 @@ public void configMotors() { turnMotor.setSmartCurrentLimit(ModuleConstants.TURNING_MOTOR_CURRENT_LIMIT); setBrakeMode(); } + + public void processInputs() { + Logger.processInputs("SubsystemInputs/MAXSwerveModule" + index, inputs); + } + + @Override + public void updateInputs() { + // swerve module position and state + + inputs.position = new SwerveModulePosition( + driveMotor.getPosition(), + new Rotation2d(turnMotor.getPosition() - chassisAngularOffset)); + + inputs.state = new SwerveModuleState(driveMotor.getVelocity(), + new Rotation2d(turnMotor.getPosition() - chassisAngularOffset)); + + // drive motor + inputs.drivePositionMeters = (driveMotor.getPosition() / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS) * 2 * Math.PI; + inputs.driveVelocityMPS = driveMotor.getVelocity(); + inputs.driveAppliedVolts = driveMotor.getAppliedOutput(); + inputs.driveSupplyCurrentAmps = driveMotor.getOutputCurrent(); + + + // turning motor + inputs.turnAppliedVolts = turnMotor.getAppliedOutput(); + inputs.turnPositionRads = turnMotor.getPosition(); + inputs.turnVelocityRadsPerSec = turnMotor.getVelocity(); + inputs.turnSupplyCurrentAmps = turnMotor.getOutputCurrent(); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/drive/MAXSwerveModuleIO.java b/src/main/java/frc/robot/subsystems/drive/MAXSwerveModuleIO.java new file mode 100644 index 00000000..db3e3703 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/MAXSwerveModuleIO.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.drive; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +public interface MAXSwerveModuleIO { + + @AutoLog + class MAXSwerveModuleIOInputs { + + public double drivePositionMeters = 0.0; + public double driveVelocityMPS = 0.0; + public double driveAppliedVolts = 0.0; + public double driveSupplyCurrentAmps = 0.0; + + + public double turnPositionRads = 0.0; + public double turnVelocityRadsPerSec = 0.0; + public double turnAppliedVolts = 0.0; + public double turnSupplyCurrentAmps = 0.0; + + public SwerveModulePosition position = new SwerveModulePosition(); // + public SwerveModuleState state = new SwerveModuleState(); + } + + default void updateInputs() {} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/drive/Swerve.java similarity index 79% rename from src/main/java/frc/robot/subsystems/Swerve.java rename to src/main/java/frc/robot/subsystems/drive/Swerve.java index 2346b2bb..dbaf8436 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/drive/Swerve.java @@ -2,12 +2,14 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.subsystems; +package frc.robot.subsystems.drive; import java.util.function.BooleanSupplier; import java.util.function.Supplier; -import com.ctre.phoenix6.hardware.Pigeon2; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + import com.pathplanner.lib.auto.AutoBuilder; import java.util.Arrays; @@ -40,22 +42,20 @@ import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.DriveConstants; import frc.robot.util.Constants.FieldConstants; -import frc.robot.util.rev.MAXSwerveModule; -import monologue.Logged; -import monologue.Annotations.Log; +import frc.robot.util.Constants.ModuleConstants; +import frc.robot.util.calc.PoseCalculations; -public class Swerve extends SubsystemBase implements Logged { +public class Swerve extends SubsystemBase { public static double twistScalar = 4; private Rotation2d gyroRotation2d = new Rotation2d(); - @Log + @AutoLogOutput(key = "Subsystems/Swerve/AlignedToAmp") private boolean isAlignedToAmp = false; private final MAXSwerveModule frontLeft, frontRight, rearLeft, rearRight; - // The gyro sensor - private final Pigeon2 gyro; + private final Gyro gyro; private final MAXSwerveModule[] swerveModules; @@ -69,22 +69,26 @@ public Swerve() { frontLeft = new MAXSwerveModule( DriveConstants.FRONT_LEFT_DRIVING_CAN_ID, DriveConstants.FRONT_LEFT_TURNING_CAN_ID, - DriveConstants.FRONT_LEFT_CHASSIS_ANGULAR_OFFSET); + DriveConstants.FRONT_LEFT_CHASSIS_ANGULAR_OFFSET, + ModuleConstants.FRONT_LEFT_INDEX); frontRight = new MAXSwerveModule( DriveConstants.FRONT_RIGHT_DRIVING_CAN_ID, DriveConstants.FRONT_RIGHT_TURNING_CAN_ID, - DriveConstants.FRONT_RIGHT_CHASSIS_ANGULAR_OFFSET); + DriveConstants.FRONT_RIGHT_CHASSIS_ANGULAR_OFFSET, + ModuleConstants.FRONT_RIGHT_INDEX); rearLeft = new MAXSwerveModule( DriveConstants.REAR_LEFT_DRIVING_CAN_ID, DriveConstants.REAR_LEFT_TURNING_CAN_ID, - DriveConstants.BACK_LEFT_CHASSIS_ANGULAR_OFFSET); + DriveConstants.BACK_LEFT_CHASSIS_ANGULAR_OFFSET, + ModuleConstants.REAR_LEFT_INDEX); rearRight = new MAXSwerveModule( DriveConstants.REAR_RIGHT_DRIVING_CAN_ID, DriveConstants.REAR_RIGHT_TURNING_CAN_ID, - DriveConstants.BACK_RIGHT_CHASSIS_ANGULAR_OFFSET); + DriveConstants.BACK_RIGHT_CHASSIS_ANGULAR_OFFSET, + ModuleConstants.REAR_RIGHT_INDEX); swerveModules = new MAXSwerveModule[] { frontLeft, @@ -93,7 +97,7 @@ public Swerve() { rearRight }; - gyro = new Pigeon2(DriveConstants.PIGEON_CAN_ID); + gyro = new Gyro(DriveConstants.PIGEON_CAN_ID); gyro.setYaw(0); resetEncoders(); setBrakeMode(); @@ -109,7 +113,7 @@ public Swerve() { poseEstimator = new SwerveDrivePoseEstimator( DriveConstants.DRIVE_KINEMATICS, - gyro.getRotation2d(), + gyro.getYawRotation2D(), getModulePositions(), new Pose2d(), // State measurements @@ -137,14 +141,16 @@ public Swerve() { @Override public void periodic() { - gyroRotation2d = gyro.getRotation2d(); + updateAndProcessSwerveModuleInputs(); + updateAndProcessGyroInputs(); + gyroRotation2d = gyro.getYawRotation2D(); poseEstimator.updateWithTime(Timer.getFPGATimestamp(), gyroRotation2d, getModulePositions()); logPositions(); - this.isAlignedToAmp = isAlignedToAmp(); + this.isAlignedToAmp = PoseCalculations.isAlignedToAmp(currentPose); } - @Log + @AutoLogOutput(key = "Subsystems/Swerve/CurrentPose2d") Pose2d currentPose = new Pose2d(); public void logPositions() { @@ -179,19 +185,41 @@ public void logPositions() { RobotContainer.robotPose2d = currentPose; } + double pitch = gyro.getPitch(); + double roll = gyro.getRoll(); + + Rotation3d rotation3d = FieldConstants.IS_SIMULATION + ? new Rotation3d( + Units.degreesToRadians(0), + Units.degreesToRadians(0), + currentPose.getRotation().getRadians()) + : new Rotation3d( + Units.degreesToRadians(-roll), + Units.degreesToRadians(-pitch+180), + currentPose.getRotation().getRadians()+Math.PI); + RobotContainer.robotPose3d = new Pose3d( new Translation3d( currentPose.getX(), currentPose.getY(), Math.hypot( - Rotation2d.fromDegrees(gyro.getRoll().refresh().getValue()).getSin() + Rotation2d.fromDegrees(roll).getSin() * DriveConstants.ROBOT_LENGTH_METERS / 2.0, - Rotation2d.fromDegrees(gyro.getPitch().refresh().getValue()).getSin() * + Rotation2d.fromDegrees(pitch).getSin() * DriveConstants.ROBOT_LENGTH_METERS / 2.0)), - new Rotation3d(0, 0, currentPose.getRotation().getRadians())); + rotation3d); RobotContainer.distanceToSpeakerMeters = currentPose.getTranslation().getDistance(FieldConstants.GET_SPEAKER_TRANSLATION()); + RobotContainer.distanceToSpeakerFeet = Math.round(Units.metersToFeet(RobotContainer.distanceToSpeakerMeters) * 1000.0) / 1000.0; + Logger.recordOutput("Subsystems/Swerve/RobotPose3d", RobotContainer.robotPose3d); + Logger.recordOutput("Subsystems/Swerve/ChassisSpeeds", speeds); + Logger.recordOutput("Subsystems/Swerve/IsUnderStage", PoseCalculations.inStageTriangle(currentPose)); + Logger.recordOutput("Subsystems/Swerve/AtHDCAngle", atHDCAngle()); + Logger.recordOutput("Subsystems/Swerve/Modules/FrontLeft/DesiredState", frontLeft.getDesiredState()); + Logger.recordOutput("Subsystems/Swerve/Modules/FrontRight/DesiredState", frontRight.getDesiredState()); + Logger.recordOutput("Subsystems/Swerve/Modules/RearLeft/DesiredState", rearLeft.getDesiredState()); + Logger.recordOutput("Subsystems/Swerve/Modules/RearRight/DesiredState", rearRight.getDesiredState()); } /** @@ -243,12 +271,17 @@ public void stopDriving() { drive(0, 0, 0, false); } - @Log - Pose2d desiredHDCPose = new Pose2d(); + @AutoLogOutput (key = "Subsystems/Swerve/DesiredHDCPose") + private Pose2d desiredHDCPose = new Pose2d(); + public void setDesiredPose(Pose2d pose) { desiredHDCPose = pose; } + public Pose2d getDesiredPose() { + return desiredHDCPose; + } + public ChassisSpeeds getRobotRelativeVelocity() { return DriveConstants.DRIVE_KINEMATICS.toChassisSpeeds(getModuleStates()); } @@ -427,10 +460,18 @@ public void resetHDC() { AutoConstants.HDC.getThetaController().reset(getPose().getRotation().getRadians()); } + public void resetDesiredHDCPose() { + setDesiredPose(new Pose2d()); + } + public Command resetHDCCommand() { return Commands.runOnce(() -> resetHDC()); } + public Command resetDesiredHDCPoseCommand() { + return Commands.runOnce(() -> resetDesiredHDCPose()); + } + public void reconfigureAutoBuilder() { AutoBuilder.configureHolonomic( this::getPose, @@ -452,7 +493,7 @@ public boolean atPose(Pose2d position) { double distance = currentPose.relativeTo(position).getTranslation().getNorm(); return MathUtil.isNear(0, distance, AutoConstants.AUTO_POSITION_TOLERANCE_METERS) - && MathUtil.isNear(0, angleDiff, AutoConstants.AUTO_POSITION_TOLERANCE_RADIANS); + && MathUtil.isNear(0, angleDiff, AutoConstants.AUTO_ROTATION_TOLERANCE_RADIANS); } public boolean atHDCPose() { @@ -460,17 +501,23 @@ public boolean atHDCPose() { } public boolean atHDCAngle() { - return MathUtil.isNear(desiredHDCPose.getRotation().getRadians(), getPose().getRotation().getRadians(), AutoConstants.AUTO_POSITION_TOLERANCE_RADIANS); + return MathUtil.isNear( + desiredHDCPose.getRotation().getRadians(), + getPose().getRotation().getRadians(), + PoseCalculations.inSpeakerShotZone(getPose().getTranslation()) + ? AutoConstants.AUTO_ROTATION_TOLERANCE_RADIANS + : AutoConstants.PASS_ROTATION_TOLERANCE_RADIANS); + } + + public void updateAndProcessSwerveModuleInputs() { + for (MAXSwerveModule swerveModule : swerveModules) { + swerveModule.updateInputs(); + swerveModule.processInputs(); + } } - public boolean isAlignedToAmp() { - Translation2d touchingAmpPose = new Translation2d( - FieldConstants.GET_AMP_POSITION().getX(), - FieldConstants.GET_AMP_POSITION().getY() - - DriveConstants.ROBOT_LENGTH_METERS / 2.0 - - DriveConstants.BUMPER_LENGTH_METERS - ); - double robotX = this.getPose().getTranslation().getDistance(touchingAmpPose); - return MathUtil.isNear(0, robotX, AutoConstants.AUTO_ALIGNMENT_DEADBAND); + public void updateAndProcessGyroInputs() { + gyro.updateInputs(); + gyro.processInputs(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/intake/Indexer.java similarity index 68% rename from src/main/java/frc/robot/subsystems/Indexer.java rename to src/main/java/frc/robot/subsystems/intake/Indexer.java index fb6d7aa6..198efb78 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/intake/Indexer.java @@ -1,4 +1,6 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; @@ -7,9 +9,9 @@ import frc.robot.util.rev.Neo; import frc.robot.util.rev.SafeSpark.TelemetryPreference; -public class Indexer extends SubsystemBase { +public class Indexer extends SubsystemBase implements IndexerIO { private final Neo motor; - private double desiredPercent = 0; + private final IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged(); public Indexer() { motor = new Neo(IntakeConstants.TRIGGER_WHEEL_CAN_ID, false); @@ -17,8 +19,14 @@ public Indexer() { motor.setTelemetryPreference(TelemetryPreference.NO_ENCODER); } + @Override + public void periodic() { + updateInputs(inputs); + Logger.processInputs("SubsystemInputs/Indexer", inputs); + } + public double getDesiredPercent() { - return desiredPercent; + return inputs.targetPercent; } public void setDesiredPercent(double percent) { @@ -27,9 +35,9 @@ public void setDesiredPercent(double percent) { IntakeConstants.INDEXER_PERCENT_LOWER_LIMIT, IntakeConstants.INDEXER_PERCENT_UPPER_LIMIT); - if (percent != desiredPercent) { - desiredPercent = percent; - motor.set(desiredPercent); + if (percent != inputs.targetPercent) { + inputs.targetPercent = percent; + motor.set(inputs.targetPercent); } } @@ -68,4 +76,11 @@ public Command setBrakeMode() { public boolean hasPiece() { return false; } + + @Override + public void updateInputs(IndexerIOInputs inputs) { + inputs.targetPercent = motor.getTargetPercent(); + inputs.appliedVolts = motor.getAppliedOutput(); + inputs.outputCurrentAmps = motor.getOutputCurrent(); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IndexerIO.java b/src/main/java/frc/robot/subsystems/intake/IndexerIO.java new file mode 100644 index 00000000..0ad21345 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IndexerIO.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IndexerIO { + + @AutoLog + class IndexerIOInputs { + + public double targetPercent = 0.0; + public double appliedVolts = 0.0; + public double outputCurrentAmps = 0.0; + + } + + default void updateInputs(IndexerIOInputs inputs) {} + +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java similarity index 62% rename from src/main/java/frc/robot/subsystems/Intake.java rename to src/main/java/frc/robot/subsystems/intake/Intake.java index 1f0b0ffd..876ff6e9 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -1,21 +1,22 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.IntakeConstants; import frc.robot.util.rev.Neo; import frc.robot.util.rev.SafeSpark.TelemetryPreference; -import monologue.Logged; -import monologue.Annotations.Log; -public class Intake extends SubsystemBase implements Logged { +public class Intake extends SubsystemBase implements IntakeIO { private final Neo motor; - - @Log - private double desiredSpeed = 0; - @Log + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + + @AutoLogOutput (key = "Subsystems/Intake/NotePossession") private boolean notePossession = FieldConstants.IS_SIMULATION; public Intake() { @@ -24,6 +25,12 @@ public Intake() { motor.setTelemetryPreference(TelemetryPreference.NO_ENCODER); } + @Override + public void periodic() { + updateInputs(inputs); + Logger.processInputs("SubsystemInputs/Intake", inputs); + } + public void setPercent(double desiredPercent) { desiredPercent = MathUtil.clamp( @@ -31,10 +38,10 @@ public void setPercent(double desiredPercent) { IntakeConstants.INTAKE_PERCENT_LOWER_LIMIT, IntakeConstants.INTAKE_PERCENT_UPPER_LIMIT); - if (desiredSpeed != desiredPercent) { - desiredSpeed = desiredPercent; + if (inputs.targetPercent != desiredPercent) { - motor.set(desiredSpeed); + motor.set(desiredPercent); + } } @@ -49,6 +56,13 @@ public Command inCommand() { return setPercentCommand(IntakeConstants.INTAKE_PERCENT); } + public Command inCommandFor(double seconds) { + return setPercentCommand(IntakeConstants.INTAKE_PERCENT) + .andThen( + Commands.waitSeconds(seconds), + stopCommand()); + } + public Command inCommandSlow() { return setPercentCommand(IntakeConstants.INTAKE_PERCENT/3.0); } @@ -62,7 +76,7 @@ public Command outCommand() { } public boolean isStopped() { - return desiredSpeed == 0; + return inputs.targetPercent == 0; } public Command stopCommand() { @@ -78,6 +92,13 @@ public Command setBrakeMode() { } public double getDesiredSpeed() { - return desiredSpeed; + return inputs.targetPercent; + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.targetPercent = motor.getTargetPercent(); + inputs.appliedVolts = motor.getAppliedOutput(); + inputs.outputCurrentAmps = motor.getOutputCurrent(); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 00000000..9caf72cc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + + @AutoLog + class IntakeIOInputs { + + public double targetPercent = 0.0; + public double appliedVolts = 0.0; + public double outputCurrentAmps = 0.0; + + } + + default void updateInputs(IntakeIOInputs inputs) {} + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/shooter/Pivot.java similarity index 67% rename from src/main/java/frc/robot/subsystems/Pivot.java rename to src/main/java/frc/robot/subsystems/shooter/Pivot.java index 3f77c36a..2a91c9b7 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/shooter/Pivot.java @@ -1,4 +1,7 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose3d; @@ -12,18 +15,18 @@ import frc.robot.util.Constants.NTConstants; import frc.robot.util.Constants.ShooterConstants; import frc.robot.util.rev.Neo; -import monologue.Logged; -import monologue.Annotations.Log; -public class Pivot extends SubsystemBase implements Logged { +public class Pivot extends SubsystemBase implements PivotIO { private Neo motor; - @Log - private double realAngle = 0, desiredAngle = 0; + private final PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged(); - @Log + @AutoLogOutput (key = "Subsystems/Pivot/AtDesiredAngle") private boolean atDesiredAngle = false; + @AutoLogOutput (key = "Subsystems/Pivot/AtDesiredPassAngle") + private boolean atDesiredPassAngle = false; + public Pivot() { motor = new Neo( ShooterConstants.SHOOTER_PIVOT_CAN_ID, @@ -39,22 +42,26 @@ public void configMotor() { motor.setSmartCurrentLimit(ShooterConstants.PIVOT_CURRENT_LIMIT); motor.setPositionConversionFactor(ShooterConstants.PIVOT_POSITION_CONVERSION_FACTOR); motor.setPID(ShooterConstants.PIVOT_PID); - // motor.getAbsoluteEncoder().setZeroOffset(68.8235307-17.6); + // motor.getAbsoluteEncoder().setZeroOffset(75.4944688-17.6); } @Override public void periodic() { - realAngle = getAngle(); + updateInputs(inputs); + Logger.processInputs("SubsystemInputs/Pivot", inputs); atDesiredAngle = - MathUtil.isNear(realAngle, desiredAngle, ShooterConstants.PIVOT_DEADBAND); + MathUtil.isNear(inputs.positionDegrees, inputs.targetPositionDegrees, ShooterConstants.PIVOT_DEADBAND_DEGREES); + + atDesiredPassAngle = + MathUtil.isNear(inputs.positionDegrees, inputs.targetPositionDegrees, ShooterConstants.PIVOT_PASS_DEADBAND_DEGREES); RobotContainer.components3d[NTConstants.PIVOT_INDEX] = new Pose3d( NTConstants.PIVOT_OFFSET_METERS.getX(), 0, NTConstants.PIVOT_OFFSET_METERS.getZ(), - new Rotation3d(0, -Units.degreesToRadians(realAngle), 0) + new Rotation3d(0, -Units.degreesToRadians(inputs.positionDegrees), 0) ); } @@ -65,13 +72,13 @@ public void periodic() { * @param double The angle to set the shooter to */ public void setAngle(double angle) { + angle = MathUtil.clamp( angle, ShooterConstants.PIVOT_LOWER_LIMIT_DEGREES, ShooterConstants.PIVOT_UPPER_LIMIT_DEGREES); motor.setTargetPosition(angle); - desiredAngle = angle; RobotContainer.desiredComponents3d[NTConstants.PIVOT_INDEX] = new Pose3d( NTConstants.PIVOT_OFFSET_METERS.getX(), @@ -110,4 +117,16 @@ public double getTargetAngle() { public boolean getAtDesiredAngle() { return atDesiredAngle; } + + public boolean getAtDesiredPassAngle() { + return atDesiredPassAngle; + } + + @Override + public void updateInputs(PivotIOInputs inputs) { + inputs.targetPositionDegrees = this.getTargetAngle(); + inputs.positionDegrees = this.getAngle(); + inputs.appliedVolts = motor.getAppliedOutput(); + inputs.outputCurrentAmps = motor.getOutputCurrent(); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/PivotIO.java b/src/main/java/frc/robot/subsystems/shooter/PivotIO.java new file mode 100644 index 00000000..57bdecfd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/PivotIO.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLog; + +public interface PivotIO { + + @AutoLog + class PivotIOInputs { + public double targetPositionDegrees = 0.0; + public double positionDegrees = 0.0; + public double appliedVolts = 0.0; + public double outputCurrentAmps = 0.0; + } + + default void updateInputs(PivotIOInputs inputs) {} + +} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java similarity index 55% rename from src/main/java/frc/robot/subsystems/Shooter.java rename to src/main/java/frc/robot/subsystems/shooter/Shooter.java index 266e37f2..f840a0ee 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,4 +1,7 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; @@ -8,27 +11,20 @@ import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.ShooterConstants; import frc.robot.util.rev.Neo; -import monologue.Logged; -import monologue.Annotations.Log; -public class Shooter extends SubsystemBase implements Logged{ +public class Shooter extends SubsystemBase implements ShooterIO { /** Creates a new shooter. */ private final Neo leftMotor; private final Neo rightMotor; - @Log - private double targetLeftSpeed = 0; - @Log - private double targetRightSpeed = 0; - - @Log - private double currentLeftSpeed = 0; - @Log - private double currentRightSpeed = 0; + private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - @Log + @AutoLogOutput (key = "Subsystems/Shooter/AtDesiredRPM") private boolean atDesiredRPM = false; + @AutoLogOutput (key = "Subsystems/Shooter/AtDesiredPassRPM") + private boolean atDesiredPassRPM = false; + public Shooter() { leftMotor = new Neo(ShooterConstants.LEFT_SHOOTER_CAN_ID, true, true); @@ -50,16 +46,24 @@ public void configMotors() { @Override public void periodic() { - currentLeftSpeed = leftMotor.getVelocity(); - currentRightSpeed = rightMotor.getVelocity(); + updateInputs(inputs); + Logger.processInputs("SubsystemInputs/Shooter", inputs); atDesiredRPM = MathUtil.isNear( - currentLeftSpeed, targetLeftSpeed, + inputs.leftVelocityRPM, inputs.leftTargetVelocityRPM, ShooterConstants.SHOOTER_RPM_DEADBAND) && MathUtil.isNear( - currentRightSpeed, targetRightSpeed, + inputs.rightVelocityRPM, inputs.rightTargetVelocityRPM, ShooterConstants.SHOOTER_RPM_DEADBAND); + + atDesiredPassRPM = + MathUtil.isNear( + inputs.leftVelocityRPM, inputs.leftTargetVelocityRPM, + ShooterConstants.SHOOTER_PASS_RPM_DEADBAND) + && MathUtil.isNear( + inputs.rightVelocityRPM, inputs.rightTargetVelocityRPM, + ShooterConstants.SHOOTER_PASS_RPM_DEADBAND); } /** @@ -71,15 +75,11 @@ public void periodic() { public void setSpeed(double speed) { leftMotor.setTargetVelocity(speed); rightMotor.setTargetVelocity(speed); - targetLeftSpeed = speed; - targetRightSpeed = speed; } public void setSpeed(Pair speeds) { leftMotor.setTargetVelocity(speeds.getFirst()); rightMotor.setTargetVelocity(speeds.getSecond()); - targetLeftSpeed = speeds.getFirst(); - targetRightSpeed = speeds.getSecond(); } /** @@ -99,11 +99,11 @@ public Command setSpeedCommand(Pair speeds) { } public Pair getSpeed() { - return new Pair(leftMotor.getVelocity(), rightMotor.getVelocity()); + return new Pair(inputs.leftVelocityRPM, inputs.rightVelocityRPM); } public double getAverageSpeed() { - return (leftMotor.getVelocity() + rightMotor.getVelocity()) / 2.0; + return (inputs.leftVelocityRPM + inputs.rightVelocityRPM) / 2.0; } /** @@ -125,11 +125,38 @@ public boolean getAtDesiredRPM() { return atDesiredRPM; } + public boolean getAtDesiredPassRPM() { + return atDesiredPassRPM; + } + public Pair getDesiredSpeeds() { - return new Pair(targetLeftSpeed, targetRightSpeed); + return new Pair(inputs.leftTargetVelocityRPM, inputs.rightTargetVelocityRPM); } public double getAverageTargetSpeed() { - return (targetLeftSpeed + targetRightSpeed) / 2.0; + return (inputs.leftTargetVelocityRPM + inputs.rightTargetVelocityRPM) / 2.0; + } + + public Command fullPower(double desiredSpeed) { + return run(() -> { + leftMotor.setVoltage(12); + rightMotor.setVoltage(12); + }).until(() -> getAverageSpeed() >= desiredSpeed) + .andThen(setSpeedCommand(desiredSpeed)); + } + + @Override + public void updateInputs(ShooterIOInputs inputs) { + inputs.leftVelocityRPM = leftMotor.getVelocity(); + inputs.leftTargetVelocityRPM = leftMotor.getTargetVelocity(); + inputs.leftPositionRotations = leftMotor.getPosition(); + inputs.leftAppliedVolts = leftMotor.getAppliedOutput(); + inputs.leftOutputCurrentAmps = leftMotor.getOutputCurrent(); + + inputs.rightVelocityRPM = rightMotor.getVelocity(); + inputs.rightTargetVelocityRPM = rightMotor.getTargetVelocity(); + inputs.rightPositionRotations = rightMotor.getPosition(); + inputs.rightAppliedVolts = rightMotor.getAppliedOutput(); + inputs.rightOutputCurrentAmps = rightMotor.getOutputCurrent(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java new file mode 100644 index 00000000..0728af0c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -0,0 +1,26 @@ +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLog; + +public interface ShooterIO { + + @AutoLog + class ShooterIOInputs { + + public double leftVelocityRPM = 0.0; + public double leftTargetVelocityRPM = 0.0; + public double leftPositionRotations = 0.0; + public double leftAppliedVolts = 0.0; + public double leftOutputCurrentAmps = 0.0; + + public double rightVelocityRPM = 0.0; + public double rightTargetVelocityRPM = 0.0; + public double rightPositionRotations = 0.0; + public double rightAppliedVolts = 0.0; + public double rightOutputCurrentAmps = 0.0; + + } + + default void updateInputs(ShooterIOInputs inputs) {} + +} diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/vision/Limelight.java similarity index 69% rename from src/main/java/frc/robot/subsystems/Limelight.java rename to src/main/java/frc/robot/subsystems/vision/Limelight.java index 919946aa..1b28bb25 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems; +package frc.robot.subsystems.vision; import java.io.IOException; import java.util.ArrayList; @@ -6,6 +6,8 @@ import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -18,7 +20,6 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -30,42 +31,35 @@ import frc.robot.util.Constants.CameraConstants; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.calc.LimelightHelpers; -import frc.robot.util.calc.LimelightHelpers.LimelightTarget_Detector; +import frc.robot.util.calc.PoseCalculations; import frc.robot.util.calc.LimelightHelpers.LimelightTarget_Fiducial; import frc.robot.util.calc.LimelightHelpers.Results; -import monologue.Logged; -import monologue.Annotations.Log; // https://github.com/NAHSRobotics-Team5667/2020-FRC/blob/master/src/main/java/frc/robot/utils/LimeLight.java -public class Limelight extends SubsystemBase implements Logged{ +public class Limelight extends SubsystemBase implements LimelightIO { String limelightName; private final Supplier robotPoseSupplier; private final SwerveDrivePoseEstimator poseEstimator; - @Log - Pose3d[] visableTags; + private final LimelightIOInputsAutoLogged inputs = new LimelightIOInputsAutoLogged(); + + Pose3d[] visibleTags; private static NetworkTableEntry timingTestEntry; private static boolean timingTestEntryValue = false; - private int pipelineIndex; - @Log public boolean isConnected = false; - @Log private Pose2d estimatedPose2d = new Pose2d(); - @Log public long timeDifference = 999_999; // Micro Seconds = 0.999999 Seconds | So the limelight is not connected if the time difference is greater than LimelightConstants.LIMELIGHT_MAX_UPDATE_TIME - @Log private Pose2d notePose2d = new Pose2d(); public Limelight(SwerveDrivePoseEstimator poseEstimator, Supplier robotPoseSupplier, String limelightName, int pipelineIndex) { // Uses network tables to check status of limelight this.limelightName = limelightName; - this.pipelineIndex = pipelineIndex; timingTestEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "TIMING_TEST_ENTRY"); this.robotPoseSupplier = robotPoseSupplier; this.poseEstimator = poseEstimator; @@ -75,26 +69,27 @@ public Limelight(SwerveDrivePoseEstimator poseEstimator, Supplier robotP @Override public void periodic() { + updateInputs(inputs); + Logger.processInputs("SubsystemInputs/" + limelightName, inputs); if (FieldConstants.IS_SIMULATION) { updateCameras(robotPoseSupplier.get()); } else { - if (pipelineIndex == 0) { - updatePoseEstimator(); - getTags(); - } else if (pipelineIndex == 1) { - notePose2d = getNotePose2d(); - } + updatePoseEstimator(); + setFiducialPoses(); } + Logger.recordOutput("Subsystems/" + limelightName + "/VisibleTags", visibleTags); + Logger.recordOutput("Subsystems/" + limelightName + "/IsConnected", isConnected); + Logger.recordOutput("Subsystems/" + limelightName + "/EstimatedPose2d", estimatedPose2d); + Logger.recordOutput("Subsystems/" + limelightName + "/TimeDifference", timeDifference); + Logger.recordOutput("Subsystems/" + limelightName + "/NotePose2d", notePose2d); + Logger.recordOutput("Subsystems/" + limelightName + "/NoteFieldPose", noteFieldPose); + Logger.recordOutput("Subsystems/" + limelightName + "/NoteFieldPosePlus14", noteFieldPosePlus14); + Logger.recordOutput("Subsystems/" + limelightName + "/TagsViable", tagsViable); } private void updatePoseEstimator() { - if (LimelightHelpers.getCurrentPipelineIndex(limelightName) != 0) { - LimelightHelpers.setPipelineIndex(limelightName, 0); - } - Results result = getResults(); - Pose2d estimatedRobotPose = result.getBotPose2d_wpiBlue(); - - LimelightTarget_Fiducial[] targets = result.targets_Fiducials; + + Pose2d estimatedRobotPose = inputs.botPose2d; this.estimatedPose2d = estimatedRobotPose; RobotContainer.visionErrorPose = RobotContainer.robotPose2d.minus(estimatedRobotPose); @@ -103,21 +98,21 @@ private void updatePoseEstimator() { if ((Robot.gameMode == GameMode.DISABLED || Robot.gameMode == GameMode.AUTONOMOUS && Robot.currentTimestamp - RobotContainer.gameModeStart < 1.75) - && hasTarget(result)) { + && hasTarget()) { xyStds = 0.001; radStds = 0.0002; poseEstimator.setVisionMeasurementStdDevs( VecBuilder.fill(xyStds, xyStds, radStds)); poseEstimator.addVisionMeasurement(estimatedRobotPose, - Timer.getFPGATimestamp() - getLatencyDiffSeconds(result)); + Timer.getFPGATimestamp() - getLatencyDiffSeconds()); } - else if (RobotContainer.enableVision && hasTarget(result)) { + else if (RobotContainer.enableVision && hasTarget()) { // multiple targets detected - if (targets.length > 1) { + if (inputs.targetIDs.length > 1) { if (Robot.gameMode == GameMode.TELEOP) { // Trust the vision even MORE - if (targets.length > 2) { + if (inputs.targetIDs.length > 2) { xyStds = Math.hypot(0.002, 0.003); } else { // We can only see two tags, (still trustable) @@ -129,7 +124,7 @@ else if (RobotContainer.enableVision && hasTarget(result)) { radStds = Units.degreesToRadians(2); } // 1 target with large area and close to estimated roxose - else if (LimelightHelpers.getTA(limelightName) > 0.14) { + else if (inputs.limelightTA > 0.14) { xyStds = Math.hypot(0.015, 0.033); radStds = Units.degreesToRadians(7); } @@ -141,7 +136,7 @@ else if (LimelightHelpers.getTA(limelightName) > 0.14) { poseEstimator.setVisionMeasurementStdDevs( VecBuilder.fill(xyStds, xyStds, radStds)); poseEstimator.addVisionMeasurement(estimatedRobotPose, - Timer.getFPGATimestamp() - getLatencyDiffSeconds(result)); + Timer.getFPGATimestamp() - getLatencyDiffSeconds()); } } @@ -151,33 +146,23 @@ public Results getResults() { public void setPipelineIndex(int index) { LimelightHelpers.setPipelineIndex(limelightName, index); + inputs.pipelineIndex = index; } - private LimelightTarget_Fiducial[] getTags() { - LimelightTarget_Fiducial[] fiducials = LimelightHelpers.getLatestResults(limelightName).targetingResults.targets_Fiducials; - - setFiducialPoses(fiducials); - - return fiducials; - } - - private void setFiducialPoses(LimelightTarget_Fiducial[] fiducials) { + private void setFiducialPoses() { ArrayList knownFiducials = new ArrayList<>(); - for (LimelightTarget_Fiducial target : fiducials) { - int tagID = (int) target.fiducialID; - if (tagID < aprilTagFieldLayout.getTags().size()) { - knownFiducials.add(aprilTagFieldLayout.getTagPose(tagID).get()); + for (int targetID : inputs.targetIDs) { + if (targetID < aprilTagFieldLayout.getTags().size()) { + knownFiducials.add(aprilTagFieldLayout.getTagPose(targetID).get()); } } - visableTags = knownFiducials.toArray(new Pose3d[0]); + visibleTags = knownFiducials.toArray(new Pose3d[0]); } - @Log Pose2d noteFieldPose = new Pose2d(); - @Log Pose2d noteFieldPosePlus14 = new Pose2d(); public Pose2d getNotePose2d() { @@ -187,21 +172,15 @@ public Pose2d getNotePose2d() { noteFieldPose = robotPoseSupplier.get().nearest(FieldConstants.GET_CENTERLINE_NOTES()); noteTranslationFromRobot = noteFieldPose.relativeTo(robotPoseSupplier.get()).getTranslation(); } else { - if (LimelightHelpers.getCurrentPipelineIndex(limelightName) != 1) { - LimelightHelpers.setPipelineIndex(limelightName, 1); - } + // if (inputs.pipelineIndex != 1) { + // setPipelineIndex(1); + // } - Results results = getResults(); - - if (noteInVision(results)) { - for (LimelightTarget_Detector ld : results.targets_Detector) { - ld.calculateYDistance(CameraConstants.LL2Pose.getZ(), CameraConstants.LL2Pose.getRotation().getY()); - ld.calculateXDistance(CameraConstants.LL2Pose.getRotation().toRotation2d().getRadians()); - } + if (noteInVision()) { Translation2d noteTranslationFromCamera = new Translation2d( - results.targets_Detector[0].calcY, - -results.targets_Detector[0].calcX + inputs.noteCalcY, + -inputs.noteCalcX ); noteTranslationFromRobot = noteTranslationFromCamera @@ -239,29 +218,25 @@ public Pose2d getNotePose2d() { return noteFieldPosePlus14; } - public boolean noteInVision(Results results) { + public boolean noteInVision() { return (FieldConstants.IS_SIMULATION && ((int) Robot.currentTimestamp/3) % 2 == 0) || - (results.valid - && results.targets_Detector.length > 0 - && LimelightHelpers.getTA(limelightName) > 0.5 - && results.targets_Detector[0].tx != 0 - && results.targets_Detector[0].ty != 0); + (inputs.validResult + && inputs.targetTxs.length > 0 + && inputs.limelightTA > 0.5 + && inputs.targetTxs[0] != 0 + && inputs.targetTys[0] != 0); } public Pose2d getPose2d() { - return LimelightHelpers.getBotPose2d_wpiBlue(limelightName); + return inputs.botPose2d; } public double getTagID() { - return LimelightHelpers.getFiducialID(limelightName); + return inputs.targetIDs[0]; } public double getLatencyDiffSeconds() { - return (LimelightHelpers.getLatency_Pipeline(limelightName)/1000d) - (LimelightHelpers.getLatency_Capture(limelightName)/1000d); - } - - public double getLatencyDiffSeconds(Results result) { - return (result.latency_pipeline/1000.0) - (result.latency_capture/1000.0); + return (inputs.latencyPipeline/1000d) - (inputs.latencyCapture/1000d); } // The below code is for simulation only @@ -298,7 +273,7 @@ public void updateCameras(Pose2d robotPose) { } } } - visableTags = poses.toArray(new Pose3d[0]); + visibleTags = poses.toArray(new Pose3d[0]); } /** @@ -326,28 +301,36 @@ private boolean isInFieldOfView(Pose2d robotPose, Pose3d cameraPose, Pose3d tagP return angleCheck && distanceCheck && isFacing; } - @Log int tagsViable = 0; - public boolean hasTarget(Results result) { + public boolean hasTarget() { - Pose2d estimatedRobotPose = result.getBotPose2d_wpiBlue(); - LimelightTarget_Fiducial[] targets = result.targets_Fiducials; + Pose2d estimatedRobotPose = inputs.botPose2d; - double singleTagAmbiguityThreshold = Robot.gameMode == GameMode.AUTONOMOUS ? 0.175 : 0.141; - double multiTagAmbiguityThreshold = Robot.gameMode == GameMode.AUTONOMOUS ? 0.1 : 0.05; - if (result == null || !result.valid - || (LimelightHelpers.getTA(limelightName) < singleTagAmbiguityThreshold && result.targets_Fiducials.length == 1) - || (result.targets_Fiducials.length > 1 && LimelightHelpers.getTA(limelightName) < multiTagAmbiguityThreshold) + double singleTagAreaThreshold = + Robot.gameMode == GameMode.AUTONOMOUS + ? CameraConstants.LIMELIGHT_3G_SINGLE_TA_CUTOFF_AUTO + : PoseCalculations.onOppositeSide(robotPoseSupplier.get().getX()) + ? CameraConstants.LIMELIGHT_3G_SINGLE_TA_CUTOFF_TELE_PASS + : CameraConstants.LIMELIGHT_3G_SINGLE_TA_CUTOFF_TELE; + + double doubleTagAreaThreshold = + Robot.gameMode == GameMode.AUTONOMOUS + ? CameraConstants.LIMELIGHT_3G_DOUBLE_TA_CUTOFF_AUTO + : CameraConstants.LIMELIGHT_3G_DOUBLE_TA_CUTOFF_TELE; + + if (!inputs.validResult + || (inputs.limelightTA < singleTagAreaThreshold && inputs.targetIDs.length == 1) + || (inputs.targetIDs.length > 1 && inputs.limelightTA < doubleTagAreaThreshold) || (estimatedRobotPose.getX() == 0 && estimatedRobotPose.getY() == 0) || Double.isNaN(estimatedRobotPose.getX()) || Double.isNaN(estimatedRobotPose.getY()) || Double.isNaN(estimatedRobotPose.getRotation().getRadians()) - || targets.length == 0 - || Double.valueOf(targets[0].tx).equals(null) - || Double.valueOf(targets[0].ty).equals(null) - || !Double.isFinite(targets[0].tx) - || !Double.isFinite(targets[0].ty)) + || inputs.targetIDs.length == 0 + || Double.valueOf(inputs.targetTxs[0]).equals(null) + || Double.valueOf(inputs.targetTys[0]).equals(null) + || !Double.isFinite(inputs.targetTxs[0]) + || !Double.isFinite(inputs.targetTys[0])) { return false; } @@ -360,20 +343,17 @@ public boolean isConnected() { // Force an update and get current time timingTestEntryValue = !timingTestEntryValue; // flip test value timingTestEntry.setBoolean(timingTestEntryValue); - long currentTime = timingTestEntry.getLastChange(); - - // Get most recent update from limelight - long lastUpdate = LimelightHelpers.getLimelightNTTableEntry(limelightName, "tl").getLastChange(); + inputs.currentTime = timingTestEntry.getLastChange(); // Calculate limelights last update - timeDifference = currentTime - lastUpdate; + timeDifference = inputs.currentTime - inputs.lastUpdate; isConnected = timeDifference < CameraConstants.LIMELIGHT_MAX_UPDATE_TIME; return isConnected; } public Pose2d getRobotPoseTargetSpace() { - return LimelightHelpers.getBotPose3d_TargetSpace(limelightName).toPose2d(); + return inputs.botPose2dTargetSpace; } public Command blinkLeds(DoubleSupplier duration) { @@ -403,4 +383,39 @@ public void enableLEDS() { public void disableLEDS() { LimelightHelpers.setLEDMode_ForceOff(limelightName); } + + public void setPriorityTagID(int id) { + LimelightHelpers.setPriorityTagID(limelightName, id); + } + + @Override + public void updateInputs(LimelightIOInputs inputs) { + Results results = getResults(); + + inputs.validResult = results != null && results.valid; + + LimelightTarget_Fiducial[] targetFiducials = results.targets_Fiducials; + + inputs.targetTxs = new double[targetFiducials.length]; + inputs.targetTys = new double[targetFiducials.length]; + inputs.targetIDs = new int[results.targets_Fiducials.length]; + + int index = 0; + for (LimelightTarget_Fiducial target : targetFiducials) { + inputs.targetIDs[index] = (int) target.fiducialID; + inputs.targetTxs[index] = target.tx; + inputs.targetTys[index] = target.ty; + index++; + } + + inputs.botPose2d = results.getBotPose2d_wpiBlue(); + inputs.botPose3d = results.getBotPose3d_wpiBlue(); + inputs.botPose2dTargetSpace = LimelightHelpers.getBotPose3d_TargetSpace(limelightName).toPose2d(); + inputs.latencyPipeline = results.latency_pipeline; + inputs.latencyCapture = results.latency_capture; + inputs.limelightTA = LimelightHelpers.getTA(limelightName); + inputs.lastUpdate = LimelightHelpers.getLimelightNTTableEntry(limelightName, "tl").getLastChange(); + + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/LimelightIO.java b/src/main/java/frc/robot/subsystems/vision/LimelightIO.java new file mode 100644 index 00000000..525f4fa5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/LimelightIO.java @@ -0,0 +1,35 @@ +package frc.robot.subsystems.vision; + + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; + +public interface LimelightIO { + + @AutoLog + class LimelightIOInputs { + + public boolean validResult = true; + public int[] targetIDs = new int[0]; + public double[] targetTxs = new double[0]; + public double[] targetTys = new double[0]; + public double limelightTA = 0.0; + public Pose2d botPose2d = new Pose2d(); + public Pose3d botPose3d = new Pose3d(); + public Pose2d botPose2dTargetSpace = new Pose2d(); + public double fiducialID = 0.0; + public double latencyPipeline = 0.0; + public double latencyCapture = 0.0; + public double noteCalcY = 0.0; + public double noteCalcX = 0.0; + public long currentTime = 0; + public long lastUpdate = 0; + public int pipelineIndex = 0; + + } + + default void updateInputs(LimelightIOInputs inputs) {} + +} diff --git a/src/main/java/frc/robot/util/Constants.java b/src/main/java/frc/robot/util/Constants.java index 1955e93e..81e3fd77 100644 --- a/src/main/java/frc/robot/util/Constants.java +++ b/src/main/java/frc/robot/util/Constants.java @@ -34,7 +34,6 @@ import frc.robot.util.custom.PatrIDConstants; import frc.robot.util.custom.SpeedAngleTriplet; import frc.robot.util.rev.Neo; -import monologue.Logged; /** * Welcome to the home of the many many variables :D @@ -45,6 +44,49 @@ * or in a comment next to it. */ public final class Constants { + + public static final class LoggingConstants { + + private static RobotType robotType = RobotType.COMPBOT; + + public static RobotType getRobot() { + if (!FieldConstants.IS_SIMULATION && robotType == RobotType.SIMBOT) { + System.out.println("Incorrect robot type selected, changing to real robot"); + robotType = RobotType.COMPBOT; + } + return robotType; + } + + public static Mode getMode() { + return switch (getRobot()) { + case DEVBOT -> FieldConstants.IS_SIMULATION ? Mode.SIM : Mode.REAL; + case COMPBOT -> FieldConstants.IS_SIMULATION ? Mode.REPLAY : Mode.REAL; + case SIMBOT -> Mode.SIM; + }; + } + + public enum Mode { + /** Running on a real robot. */ + REAL, + + /** Running a physics simulator. */ + SIM, + /** Replaying from a log file. */ + REPLAY + } + + public enum RobotType { + DEVBOT, + COMPBOT, + SIMBOT + } + + public static final String SUBSYSTEM_LOG_PATH = "Subsystems/"; + public static final String CALC_LOG_PATH = "Calc/"; + public static final String MANAGER_LOG_PATH = "Managers/"; + + } + public static final class DriveConstants { // Driving Parameters - Note that these are not the maximum capable speeds of // the robot, rather the allowed maximum speeds @@ -54,6 +96,12 @@ public static final class DriveConstants { public static final double MAX_TELEOP_SPEED_METERS_PER_SECOND = 4.7; + public static final double PASS_ROTATION_DEADBAND = 10; + + public static final double PASS_ROBOT_VELOCITY_THRESHOLD_METERS_PER_SECOND = 0.75; + + public static final double ODOMETRY_FREQUENCY = 250.0; + // Chassis configuration // Distance between centers of right and left wheels on robot public static final double TRACK_WIDTH = Units.inchesToMeters(25.5); @@ -104,7 +152,7 @@ public static final class DriveConstants { public static final boolean GYRO_REVERSED = true; } - public static final class TuningConstants implements Logged { + public static final class TuningConstants { public final int DRIVE_INDEX = 0; public final int PIVOT_INDEX = 1; public final int SHOOTER_INDEX = 2; @@ -121,12 +169,13 @@ public static final class ShooterConstants { public static final double SHOOTER_VELOCITY_CONVERSION_FACTOR = 1.0; // degrees public static final double PIVOT_POSITION_CONVERSION_FACTOR = 360; + public static final double PIVOT_ZERO_OFFSET = 51.2235296; public static final PatrIDConstants SHOOTER_PID = new PatrIDConstants( 0.002, - 0, + 0.000000001, 0.006, - 0.0001762 + 0.000168 ); public static final PatrIDConstants PIVOT_PID = new PatrIDConstants( @@ -140,98 +189,62 @@ public static final class ShooterConstants { public static final double SHOOTER_BACK_SPEED = -0.5; - public static final double PIVOT_DEADBAND = 1; + public static final double PIVOT_DEADBAND_DEGREES = 1; + public static final double PIVOT_PASS_DEADBAND_DEGREES = 4; public static final double SHOOTER_RPM_DEADBAND = 50; + public static final double SHOOTER_PASS_RPM_DEADBAND = 150; public static final double PIVOT_LOWER_LIMIT_DEGREES = 21; - public static final double PIVOT_UPPER_LIMIT_DEGREES = 59; + public static final double PIVOT_UPPER_LIMIT_DEGREES = 56; public static final double PIVOT_RAISE_ANGLE_DEGREES = 49; + public static final double SLIDE_PASS_AVERAGE_RPM = 2500; + public static final SpeedAngleTriplet SHOOTER_AMP_TRIPLET = SpeedAngleTriplet.of(712.0, 554.0, 55.4); - public static final double SHOOTER_RPM_LOWER_LIMIT = -NeoMotorConstants.NEO_FREE_SPEED_RPM; - public static final double SHOOTER_RPM_UPPER_LIMIT = NeoMotorConstants.NEO_FREE_SPEED_RPM; + public static final double SHOOTER_RPM_LOWER_LIMIT = -NeoMotorConstants.VORTEX_FREE_SPEED_RPM; + public static final double SHOOTER_RPM_UPPER_LIMIT = NeoMotorConstants.VORTEX_FREE_SPEED_RPM; public static final double DEFAULT_RPM = 2500; - - public static final double SHOOTER_PASS_SECONDS = 2; public static final double MEASUREMENT_INTERVAL_FEET = 1.0; + + public static final double TUNED_SHOOTER_MAX_DISTANCE = 19.0; // update accordingly with map + /** - * The distances are in feet, the speeds are in RPM, and the angles are in - * degrees. * The distances are in feet, the speeds are in RPM, and the angles are in * degrees. */ - public static final HashMap SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP = new HashMap() { + public static final HashMap SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP = new HashMap() { { - put(4, SpeedAngleTriplet.of(1763.0, 2316.0, 56.1)); - put(5, SpeedAngleTriplet.of(1763.0, 2316.0, 57.6)); - // put(6, SpeedAngleTriplet.of(2088.0, 2088.0, 50.0)); - // put(7, SpeedAngleTriplet.of(2188.0, 2188.0, 45.7)); - // put(8, SpeedAngleTriplet.of(2313.0, 2313.0, 41.3)); - // put(9, SpeedAngleTriplet.of(2465.0, 2465.0, 40.5)); - // put(10, SpeedAngleTriplet.of(2633.0, 2633.0, 38.1)); - // put(11, SpeedAngleTriplet.of(2795.0, 2795.0, 35.8)); - // put(12, SpeedAngleTriplet.of(2993.0, 2993.0, 34.3)); - // put(13, SpeedAngleTriplet.of(3526.0, 3526.0, 32.3)); - // put(14, SpeedAngleTriplet.of(3561.0, 3561.0, 31.0)); - // put(15, SpeedAngleTriplet.of(3756.0, 3756.0, 29.9)); - // put(16, SpeedAngleTriplet.of(3928.0, 3928.0, 30.0)); - // put(17, SpeedAngleTriplet.of(3928.0, 3928.0, 29.1)); - - // V2 - // put(6, SpeedAngleTriplet.of(2127.0, 2127.0, 48.3)); - // put(7, SpeedAngleTriplet.of(2127.0, 2127.0, 46.0)); - // put(8, SpeedAngleTriplet.of(2528.0, 2528.0, 41.9)); - // put(9, SpeedAngleTriplet.of(2600.0, 2600.0, 37.5)); - // put(10, SpeedAngleTriplet.of(2884.0, 2894.0, 37.1)); - // put(11, SpeedAngleTriplet.of(2924.0, 2930.0, 33.3)); - // put(12, SpeedAngleTriplet.of(2924.0, 2930.0, 32.3)); - // // V3 (note inside of indexer) - // put(13, SpeedAngleTriplet.of(3311.0, 3034.0, 30.3)); - // put(14, SpeedAngleTriplet.of(3589.0, 3312.0, 30.3)); - // // 2/28/24 - this was the day after bands were added - // put(15, SpeedAngleTriplet.of(3726.0, 3600.0, 27.3)); - - // put(16, SpeedAngleTriplet.of(3986.0, 3990.0, 33.7)); - // put(17, SpeedAngleTriplet.of(3986.0, 3990.0, 33)); - // put(18, SpeedAngleTriplet.of(3986.0, 3990.0, 32.5)); - // put(19, SpeedAngleTriplet.of(4201.0, 4205.0, 32.0)); - - // put(7, SpeedAngleTriplet.of(2840.0, 2850.0, 44.6)); - // put(8, SpeedAngleTriplet.of(2810.0, 2820.0, 39.5)); - // put(9, SpeedAngleTriplet.of(2886.0, 2886.0, 38.4)); - // put(10, SpeedAngleTriplet.of(2888.0, 2897.0, 36.6)); - // // Driverstation has some words for you: - // // 樀愀瘀愀㨀㐀㄀㈀⤀㨀 䰀漀漀瀀 琀椀洀攀 漀昀 　⸀　㈀猀 漀瘀攀爀爀甀渀 ＀෾਀＀￾￾￾￾￾⃾ ＀෾਀＀ - // put(11, SpeedAngleTriplet.of(2943.0, 2930.0, 36)); - // put(12, SpeedAngleTriplet.of(2943.0, 2935.0, 35.9)); - // put(13, SpeedAngleTriplet.of(3319.0, 3042.0, 35.3)); - // put(14, SpeedAngleTriplet.of(3587.0, 3310.0, 34.7)); - // put(15, SpeedAngleTriplet.of(3586.0, 3371.0, 34)); - - // NEW SHOOTER WOOOHOO - put(6, SpeedAngleTriplet.of(3007.0, 2850.0, 50.1)); - put(7, SpeedAngleTriplet.of(3160.0, 2865.0, 46.3)); - put(8, SpeedAngleTriplet.of(3310.0, 3017.0, 43.4)); - put(9, SpeedAngleTriplet.of(3502.0, 3202.0, 40.2)); - put(10, SpeedAngleTriplet.of(3706.0, 3305.0, 37.8)); - put(11, SpeedAngleTriplet.of(3856.0, 3539.0, 34.5)); - put(12, SpeedAngleTriplet.of(3921.0, 3558.0, 33.3)); - put(13, SpeedAngleTriplet.of(4075.0, 3691.0, 30.9)); - // Future note, 13.2ft is a common shot which should have its own calibration point - put(14, SpeedAngleTriplet.of(4190.0, 3731.0, 29.4)); - put(15, SpeedAngleTriplet.of(4531.0, 4058.0, 28)); - put(16, SpeedAngleTriplet.of(4190.0, 3731.0, 26.7)); - + + // On Mark, Rudy, and Aidan these speeds are perfect + + // **** GOOD SPEEDS **** + // PRE-BEACH + put(4.0, SpeedAngleTriplet.of(1971.0, 2524.0, 56)); + put(5.0, SpeedAngleTriplet.of(1971.0, 2524.0, 52.7)); + put(6.0, SpeedAngleTriplet.of(2084.0, 2634.0, 48.5)); + put(7.0, SpeedAngleTriplet.of(2089.0, 2639.0, 44.5)); + put(8.0, SpeedAngleTriplet.of(2375.0, 2925.0, 39.6)); + put(9.0, SpeedAngleTriplet.of(2574.0, 3124.0, 36.0)); + put(10.0, SpeedAngleTriplet.of(2700.0, 3300.0, 34.7)); + put(11.0, SpeedAngleTriplet.of(3009.0, 3611.0, 33.1)); + put(12.0, SpeedAngleTriplet.of(3196.0, 3798.0, 31.3)); + put(13.0, SpeedAngleTriplet.of(3532.0, 4118.0, 28.9)); + put(14.0, SpeedAngleTriplet.of(3690.0, 4205.0, 28.0)); + put(15.0, SpeedAngleTriplet.of(3860.0, 4507.0, 27.2)); + put(16.0, SpeedAngleTriplet.of(4045.0, 4692.0, 26.7)); + put(17.0, SpeedAngleTriplet.of(4163.0, 4810.0, 26.1)); + put(18.0, SpeedAngleTriplet.of(4306.0, 4955.0, 24.8)); + put(19.0, SpeedAngleTriplet.of(4389.0, 5038.0, 24.0)); } }; public static final InterpolatingTreeMap INTERPOLATION_MAP = new InterpolatingTreeMap<>( InverseInterpolator.forDouble(), SpeedAngleTriplet.getInterpolator()) {{ - for (Map.Entry entry : SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP.entrySet()) { + for (Map.Entry entry : SPEAKER_DISTANCES_TO_SPEEDS_AND_ANGLE_MAP.entrySet()) { put(entry.getKey().doubleValue(), entry.getValue()); } }}; @@ -262,7 +275,7 @@ public static final class ElevatorConstants { public static final PatrIDConstants ELEVATOR_PID = new PatrIDConstants(10, 0, 0); // TODO: set these values - public static final double BOTTOM_POS = -.005; + public static final double BOTTOM_POS = 0; public static final double INTAKE_TIME = 0; public static final double AMPPER_OUTTAKE_PERCENT = -1; public static final double AMPPER_INTAKE_PERCENT = 1; @@ -272,8 +285,8 @@ public static final class ElevatorConstants { public static final double DEBUG_ELEVATOR_POS = 0.47; // This position is not used public static final double AMP_PLACE_POS = 0.409; - public static final double NOTE_FIX_POS = 0.096; - public static final double INDEX_POS = 0.09; + public static final double NOTE_FIX_POS = 0.086; + public static final double INDEX_POS = 0.14; public static final double DROP_POS = 0.11; public static final double GUILLOTONE_POS = 0.224; public static final double UNSTUCK_POS = 0.175; @@ -283,8 +296,8 @@ public static final class ElevatorConstants { public static final double STUCK_TIME_SECONDS = 0.25; public static final double UNSTUCK_OUTTAKE_TIME_SECONDS = 0.3; - public static final double ELEVATOR_TOP_LIMIT = 0.49; - public static final double ELEVATOR_BOTTOM_LIMIT = -0.0254; + public static final double ELEVATOR_TOP_LIMIT = 0.475; + public static final double ELEVATOR_BOTTOM_LIMIT = 0; public static final double AMPPER_LOWER_PERCENT_LIMIT = -1; public static final double AMPPER_UPPER_PERCENT_LIMIT = 1; @@ -317,25 +330,28 @@ public static final class ClimbConstants { public static final double TOP_LIMIT = 0.517; public static final double BOTTOM_LIMIT = 0.0; - public static final double CLIMB_DEADBAND = 0.002; + public static final double CLIMB_DEADBAND = 0.005; public static final double DISTANCE_FROM_ORIGIN_METERS = 0.3048; } public static final class AutoConstants { - // The below values need to be tuned for each new robot. + // The below values need to be tuned for each new robot.. // They are currently set to the values suggested by Choreo - public static final double MAX_SPEED_METERS_PER_SECOND = 5; - public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 5; + public static final double MAX_SPEED_METERS_PER_SECOND = 4.5; + public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 3.5; // Below is gotten from choreo public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = Units.degreesToRadians(1137.21); public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED = Units.degreesToRadians(792.90); public static final double AUTO_POSITION_TOLERANCE_METERS = Units.inchesToMeters(1); - public static final double AUTO_POSITION_TOLERANCE_RADIANS = Units.degreesToRadians(3); + public static final double AUTO_ROTATION_TOLERANCE_RADIANS = Units.degreesToRadians(2); + public static final double PASS_ROTATION_TOLERANCE_RADIANS = Units.degreesToRadians(10); + + public static final double AUTO_ALIGNMENT_DEADBAND = Units.inchesToMeters(1.5); - public static final double AUTO_ALIGNMENT_DEADBAND = Units.inchesToMeters(3); + public static final double UNDER_STAGE_DEADBAND = 0.02; /* * XY: @@ -352,12 +368,13 @@ public static final class AutoConstants { public static final double XY_CORRECTION_I = 0.0125; public static final double XY_CORRECTION_D = 0.0125; - private static final PIDController XY_PID = new PIDController( + public static final PIDController XY_PID = new PIDController( AutoConstants.XY_CORRECTION_P, 0, AutoConstants.XY_CORRECTION_D); public static final double ROTATION_CORRECTION_P = 3.725; + public static final double ROTATION_CORRECTION_P_TELE = 5.75; public static final double ROTATION_CORRECTION_I = 0; public static final double ROTATION_CORRECTION_D = 0; @@ -365,8 +382,8 @@ public static final class AutoConstants { public static final boolean USE_OBJECT_DETECTION = true; - private static final ProfiledPIDController THETA_PID = new ProfiledPIDController( - AutoConstants.ROTATION_CORRECTION_P, + public static final ProfiledPIDController THETA_PID = new ProfiledPIDController( + AutoConstants.ROTATION_CORRECTION_P_TELE, AutoConstants.ROTATION_CORRECTION_I, AutoConstants.ROTATION_CORRECTION_D, new TrapezoidProfile.Constraints( @@ -415,21 +432,25 @@ public static final class AutoConstants { public static final String SKIPPING_UP_PATH_NAME = "C5-1" + PATH_EXTENSION; public static final String[] AUTO_NAMES = new String[] { - // "S C5-4 S OBJ", - "S C5-1 S", - "S W3-1 S C2-5 S", - "S W3-1 S C2-5 S 2", - - "S W3-1 S C1-2 S", - "S W3-1 S C1-2 S 2", - // "S W3-1 S C2-3 S 2 OBJ", - // "S W3-1 S C2-3 S OBJ", - // "S W3-1 C1-3 OBJ", - // "S C1-3 OBJ Over W1", - // "S C1-5 C5-1 OBJ Over W1", - "S C1-5 S Over W1", - // "S C5-1 C1-5 OBJ", - // "Proxy" + // "S C2-3 S", // HOME AUTO + // "S C5-1 S", + // "S C5-3 3-5 S", + // "S W3-1 S C2-5 S", + // "S W3-1 S C2-3 S", // HOME AUTO + // "S W3-1 S C3-5 S", + // "S W3-1 S", + // "S W2 S C3-5 S", // UNTESTED + // "S W2 S C1-5 S", // UNTESTED + // "X-MEN", // UNTESTED + // "S W2 S C1-2 S W1 S C3-5 S", // UNTESTED + "S C1-5 S Over W1 DEF", + "S W1 S C1-5 S DEF", + "S W3-1 S C1-5 S DEF", + "S W2 S C3-5 S DEF", + "S W3-1 S C2-5 S DEF", + "S W1 S C2-5 S DEF", + "S C5-1 S DEF", + "S C4-1 S DEF" }; } @@ -489,7 +510,7 @@ private enum SwerveGearing { // Calculations required for driving motor conversion factors and feed forward public static final double DRIVING_MOTOR_FREE_SPEED_RPS = NeoMotorConstants.VORTEX_FREE_SPEED_RPM / 60; - public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(1.468519624221792*2.0); + public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(1.4791980575846997*2.0); public static final double WHEEL_CIRCUMFERENCE_METERS = WHEEL_DIAMETER_METERS * Math.PI; // 45 teeth on the wheel's bevel gear, 15 teeth on the bevel pinion public static final double DRIVING_MOTOR_REDUCTION = (45.0 * CURRENT_GEARING.spurTeeth) / (CURRENT_GEARING.pinionTeeth * 15.0); @@ -541,6 +562,12 @@ private enum SwerveGearing { public static final int NEO_CURRENT_LIMIT = 50; // amps public static final int VORTEX_CURRENT_LIMIT = 80; // amps public static final int TURNING_MOTOR_CURRENT_LIMIT = 20; // amps + + public static final int FRONT_LEFT_INDEX = 0; + public static final int FRONT_RIGHT_INDEX = 1; + public static final int REAR_LEFT_INDEX = 2; + public static final int REAR_RIGHT_INDEX = 3; + } public static final class OIConstants { @@ -559,7 +586,15 @@ public static final class OIConstants { public static final double CONTROLLER_CORNER_SLOPE_1 = 1 / 0.7; public static final double CONTROLLER_CORNER_SLOPE_2 = 0.7; - public static final boolean SINGLE_DRIVER_MODE = true; + public static final DriverMode DRIVER_MODE = DriverMode.DOUBLE; + + public enum DriverMode { + SINGLE, + DOUBLE, + DEV + } + + public static boolean OPERATOR_PRESENT = DRIVER_MODE == DriverMode.DOUBLE; } public static final class LEDConstants { @@ -607,7 +642,6 @@ public static final class NeoMotorConstants { public static final int MAX_PERIODIC_STATUS_TIME_MS = 32766; public static final int FAST_PERIODIC_STATUS_TIME_MS = 15; - // This gets filled out as motors are created on the robot public static final HashMap MOTOR_MAP = new HashMap(); @@ -704,7 +738,10 @@ public static final class FieldConstants { public static final double FIELD_HEIGHT_METERS = 8.2112312; public static final double CHAIN_HEIGHT_METERS = Units.feetToMeters(4); public static final double SPEAKER_HEIGHT_METERS = 2.082813; - public static final double PASS_HEIGHT_METERS = Units.feetToMeters(13.5); + + public static final double PASS_HEIGHT_METERS = Units.feetToMeters(9.5); + + public static final int[] SPEAKER_MID_TAGS = {4, 7}; // Field: // https://cad.onshape.com/documents/dcbe49ce579f6342435bc298/w/b93673f5b2ec9c9bdcfec487/e/6ecb2d6b7590f4d1c820d5e3 @@ -764,6 +801,28 @@ public static final class FieldConstants { add(GeometryUtil.flipFieldPose(blueSubwoofer)); }}; + public static final List SAMPLE_CENTER_PASS_POSITIONS = new ArrayList() {{ + // All points are in meters and radians + // All relative to the blue origin + // Blue Source + Pose2d bluePos = new Pose2d(8.268, 5.736, Rotation2d.fromRadians(2.858)); + add(bluePos); + + // Red Source + add(GeometryUtil.flipFieldPose(bluePos)); + }}; + + public static final List SAMPLE_SOURCE_PASS_POSITIONS = new ArrayList() {{ + // All points are in meters and radians + // All relative to the blue origin + // Blue Source + Pose2d bluePos = new Pose2d(15.482, 1.143, Rotation2d.fromRadians(2.689)); + add(bluePos); + + // Red Source + add(GeometryUtil.flipFieldPose(bluePos)); + }}; + public static final List CHAIN_POSE3DS = new ArrayList() {{ // All points are in meters and radians // All relative to the blue origin @@ -781,6 +840,23 @@ public static final class FieldConstants { add(new Pose3d(GeometryUtil.flipFieldPose(blueChain3.toPose2d())).plus(new Transform3d(0.0, 0.0, CHAIN_HEIGHT_METERS, new Rotation3d()))); }}; + public static final List STAGE_POINTS = new ArrayList() {{ + + // Blue points + Translation2d bluePoint1 = new Translation2d(3.026, 4.064); + Translation2d bluePoint2 = new Translation2d(5.801, 2.507); + Translation2d bluePoint3 = new Translation2d(5.801, 5.667); + add(bluePoint1); + add(bluePoint2); + add(bluePoint3); + + // Red points + add(GeometryUtil.flipFieldPosition(bluePoint1)); + add(GeometryUtil.flipFieldPosition(bluePoint2)); + add(GeometryUtil.flipFieldPosition(bluePoint3)); + + }}; + // Speaker Positions: Blue alliance left // @formatter:off // @@ -801,52 +877,84 @@ public static final class FieldConstants { public static final List AMP_POSITIONS = new ArrayList() {{ // All points are in meters and radians // All relative to the blue origin - Pose2d bluePose = new Pose2d(1.827, FIELD_HEIGHT_METERS, Rotation2d.fromDegrees(-90)); + Pose2d bluePose = new Pose2d(1.807, FIELD_HEIGHT_METERS, Rotation2d.fromDegrees(-90)); Pose2d redPose = GeometryUtil.flipFieldPose(bluePose); add(bluePose); add(redPose); }}; - - public static final List PASS_APEX_POSITIONS = new ArrayList() {{ + + + public static final List CENTER_PASS_TARGET_POSITIONS = new ArrayList() {{ // I swear bulldogs and hawaiin kids just had to get in here somehow - Pose2d bluePose = new Pose2d(4.581, 4.359, Rotation2d.fromDegrees(0)); + Pose2d bluePose = new Pose2d(0.630, 6.405, Rotation2d.fromDegrees(0)); Pose2d redPose = GeometryUtil.flipFieldPose(bluePose).plus(new Transform2d(0, 0, Rotation2d.fromDegrees(180))); add(bluePose); add(redPose); }}; - public static final List PASS_TARGET_POSITIONS = new ArrayList() {{ + public static final List SOURCE_PASS_TARGET_POSITIONS = new ArrayList() {{ // I swear bulldogs and hawaiin kids just had to get in here somehow - Pose2d bluePose = new Pose2d(1.07, 6.99, Rotation2d.fromDegrees(0)); + Pose2d bluePose = new Pose2d(7.068, 7.266, Rotation2d.fromDegrees(0)); Pose2d redPose = GeometryUtil.flipFieldPose(bluePose).plus(new Transform2d(0, 0, Rotation2d.fromDegrees(180))); add(bluePose); add(redPose); }}; + public static final List PRESET_SHOT_POSITIONS = new ArrayList() {{ + Pose2d orbitBluePose = new Pose2d(3.653, 5.957, new Rotation2d(-2.929)); + Pose2d podiumBlueShotSpot = new Pose2d(2.425, 4.416, new Rotation2d(2.614)); + add(podiumBlueShotSpot); + + add(new Pose2d(0, 0, new Rotation2d(0))); + add(orbitBluePose); + add(GeometryUtil.flipFieldPose(orbitBluePose)); + add(GeometryUtil.flipFieldPose(podiumBlueShotSpot)); + }}; + + public static final List STEAL_BOX_CORNERS = new ArrayList() {{ + Translation2d bluePose = new Translation2d(2.658, 2.215); + Translation2d redPose = GeometryUtil.flipFieldPosition(bluePose); + + add(bluePose); + add(redPose); + }}; + private static int getAllianceIndex(Alliance defaultAlliance) { return defaultAlliance == Alliance.Blue ? (Robot.isRedAlliance() ? 1 : 0) : (Robot.isBlueAlliance() ? 1 : 0); } + public static Pose2d GET_SPEAKER_POSITION() { return SPEAKER_POSITIONS.get(getAllianceIndex(Alliance.Blue)); } + public static List GET_PRESET_SHOT_POSES() { + return PRESET_SHOT_POSITIONS; + } public static Pose2d GET_SUBWOOFER_POSITION() { return SUBWOOFER_POSITIONS.get(getAllianceIndex(Alliance.Blue)); } - public static Pose2d GET_PASS_APEX_POSITION() { - return PASS_APEX_POSITIONS.get(getAllianceIndex(Alliance.Blue)); + public static Pose2d GET_SAMPLE_CENTER_PASS_POSITION() { + return SAMPLE_CENTER_PASS_POSITIONS.get(getAllianceIndex(Alliance.Blue)); + } + + public static Pose2d GET_SAMPLE_SOURCE_PASS_POSITION() { + return SAMPLE_SOURCE_PASS_POSITIONS.get(getAllianceIndex(Alliance.Blue)); + } + + public static Pose2d GET_CENTER_PASS_TARGET_POSITION() { + return CENTER_PASS_TARGET_POSITIONS.get(getAllianceIndex(Alliance.Blue)); } - public static Pose2d GET_PASS_TARGET_POSITION() { - return PASS_TARGET_POSITIONS.get(getAllianceIndex(Alliance.Blue)); + public static Pose2d GET_SOURCE_PASS_TARGET_POSITION() { + return SOURCE_PASS_TARGET_POSITIONS.get(getAllianceIndex(Alliance.Blue)); } public static Pose2d GET_SOURCE_POSITION() { @@ -870,6 +978,15 @@ public static List GET_CHAIN_POSITIONS() { return CHAIN_POSITIONS.subList(startIndex, startIndex + 3); } + public static List GET_STAGE_POINTS() { + int startIndex = Robot.isRedAlliance() ? 3 : 0; + return STAGE_POINTS.subList(startIndex, startIndex + 3); + } + + public static Translation2d GET_STEAL_BOX_CORNER() { + return STEAL_BOX_CORNERS.get(getAllianceIndex(Alliance.Blue)); + } + public static final double CHAIN_LENGTH_METERS = Units.inchesToMeters(100); public static double CENTERLINE_X = FIELD_WIDTH_METERS / 2.0; @@ -975,9 +1092,9 @@ public static List GET_CHAIN_POSITIONS() { }}; // 12.4ft from the speaker - public static final Translation2d PODIUM_SHOT_SPOT = new Translation2d(2.55, 4.19); + public static final Pose2d PODIUM_SHOT_SPOT = new Pose2d(2.55, 4.19, new Rotation2d(0)); // 9.76ft from the speaker - public static final Translation2d ORBIT_POSE = new Translation2d(2.884, 6.304); + public static final Pose2d ORBIT_POSE = new Pose2d(2.884, 6.304, new Rotation2d(0)); public static final List SHOOTING_POSITIONS = new ArrayList() {{ // We only want the blue alliance speaker translation since @@ -1012,7 +1129,9 @@ public static List GET_CHAIN_POSITIONS() { }}; // Within a range of the [red circle](https://www.desmos.com/calculator/cu3ocssv5d) - public static final double AUTOMATIC_SHOOTER_DISTANCE_RADIUS = 8.5; + public static final double AUTOMATIC_SHOOTER_DISTANCE_RADIUS_METERS = 8.5; + + public static final double SPEAKER_CLEANUP_DISTANCE_METERS = 3.4; public static List GET_SHOOTING_POSITIONS() { int startingIndex = Robot.isRedAlliance() ? SHOOTING_POSITIONS.size() / 2 : 0; @@ -1068,22 +1187,30 @@ public static final class CameraConstants { public static final long LIMELIGHT_MAX_UPDATE_TIME = 200_000; // Micro Seconds = 0.2 Seconds + public static final double LIMELIGHT_3G_DOUBLE_TA_CUTOFF_AUTO = 0.070; + public static final double LIMELIGHT_3G_DOUBLE_TA_CUTOFF_TELE = 0.050 + ; + + public static final double LIMELIGHT_3G_SINGLE_TA_CUTOFF_AUTO = 0.175; + public static final double LIMELIGHT_3G_SINGLE_TA_CUTOFF_TELE = 0.141; + public static final double LIMELIGHT_3G_SINGLE_TA_CUTOFF_TELE_PASS = 0.141; + public static final Pose3d LL3Pose = new Pose3d( - // forward positive, right positive, up positive - 0.204, - -0.234, + // forward positive, right positive, up positive+ + 0.2872, // Mind control ll pose + 0.234, 0.651, - new Rotation3d(0, Units.degreesToRadians(13), 0)); + new Rotation3d(0, Units.degreesToRadians(15), 0)); public static final Pose3d LL2Pose = new Pose3d( // forward positive, right positive, up positive -0.272724, -0.311903, - 0.433974, + 0.651, // Pitch angle is measure from the horizontal // it is negative because it points down - new Rotation3d(0, Units.degreesToRadians(-11), Units.degreesToRadians(180))); + new Rotation3d(0, Units.degreesToRadians(15), Units.degreesToRadians(180))); public static final Pose3d[] cameras = new Pose3d[] {LL3Pose, LL2Pose}; } @@ -1122,7 +1249,8 @@ public static final class NTConstants { put("dropPiece1", 0.5); put("ampPosition", 0.37); - put("atan++", 0.86); + put("centerAtan++", 0.86); + put("sourceAtan++", 0.86); put("placeOuttake", 0.6995); put("prepPiece", 0.3); diff --git a/src/main/java/frc/robot/util/auto/ChoreoStorage.java b/src/main/java/frc/robot/util/auto/ChoreoStorage.java index 63658679..c8f85def 100644 --- a/src/main/java/frc/robot/util/auto/ChoreoStorage.java +++ b/src/main/java/frc/robot/util/auto/ChoreoStorage.java @@ -10,8 +10,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.FieldConstants; -import monologue.Logged; -import monologue.Annotations.Log; import java.util.HashMap; import java.util.Map; @@ -24,11 +22,10 @@ * with each segment having its own method * to make sure that the modularity stays clean */ -public class ChoreoStorage implements Logged { +public class ChoreoStorage { private final BooleanSupplier hasPieceSupplier; private final Map pathCache = new HashMap<>(); - @Log.NT private SendableChooser autoChooser = new SendableChooser<>(); /** diff --git a/src/main/java/frc/robot/util/auto/PathPlannerStorage.java b/src/main/java/frc/robot/util/auto/PathPlannerStorage.java index 3efd6d12..be21f9c8 100644 --- a/src/main/java/frc/robot/util/auto/PathPlannerStorage.java +++ b/src/main/java/frc/robot/util/auto/PathPlannerStorage.java @@ -18,22 +18,19 @@ import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.calc.PoseCalculations; -import frc.robot.util.custom.PatriSendableChooser; import frc.robot.Robot; import frc.robot.RobotContainer; import frc.robot.Robot.GameMode; -import frc.robot.subsystems.Limelight; -import frc.robot.subsystems.Swerve; -import monologue.Logged; -import monologue.Monologue; -import monologue.Annotations.IgnoreLogged; -import monologue.Annotations.Log; +import frc.robot.subsystems.drive.Swerve; +import frc.robot.subsystems.vision.Limelight; import java.util.ArrayList; import java.util.HashMap; import java.util.List; import java.util.function.BooleanSupplier; import java.util.function.Consumer; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; + /** * This file represents all of the auto paths that we will have * They will be primarily compiled through @@ -41,20 +38,19 @@ * with each segment having its own method * to make sure that the modularity stays clean */ -public class PathPlannerStorage implements Logged { +public class PathPlannerStorage { - private final BooleanSupplier colorSensorSupplier; - - @Log.NT - private PatriSendableChooser autoChooser = new PatriSendableChooser<>(); + private final BooleanSupplier shooterSensor; + private final BooleanSupplier elevatorSensor; + + private final LoggedDashboardChooser autoChooser = new LoggedDashboardChooser<>("Auto Routine"); public static final ArrayList AUTO_STARTING_POSITIONS = new ArrayList(); public static List NOTE_POSES = FieldConstants.GET_CENTERLINE_NOTES(); - @IgnoreLogged private Swerve swerve; - @IgnoreLogged + private Limelight limelight; public static final PathConstraints PATH_CONSTRAINTS = @@ -70,8 +66,16 @@ public class PathPlannerStorage implements Logged { * @param hasPieceSupplier A supplier that returns whether or not the robot has a piece. * This could be a sensor, motor current, or other system. */ - public PathPlannerStorage(BooleanSupplier colorSensorSupplier, Swerve swerve, Limelight limelight) { - this.colorSensorSupplier = colorSensorSupplier; + public PathPlannerStorage(BooleanSupplier shooterSensorSupplier, BooleanSupplier elevatorSensorSupplier, Swerve swerve, Limelight limelight) { + this.shooterSensor = shooterSensorSupplier; + this.elevatorSensor = elevatorSensorSupplier; + this.swerve = swerve; + this.limelight = limelight; + } + + public PathPlannerStorage(BooleanSupplier simulatedSensorSupplier, Swerve swerve, Limelight limelight) { + this.shooterSensor = simulatedSensorSupplier; + this.elevatorSensor = simulatedSensorSupplier; this.swerve = swerve; this.limelight = limelight; } @@ -111,36 +115,40 @@ public void configureAutoChooser() { } public Command getSelectedAuto() { - return autoChooser.getSelected(); + return autoChooser.get(); } public String getSelectedAutoName() { - return autoChooser.getSelectedName(); + return autoChooser.getSendableChooser().getSelected(); } - public void bindListener(Consumer consumer) { - autoChooser.onChange(consumer); + public void bindListener(Consumer consumer) { + autoChooser.getSendableChooser().onChange(consumer); } - public PatriSendableChooser getAutoChooser() { + public LoggedDashboardChooser getAutoChooser() { return this.autoChooser; } public Command updatePathViewerCommand() { return Commands.either( Commands.runOnce(() -> { + List autoPoses = getAutoPoses(getSelectedAutoName()); RobotContainer.field2d.getObject("path") - .setPoses(getAutoPoses(getSelectedAutoName())); + .setPoses(autoPoses); + if (autoPoses.size() > 0) + RobotContainer.autoStartingPose = autoPoses.get(0); }), Commands.runOnce(() -> { RobotContainer.field2d.getObject("path").setPoses(new ArrayList<>()); + RobotContainer.autoStartingPose = new Pose2d(); }), () -> Robot.gameMode == GameMode.DISABLED ).ignoringDisable(true); } - private Consumer getUpdatePathViewerCommand() { - return (command) -> { + private Consumer getUpdatePathViewerCommand() { + return (string) -> { updatePathViewerCommand().schedule(); }; } @@ -189,12 +197,24 @@ public Command generateCenterLogicOBJ(int startingNote, int endingNote, Swerve s return commandGroup; } + public Command generateCenterLogicDefault(int startingNote, int endingNote, Swerve swerve) { + SequentialCommandGroup commandGroup = new SequentialCommandGroup(); + boolean goingDown = startingNote < endingNote; + int increment = goingDown ? 1 : -1; + + for (int i = startingNote; (goingDown && i <= endingNote) || (!goingDown && i >= endingNote); i += increment) { + commandGroup.addCommands(generateCenterDefaultCommand(i, endingNote, goingDown, increment, commandGroup)); + } + + return commandGroup; + } + /** * Generates a command for the object detection scenario. */ private Command generateObjectDetectionCommand(int i, int endingNote, boolean goingDown, SequentialCommandGroup commandGroup) { int currentIndex = i - 1; - int nextIndex = currentIndex + (goingDown ? 1 : -1); + int nextIndex = currentIndex + (goingDown ? 1 : -1); if ((goingDown && i < endingNote) || (!goingDown && i > endingNote)) { return Commands.defer( @@ -202,7 +222,7 @@ private Command generateObjectDetectionCommand(int i, int endingNote, boolean go goToNote() .andThen(pathfindToShoot() .deadlineWith(NamedCommands.getCommand("ToIndexer") - .onlyIf(() -> !colorSensorSupplier.getAsBoolean())) + .onlyIf(() -> !shooterSensor.getAsBoolean())) .andThen(Commands.runOnce(swerve::stopDriving).andThen(NamedCommands.getCommand("PrepareSWD")) .raceWith(NamedCommands.getCommand("ShootInstantlyWhenReady"))) .andThen(pathfindToNextNote(nextIndex, goingDown))), @@ -216,7 +236,7 @@ private Command generateObjectDetectionCommand(int i, int endingNote, boolean go goToNote(), pathfindToShoot() .deadlineWith(NamedCommands.getCommand("ToIndexer") - .onlyIf(() -> !colorSensorSupplier.getAsBoolean())) + .onlyIf(() -> !shooterSensor.getAsBoolean())) .andThen(Commands.runOnce(swerve::stopDriving).andThen(NamedCommands.getCommand("PrepareSWD")) .raceWith(NamedCommands.getCommand("ShootInstantlyWhenReady"))) ), @@ -229,7 +249,7 @@ private Command generateObjectDetectionCommand(int i, int endingNote, boolean go goToNote(), pathfindToShoot() .deadlineWith(NamedCommands.getCommand("ToIndexer") - .onlyIf(() -> !colorSensorSupplier.getAsBoolean())) + .onlyIf(() -> !shooterSensor.getAsBoolean())) .andThen(Commands.runOnce(swerve::stopDriving).andThen(NamedCommands.getCommand("PrepareSWD")) .raceWith(NamedCommands.getCommand("ShootInstantlyWhenReady"))) ) @@ -249,12 +269,16 @@ private Command generateNonObjectDetectionCommand(int i, int endingNote, boolean if (i == FieldConstants.CENTER_NOTE_COUNT && goingDown || i == 1 && !goingDown || i == endingNote) { return Commands.defer(() -> - AutoBuilder.followPath(shootNote) - .deadlineWith(NamedCommands.getCommand("StopIntake") + Commands.sequence( + Commands.deadline( + AutoBuilder.followPath(shootNote), + NamedCommands.getCommand("StopIntake") .andThen(NamedCommands.getCommand("ToIndexer") - .onlyIf(() -> !colorSensorSupplier.getAsBoolean()))) - .andThen(NamedCommands.getCommand("ShootInstantlyWhenReady")) - .deadlineWith(NamedCommands.getCommand("PrepareSWD")).onlyIf(colorSensorSupplier), + .onlyIf(() -> !shooterSensor.getAsBoolean())), + NamedCommands.getCommand("PrepareSWD") + ), + NamedCommands.getCommand("ShootInstantlyWhenReady") + ).onlyIf(() -> shooterSensor.getAsBoolean() || elevatorSensor.getAsBoolean()), commandGroup.getRequirements()); } @@ -262,40 +286,85 @@ private Command generateNonObjectDetectionCommand(int i, int endingNote, boolean PathPlannerPath skipNote = PathPlannerPath.fromPathFile("C" + i + " C" + (i + increment)); Command shootAndMoveToNextNote = - AutoBuilder.followPath(shootNote) - .deadlineWith(NamedCommands.getCommand("StopIntake") + Commands.sequence( + Commands.deadline( + AutoBuilder.followPath(shootNote) + .raceWith(Commands.waitUntil(() -> !shooterSensor.getAsBoolean() && !elevatorSensor.getAsBoolean() && swerve.insideOwnWing())), + NamedCommands.getCommand("StopIntake") .andThen(NamedCommands.getCommand("ToIndexer") - .onlyIf(() -> !colorSensorSupplier.getAsBoolean()))) - .andThen(NamedCommands.getCommand("ShootInstantlyWhenReady")) - .deadlineWith(NamedCommands.getCommand("PrepareSWD")) - .raceWith(Commands.waitUntil(() -> !colorSensorSupplier.getAsBoolean() && swerve.insideOwnWing())) - - .andThen( - Commands.race( - Commands.sequence( - AutoBuilder.followPath(getNoteAfterShot), - Commands.waitSeconds(.3) - ), - Commands.sequence( - NamedCommands.getCommand("StopAll"), - Commands.waitSeconds(1), - NamedCommands.getCommand("ToIndexer") - ) + .onlyIf(() -> !shooterSensor.getAsBoolean())), + NamedCommands.getCommand("PrepareSWD") + ), + NamedCommands.getCommand("ShootInstantlyWhenReady"), + Commands.race( + Commands.sequence( + AutoBuilder.followPath(getNoteAfterShot), + Commands.waitSeconds(.3) + ), + Commands.sequence( + NamedCommands.getCommand("StopAll"), + Commands.waitSeconds(1), + NamedCommands.getCommand("ToIndexer") ) - ); + ) + ); Command skipNoteCommand = AutoBuilder.followPath(skipNote) .raceWith(NamedCommands.getCommand("ToIndexer")); - return Commands.waitUntil(colorSensorSupplier).withTimeout(0.45).andThen( + return Commands.waitUntil(() -> shooterSensor.getAsBoolean() || elevatorSensor.getAsBoolean()).withTimeout(0.4).andThen( Commands.defer(() -> Commands.either( shootAndMoveToNextNote, skipNoteCommand, - colorSensorSupplier), + () -> shooterSensor.getAsBoolean() || elevatorSensor.getAsBoolean()), commandGroup.getRequirements())); } + private Command generateCenterDefaultCommand(int i, int endingNote, boolean goingDown, int increment, SequentialCommandGroup commandGroup) { + String shootingLocation = (i < 3) ? "L" : (i == 3) ? "M" : "R"; + PathPlannerPath shootNote = PathPlannerPath.fromPathFile("C" + i + " " + shootingLocation); + + if (i == FieldConstants.CENTER_NOTE_COUNT && goingDown || i == 1 && !goingDown || i == endingNote) { + return Commands.defer(() -> + Commands.sequence( + Commands.deadline( + AutoBuilder.followPath(shootNote), + NamedCommands.getCommand("StopIntake") + .andThen(NamedCommands.getCommand("ToIndexer") + .onlyIf(() -> !shooterSensor.getAsBoolean())), + NamedCommands.getCommand("PrepareSWD") + ), + NamedCommands.getCommand("ShootInstantlyWhenReady2") + ), + commandGroup.getRequirements()); + } + + PathPlannerPath getNoteAfterShot = PathPlannerPath.fromPathFile(shootingLocation + " C" + (i + increment)); + + Command shootAndMoveToNextNote = + Commands.sequence( + Commands.deadline( + AutoBuilder.followPath(shootNote), + NamedCommands.getCommand("StopIntake") + .andThen(NamedCommands.getCommand("ToIndexer") + .unless(shooterSensor)), + NamedCommands.getCommand("PrepareSWD") + ), + NamedCommands.getCommand("ShootInstantlyWhenReady2"), + Commands.deadline( + AutoBuilder.followPath(getNoteAfterShot), + Commands.sequence( + NamedCommands.getCommand("StopAll"), + Commands.waitSeconds(1), + NamedCommands.getCommand("ToIndexer") + ) + ) + ); + + return Commands.defer(() -> shootAndMoveToNextNote, commandGroup.getRequirements()); + } + /** * Uses Pathplanner's pathfinding algorithm to go to the closest shooting position * from the swerve subsystem's current position @@ -312,7 +381,7 @@ public Command pathfindToShoot() { .raceWith( Commands.waitSeconds(1.5) .andThen( - Commands.waitUntil(() -> !colorSensorSupplier.getAsBoolean()) + Commands.waitUntil(() -> !shooterSensor.getAsBoolean() && !elevatorSensor.getAsBoolean()) ).alongWith( NamedCommands.getCommand("PrepareShooter" + PoseCalculations.getBestShootingPoseString(swerve.getPose())) ) @@ -355,7 +424,7 @@ private boolean reasonableNoteInVision() { double x_cushon = Units.inchesToMeters(40); double y_cushon = Units.inchesToMeters(12); return - limelight.noteInVision(limelight.getResults()) + limelight.noteInVision() && ((Robot.isBlueAlliance() && noteTranslation.getX() < FieldConstants.CENTERLINE_X + x_cushon) || (Robot.isRedAlliance() && noteTranslation.getX() > FieldConstants.CENTERLINE_X - x_cushon) && noteTranslation.getDistance(swerve.getPose().getTranslation()) < 2.75 @@ -376,7 +445,7 @@ public Command goToNote() { swerve.getChaseCommand( limelight::getNotePose2d, - () -> colorSensorSupplier.getAsBoolean() + () -> shooterSensor.getAsBoolean() || elevatorSensor.getAsBoolean() // Add 20 inches of cushion since we can't get penalized until we go 35 inches past the center line (bumpers fully over) // Keep in mind this is the note itself being 35 inches, the robot can only go 35/2 inches // since the pose is from the center but the note is from the edge (since the intake gets it) diff --git a/src/main/java/frc/robot/util/calc/AlignmentCalc.java b/src/main/java/frc/robot/util/calc/AlignmentCalc.java index 1141150b..23d0d672 100644 --- a/src/main/java/frc/robot/util/calc/AlignmentCalc.java +++ b/src/main/java/frc/robot/util/calc/AlignmentCalc.java @@ -10,17 +10,14 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc.robot.Robot; -import frc.robot.RobotContainer; import frc.robot.commands.managers.ShooterCmds; -import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.drive.Swerve; import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.DriveConstants; import frc.robot.util.Constants.FieldConstants; -import monologue.Annotations.IgnoreLogged; public class AlignmentCalc { - @IgnoreLogged private Swerve swerve; public AlignmentCalc(Swerve swerve) { @@ -97,6 +94,18 @@ public ChassisSpeeds getAmpAlignmentSpeeds() { ); } + public ChassisSpeeds getPresetAlignmentSpeeds() { + Pose2d targetPose = PoseCalculations.getClosestShootingPose(swerve.getPose()); + swerve.setDesiredPose(targetPose); + return + AutoConstants.HDC.calculate( + swerve.getPose(), + targetPose, + 0, + targetPose.getRotation() + ); + } + /** * Supplier for the amp alignment chassis speeds * @@ -106,6 +115,10 @@ public Supplier getAmpAlignmentSpeedsSupplier() { return () -> getAmpAlignmentSpeeds(); } + public Supplier getPresetAlignmentSpeedsSupplier() { + return () -> getPresetAlignmentSpeeds(); + } + /** * Calculates the rotational speeds to align the robot to the chain. * @@ -172,7 +185,7 @@ public Supplier getSourceRotationalSpeedsSupplier(DoubleSupplier */ public ChassisSpeeds getSpeakerRotationalSpeeds(double driverX, double driverY, ShooterCmds shooterCmds) { return - getRotationalSpeeds(driverX, driverY, shooterCmds.shooterCalc.calculateRobotAngleToSpeaker(swerve.getPose(), swerve.getRobotRelativeVelocity())); + getRotationalSpeeds(driverX, driverY, shooterCmds.shooterCalc.calculateRobotAngleToSpeaker(swerve.getPose())); } public ChassisSpeeds getPassRotationalSpeeds(double driverX, double driverY, ShooterCmds shooterCmds) { @@ -277,19 +290,4 @@ public Supplier getTrapControllerSpeedsSupplier(DoubleSupplier dr return () -> getTrapControllerSpeeds(driverX.getAsDouble(), driverY.getAsDouble()); } - /** - * Detects if the robot is on the opposite side of the field. - * Uses the robot's x position to determine if it has crossed the centerline. - * - * @return true if the robot is on the opposite side of the field - */ - public boolean onOppositeSide() { - return Robot.isRedAlliance() - ? swerve.getPose().getX() < FieldConstants.BLUE_WING_X - : swerve.getPose().getX() > FieldConstants.RED_WING_X; - } - - public boolean closeToSpeaker() { - return RobotContainer.distanceToSpeakerMeters < 7.3; - } } diff --git a/src/main/java/frc/robot/util/calc/LimelightConversion.java b/src/main/java/frc/robot/util/calc/LimelightConversion.java index 429ff77b..00f7c0d1 100644 --- a/src/main/java/frc/robot/util/calc/LimelightConversion.java +++ b/src/main/java/frc/robot/util/calc/LimelightConversion.java @@ -1,6 +1,7 @@ package frc.robot.util.calc; import org.json.simple.JSONObject; +import org.littletonrobotics.junction.AutoLogOutput; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -10,8 +11,6 @@ import edu.wpi.first.math.numbers.N4; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.custom.Scale3d; -import monologue.Logged; -import monologue.Annotations.Log; import java.util.HashMap; @@ -47,8 +46,8 @@ * ] * } */ -public class LimelightConversion implements Logged { - @Log +public class LimelightConversion { + @AutoLogOutput (key = "Calc/LimelightMapping/TagMap") public final HashMap tagMap = new HashMap<>(); private int tagLength = 16; diff --git a/src/main/java/frc/robot/util/calc/LimelightHelpers.java b/src/main/java/frc/robot/util/calc/LimelightHelpers.java index b1b18505..31ebdfea 100644 --- a/src/main/java/frc/robot/util/calc/LimelightHelpers.java +++ b/src/main/java/frc/robot/util/calc/LimelightHelpers.java @@ -6,7 +6,6 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.units.Unit; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; diff --git a/src/main/java/frc/robot/util/calc/LimelightMapping.java b/src/main/java/frc/robot/util/calc/LimelightMapping.java index 136b3831..97c47f04 100644 --- a/src/main/java/frc/robot/util/calc/LimelightMapping.java +++ b/src/main/java/frc/robot/util/calc/LimelightMapping.java @@ -5,7 +5,6 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; @@ -17,16 +16,15 @@ import frc.robot.util.Constants.FieldConstants; import frc.robot.util.calc.LimelightHelpers.LimelightTarget_Fiducial; import frc.robot.util.calc.LimelightHelpers.Results; -import monologue.Logged; -import monologue.Annotations.Log; import java.util.ArrayList; import java.util.HashMap; import java.util.function.Supplier; import org.json.simple.JSONObject; +import org.littletonrobotics.junction.AutoLogOutput; -public class LimelightMapping extends SubsystemBase implements Logged { +public class LimelightMapping extends SubsystemBase { private LimelightConversion limelightConversion; private HashMap poses; private String limelightName; @@ -35,7 +33,7 @@ public class LimelightMapping extends SubsystemBase implements Logged { private SwerveDrivePoseEstimator poseEstimator; Supplier robotPoseSupplier; - @Log + @AutoLogOutput (key = "Calc/LimelightMapping/ModifiedTagPoses") private Pose3d[] modifiedTagPoses = new Pose3d[16]; public LimelightMapping(SwerveDrivePoseEstimator poseEstimator, Supplier robotPoseSupplier, String limelightName) { @@ -188,7 +186,7 @@ public Command updatePoseEstimatorCommand() { ); } - @Log + @AutoLogOutput (key = "Calc/LimelightMapping/EstimatedPose2d") Pose2d estimatedPose2d = new Pose2d(); private void updatePoseEstimator() { diff --git a/src/main/java/frc/robot/util/calc/PoseCalculations.java b/src/main/java/frc/robot/util/calc/PoseCalculations.java index 6564e725..97d0141b 100644 --- a/src/main/java/frc/robot/util/calc/PoseCalculations.java +++ b/src/main/java/frc/robot/util/calc/PoseCalculations.java @@ -1,14 +1,22 @@ package frc.robot.util.calc; +import java.util.List; + +import org.littletonrobotics.junction.AutoLogOutput; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import frc.robot.Robot; +import frc.robot.RobotContainer; +import frc.robot.util.Constants.AutoConstants; import frc.robot.util.Constants.ClimbConstants; +import frc.robot.util.Constants.DriveConstants; import frc.robot.util.Constants.FieldConstants; -import monologue.Logged; -import monologue.Annotations.Log; +import frc.robot.util.Constants.ShooterConstants; -public class PoseCalculations implements Logged { +public class PoseCalculations { /** * Calculates the intercepts of the chain based on the given robot position. @@ -41,10 +49,10 @@ public static Pose2d getClosestChain(Pose2d position) { return position.nearest(FieldConstants.GET_CHAIN_POSITIONS()); } - @Log + @AutoLogOutput (key = "Calc/PoseCalc/ClosestShootingPose") private static Pose2d closestShootingPose; - @Log + @AutoLogOutput (key = "Calc/PoseCalc/BestShootingPose") public static Pose2d getBestShootingPose(Pose2d position) { double yValue = position.getY(); if (yValue > 5) @@ -57,6 +65,10 @@ public static Pose2d getBestShootingPose(Pose2d position) { return FieldConstants.GET_SHOOTING_POSITIONS().get(1); } + public static Pose2d getClosestShootingPose(Pose2d pose) { + return pose.nearest(FieldConstants.PRESET_SHOT_POSITIONS); + } + public static String getBestShootingPoseString(Pose2d position) { closestShootingPose = getBestShootingPose(position); if (closestShootingPose.equals(FieldConstants.GET_SHOOTING_POSITIONS().get(0))) @@ -91,4 +103,107 @@ private static double getChainIntercept(double x) { return MathUtil.clamp(calculation, 0.725, ClimbConstants.EXTENSION_LIMIT_METERS); } + public static boolean inAllianceWing(double xPosition) { + return Robot.isRedAlliance() + ? xPosition > FieldConstants.RED_WING_X + : xPosition < FieldConstants.BLUE_WING_X; + } + + /** + * Detects if the robot is in the opposite wing + * Uses the robot's x position to determine if it has crossed the opp wing line. + * + * @return true if the robot is in the opposing wing + */ + public static boolean onOppositeWing(double xPosition) { + return Robot.isRedAlliance() + ? xPosition < FieldConstants.BLUE_WING_X + : xPosition > FieldConstants.RED_WING_X; + } + + /** + * Detects if the robot is on the opposite side of the field. + * Uses the robot's x position to determine if it has crossed the centerline. + * + * @return true if the robot is on the opposite side of the field + */ + public static boolean onOppositeSide(double xPosition) { + return Robot.isRedAlliance() + ? xPosition < FieldConstants.CENTERLINE_X + : xPosition > FieldConstants.CENTERLINE_X; + } + + // Note: this method uses a static variable rather than a parameter + public static boolean closeToSpeaker() { + return RobotContainer.distanceToSpeakerFeet <= ShooterConstants.TUNED_SHOOTER_MAX_DISTANCE; + } + + public static boolean inSpeakerShotZone(Translation2d translation) { + Translation2d stealBoxCorner = FieldConstants.GET_STEAL_BOX_CORNER(); + return + inAllianceWing(translation.getX()) + && ((Robot.isRedAlliance() + ? translation.getX() < stealBoxCorner.getX() + : translation.getX() > stealBoxCorner.getX()) + || translation.getY() > stealBoxCorner.getY()); + } + + /** + * Detects if the robot is inside of the area of the stage + * + * @return true if the robot is inside of the area of the stage + */ + public static boolean inStageTriangle(Pose2d position) { + + List points = FieldConstants.GET_STAGE_POINTS(); + + double a1 = points.get(0).minus(position.getTranslation()).getNorm(); + double b1 = points.get(2).minus(position.getTranslation()).getNorm(); + double c1 = points.get(2).minus(points.get(0)).getNorm(); + double s1 = 0.5 * (a1 + b1 + c1); + + double a2 = a1; + double b2 = points.get(1).minus(position.getTranslation()).getNorm(); + double c2 = points.get(1).minus(points.get(0)).getNorm(); + double s2 = 0.5 * (a2 + b2 + c2); + + double a3 = b2; + double b3 = b1; + double c3 = points.get(2).minus(points.get(1)).getNorm(); + double s3 = 0.5 * (a3 + b3 + c3); + + double mainArea = 0.5 * Math.abs(points.get(2).getY() - points.get(1).getY()) * Math.abs(points.get(1).getX() - points.get(0).getX()); + double area1 = Math.sqrt(s1 * (s1 - a1) * (s1 - b1) * (s1 - c1)); + double area2 = Math.sqrt(s2 * (s2 - a2) * (s2 - b2) * (s2 - c2)); + double area3 = Math.sqrt(s3 * (s3 - a3) * (s3 - b3) * (s3 - c3)); + + return MathUtil.applyDeadband(mainArea - (area1 + area2 + area3), AutoConstants.UNDER_STAGE_DEADBAND) == 0; + } + + public static boolean isAlignedToAmp(Pose2d position) { + Translation2d touchingAmpPose = new Translation2d( + FieldConstants.GET_AMP_POSITION().getX(), + FieldConstants.GET_AMP_POSITION().getY() + - DriveConstants.ROBOT_LENGTH_METERS / 2.0 + - DriveConstants.BUMPER_LENGTH_METERS + ); + double robotX = position.getTranslation().getDistance(touchingAmpPose); + return MathUtil.isNear(0, robotX, AutoConstants.AUTO_ALIGNMENT_DEADBAND); + } + + public static boolean inSourcePassZone(Pose2d position) { + return + Robot.isRedAlliance() + ? position.getX() < FieldConstants.BLUE_WING_X + : position.getX() > FieldConstants.RED_WING_X; + } + + public static boolean inLowPassZone(Pose2d position) { + return + position.getY() > FieldConstants.GET_STAGE_POINTS().get(2).getY() + || (Robot.isRedAlliance() + ? position.getX() > FieldConstants.NOTE_TRANSLATIONS[3].getX() + : position.getX() < FieldConstants.NOTE_TRANSLATIONS[0].getX()); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/calc/ShooterCalc.java b/src/main/java/frc/robot/util/calc/ShooterCalc.java index 894e4c1f..5037e3ac 100644 --- a/src/main/java/frc/robot/util/calc/ShooterCalc.java +++ b/src/main/java/frc/robot/util/calc/ShooterCalc.java @@ -2,6 +2,8 @@ import java.util.function.BooleanSupplier; +import org.littletonrobotics.junction.AutoLogOutput; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; @@ -11,21 +13,17 @@ import edu.wpi.first.math.util.Units; import frc.robot.Robot; import frc.robot.commands.logging.NT; -import frc.robot.subsystems.Pivot; -import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.shooter.Pivot; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.util.Constants.FieldConstants; import frc.robot.util.Constants.NTConstants; import frc.robot.util.Constants.ShooterConstants; import frc.robot.util.custom.SpeedAngleTriplet; -import monologue.Logged; -import monologue.Annotations.IgnoreLogged; -import monologue.Annotations.Log; -public class ShooterCalc implements Logged { +public class ShooterCalc { - @IgnoreLogged private Shooter shooter; - @IgnoreLogged + private Pivot pivot; public ShooterCalc(Shooter shooter, Pivot pivot) { @@ -33,9 +31,14 @@ public ShooterCalc(Shooter shooter, Pivot pivot) { this.pivot = pivot; } - - @Log - double realHeight, gravitySpeedL, gravitySpeedR, gravityAngle; + @AutoLogOutput (key = "Calc/ShooterCalc/RealHeight") + double realHeight; + @AutoLogOutput (key = "Calc/ShooterCalc/GravitySpeedLeft") + double gravitySpeedL; + @AutoLogOutput (key = "Calc/ShooterCalc/GravitySpeedRight") + double gravitySpeedR; + @AutoLogOutput (key = "Calc/ShooterCalc/GravityAngle") + double gravityAngle; /** * Calculates the shooter speeds required to reach the speaker position. @@ -81,9 +84,9 @@ public SpeedAngleTriplet calculateSWDTripletAuto(Pose2d pose, ChassisSpeeds spee ); } - public SpeedAngleTriplet calculatePassTriplet(Pose2d robotPose) { - Rotation2d pivotAngle = calculatePassPivotAngle(robotPose); - Pair shooterSpeeds = calculateShooterSpeedsForPassApex(robotPose, pivotAngle); + public SpeedAngleTriplet calculatePassTriplet(Pose2d robotPose, boolean lowPass) { + Rotation2d pivotAngle = calculatePassPivotAngle(robotPose, lowPass); + Pair shooterSpeeds = calculateShooterSpeedsForPassApex(robotPose, pivotAngle, lowPass); return SpeedAngleTriplet.of( // Don't ask. It works. Is this how we finally beat the hawaiian kids? shooterSpeeds.getFirst(), @@ -91,6 +94,10 @@ public SpeedAngleTriplet calculatePassTriplet(Pose2d robotPose) { pivotAngle.getDegrees()); } + public SpeedAngleTriplet calculatePassTriplet(Pose2d robotPose) { + return calculatePassTriplet(robotPose, PoseCalculations.inLowPassZone(robotPose)); + } + public SpeedAngleTriplet calculateApexTriplet(Pose2d robotPose) { Rotation2d pivotAngle = calculatePivotAngle(robotPose); Pair shooterSpeeds = calculateShooterSpeedsForSpeakerApex(robotPose, pivotAngle); @@ -123,16 +130,19 @@ public Rotation2d calculatePivotAngle(Pose2d robotPose) { ); } - public Rotation2d calculatePassPivotAngle(Pose2d robotPose) { - // Calculate the robot's pose relative to the speaker's position - robotPose = robotPose.relativeTo(FieldConstants.GET_PASS_APEX_POSITION()); + public Rotation2d calculatePassPivotAngle(Pose2d robotPose, boolean lowPass) { - // Calculate the distance in feet from the robot to the speaker - double distanceMeters = robotPose.getTranslation().getNorm(); + // Calculate the robot's pose relative to the desired pass's apex position + robotPose = robotPose.relativeTo(FieldConstants.GET_CENTER_PASS_TARGET_POSITION()); + + // Calculate the distance and height in meters from the robot to the pass target pose + double rangeMeters = robotPose.getTranslation().getNorm(); + double v0z = ShooterConstants.PASS_V0Z; + + double theta = Math.atan((-2*v0z*v0z)/(rangeMeters*-ShooterConstants.GRAVITY)); // Return a new rotation object that represents the pivot angle - // The pivot angle is calculated based on the speaker's height and the distance to the speaker - return new Rotation2d(distanceMeters, FieldConstants.PASS_HEIGHT_METERS + NT.getValue("atan++")); + return new Rotation2d(lowPass ? Math.PI / 2 - theta : theta); } /** @@ -146,10 +156,17 @@ public BooleanSupplier readyToShootSupplier() { return () -> pivot.getAtDesiredAngle() && shooter.getAtDesiredRPM() - && shooter.getAverageTargetSpeed() > 0 - && shooter.getAverageTargetSpeed() != ShooterConstants.DEFAULT_RPM; + && shooter.getAverageTargetSpeed() > 0; } + public BooleanSupplier readyToPassSupplier() { + return () -> + pivot.getAtDesiredPassAngle() + && shooter.getAtDesiredPassRPM() + && shooter.getAverageTargetSpeed() > 0; + } + + public SpeedAngleTriplet calculateSpeakerTriplet(Translation2d robotPose) { // Get our position relative to the desired field element // Use the distance as our key for interpolation @@ -158,15 +175,15 @@ public SpeedAngleTriplet calculateSpeakerTriplet(Translation2d robotPose) { return ShooterConstants.INTERPOLATION_MAP.get(distanceFeet); } - @Log + @AutoLogOutput (key = "Calc/ShooterCalc/AngleToSpeaker") Rotation2d currentAngleToSpeaker = new Rotation2d(); - @Log + @AutoLogOutput (key = "Calc/ShooterCalc/DesiredSWDPose") public Pose2d desiredSWDPose = new Pose2d(); - @Log + @AutoLogOutput (key = "Calc/ShooterCalc/DesiredNoteMPS") double desiredMPSForNote = 0; - @Log + @AutoLogOutput (key = "Calc/ShooterCalc/DegreesToSpeakerReferenced") double degreesToSpeakerReferenced = 0; - @Log + @AutoLogOutput (key = "Calc/ShooterCalc/AngleDifference") double angleDifference = 0; /** @@ -189,8 +206,9 @@ public Rotation2d calculateRobotAngleToPose(Pose2d robotPose, ChassisSpeeds robo Rotation2d currentAngleToTarget = new Rotation2d(poseRelativeToTarget.getX(), poseRelativeToTarget.getY()); double velocityArcTan = Math.atan2( velocityTangent, - target.equals(FieldConstants.GET_PASS_TARGET_POSITION()) - ? rpmToVelocity(calculatePassTriplet(robotPose).getSpeeds()) + (target.equals(FieldConstants.GET_CENTER_PASS_TARGET_POSITION())) + // Run this calculation with high pass triplet, as low pass requires little rotational precision in swd + ? rpmToVelocity(calculatePassTriplet(robotPose).getSpeeds()) : rpmToVelocity(calculateSWDTriplet(robotPose, robotVelocity).getSpeeds()) // rpmToVelocity(calculateShooterSpeedsForApex(robotPose, calculatePivotAngle(robotPose))) ); @@ -207,12 +225,20 @@ public Rotation2d calculateRobotAngleToPose(Pose2d robotPose, ChassisSpeeds robo return desiredRotation2d; } + public Rotation2d calculateRobotAngleToPose(Pose2d robotPose, Pose2d target) { + return calculateRobotAngleToPose(robotPose, new ChassisSpeeds(), target); + } + public Rotation2d calculateRobotAngleToPass(Pose2d robotPose) { return calculateRobotAngleToPass(robotPose, new ChassisSpeeds()); } public Rotation2d calculateRobotAngleToPass(Pose2d robotPose, ChassisSpeeds robotVelocity) { - return calculateRobotAngleToPose(robotPose, robotVelocity, FieldConstants.GET_PASS_TARGET_POSITION()); + return + calculateRobotAngleToPose( + robotPose, + robotVelocity, + FieldConstants.GET_CENTER_PASS_TARGET_POSITION()); } public Rotation2d calculateRobotAngleToSpeaker(Pose2d pose) { @@ -313,8 +339,9 @@ private Pair calculateShooterSpeedsForSpeakerApex(Pose2d robotPo return Pair.of(desiredRPM, desiredRPM); } - private Pair calculateShooterSpeedsForPassApex(Pose2d robotPose, Rotation2d pivotAngle) { - double desiredRPM = velocityToRPM(ShooterConstants.PASS_V0Z / (pivotAngle.getSin())); - return Pair.of(desiredRPM/1.5, desiredRPM/1.5); + private Pair calculateShooterSpeedsForPassApex(Pose2d robotPose, Rotation2d pivotAngle, boolean lowPass) { + double v0z = ShooterConstants.PASS_V0Z; + double desiredRPM = velocityToRPM(v0z / (lowPass ? pivotAngle.getCos() : pivotAngle.getSin())); + return Pair.of(desiredRPM / 1.2, desiredRPM / 1.2); } } diff --git a/src/main/java/frc/robot/util/custom/PatrIDConstants.java b/src/main/java/frc/robot/util/custom/PatrIDConstants.java index 53ec4b89..010b3e1f 100644 --- a/src/main/java/frc/robot/util/custom/PatrIDConstants.java +++ b/src/main/java/frc/robot/util/custom/PatrIDConstants.java @@ -1,24 +1,22 @@ package frc.robot.util.custom; -import monologue.Logged; -import monologue.Annotations.Log; +import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.AutoLogOutput; /** * PID constants used to create PID controllers * This class is special becuase of its overloaded constructors * It just helps keep everything organized */ -public class PatrIDConstants implements Logged { - - @Log - private final double - P, - I, - D, - FF, - iZone, - minOutput, - maxOutput; +public class PatrIDConstants { + + private final double P; + private final double I; + private final double D; + private final double FF; + private final double iZone; + private final double minOutput; + private final double maxOutput; public PatrIDConstants(double P) { this(P, 0); diff --git a/src/main/java/frc/robot/commands/managers/SelectiveConditionalCommand.java b/src/main/java/frc/robot/util/custom/SelectiveConditionalCommand.java similarity index 98% rename from src/main/java/frc/robot/commands/managers/SelectiveConditionalCommand.java rename to src/main/java/frc/robot/util/custom/SelectiveConditionalCommand.java index ac5a324a..fdf151d9 100644 --- a/src/main/java/frc/robot/commands/managers/SelectiveConditionalCommand.java +++ b/src/main/java/frc/robot/util/custom/SelectiveConditionalCommand.java @@ -1,4 +1,4 @@ -package frc.robot.commands.managers; +package frc.robot.util.custom; import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; @@ -22,6 +22,7 @@ * This class is provided by the NewCommands VendorDep */ public class SelectiveConditionalCommand extends Command { + private final Command onTrueCommand; private final Command onFalseCommand; private final BooleanSupplier conditionSupplier; diff --git a/src/main/java/frc/robot/util/custom/SpeedAngleTriplet.java b/src/main/java/frc/robot/util/custom/SpeedAngleTriplet.java index a44597c8..0750e348 100644 --- a/src/main/java/frc/robot/util/custom/SpeedAngleTriplet.java +++ b/src/main/java/frc/robot/util/custom/SpeedAngleTriplet.java @@ -1,7 +1,7 @@ package frc.robot.util.custom; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Pair; +import edu.wpi.first.math.Pair; import edu.wpi.first.math.interpolation.Interpolator; /** diff --git a/src/main/java/frc/robot/util/custom/TransistorPower.java b/src/main/java/frc/robot/util/custom/TransistorPower.java new file mode 100644 index 00000000..268d2870 --- /dev/null +++ b/src/main/java/frc/robot/util/custom/TransistorPower.java @@ -0,0 +1,38 @@ +package frc.robot.util.custom; + +import edu.wpi.first.wpilibj.AnalogOutput; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; + +public class TransistorPower extends AnalogOutput { + public TransistorPower(int port) { + super(port); + } + + public TransistorPower(int port, boolean state) { + super(port); + set(state); + } + + public void setState(boolean state) { + setVoltage(state ? 5 : 0); + } + + public void set(boolean state) { + setState(state); + } + + public boolean get() { + return getVoltage() > 0.5; + } + + public Command getPowerCycleCommand() { + return Commands.runOnce(() -> set(false)) + .andThen(Commands.waitSeconds(1), + Commands.runOnce(() -> set(true))).ignoringDisable(true); + } + + public void schedulePowerCycleCommand() { + getPowerCycleCommand().schedule(); + } +} diff --git a/src/main/java/frc/robot/util/rev/Neo.java b/src/main/java/frc/robot/util/rev/Neo.java index 01f69af0..b0fc6fe6 100644 --- a/src/main/java/frc/robot/util/rev/Neo.java +++ b/src/main/java/frc/robot/util/rev/Neo.java @@ -32,6 +32,7 @@ public class Neo extends SafeSpark { private double targetVelocity = 0; private double targetPercent = 0; + /** * Creates a new Neo motor with the default settings. * This assumes no inversion and is relative encoder only diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 00000000..63dacbb5 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,42 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "3.2.1", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2024", + "mavenUrls": [], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "wpilib-shim", + "version": "3.2.1" + }, + { + "groupId": "org.littletonrobotics.akit.junction", + "artifactId": "junction-core", + "version": "3.2.1" + }, + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-api", + "version": "3.2.1" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit.conduit", + "artifactId": "conduit-wpilibio", + "version": "3.2.1", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json index 387b2de5..a5f886a3 100644 --- a/vendordeps/ChoreoLib.json +++ b/vendordeps/ChoreoLib.json @@ -1,7 +1,7 @@ { "fileName": "ChoreoLib.json", "name": "ChoreoLib", - "version": "2024.1.3", + "version": "2024.2.3", "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.choreo.lib", "artifactId": "ChoreoLib-java", - "version": "2024.1.3" + "version": "2024.2.3" }, { "groupId": "com.google.code.gson", @@ -25,7 +25,7 @@ { "groupId": "com.choreo.lib", "artifactId": "ChoreoLib-cpp", - "version": "2024.1.3", + "version": "2024.2.3", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index a0197060..6dc648db 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.7", + "version": "2024.2.8", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.7" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.7", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 2b7d1720..03223850 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.2.0", + "version": "24.3.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.2.0" + "version": "24.3.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true,