diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/src/main/deploy/pathplanner/autos/Basic Comp Auto - RIGHT.auto b/src/main/deploy/pathplanner/autos/Basic Comp Auto - RIGHT.auto deleted file mode 100644 index e4e2444..0000000 --- a/src/main/deploy/pathplanner/autos/Basic Comp Auto - RIGHT.auto +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Start to Shoot - RIGHT" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "path", - "data": { - "pathName": "Shoot to Overbump - RIGHT" - } - }, - { - "type": "path", - "data": { - "pathName": "Collect - RIGHT" - } - }, - { - "type": "path", - "data": { - "pathName": "Overbump to Shoot - RIGHT" - } - }, - { - "type": "named", - "data": { - "name": "Shoot" - } - }, - { - "type": "path", - "data": { - "pathName": "Shoot to Climb - RIGHT" - } - } - ] - } - }, - "resetOdom": true, - "folder": "Comp Autos", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Basic Comp Auto - LEFT.auto b/src/main/deploy/pathplanner/autos/Basic Comp Auto.auto similarity index 63% rename from src/main/deploy/pathplanner/autos/Basic Comp Auto - LEFT.auto rename to src/main/deploy/pathplanner/autos/Basic Comp Auto.auto index 179f23f..e9f47c7 100644 --- a/src/main/deploy/pathplanner/autos/Basic Comp Auto - LEFT.auto +++ b/src/main/deploy/pathplanner/autos/Basic Comp Auto.auto @@ -7,49 +7,49 @@ { "type": "path", "data": { - "pathName": "Start to Shoot - LEFT" + "pathName": "Start to Shoot" } }, { - "type": "named", + "type": "wait", "data": { - "name": "Shoot" + "waitTime": 1.0 } }, { "type": "path", "data": { - "pathName": "Shoot to Overbump - LEFT" + "pathName": "Shoot to Overbump" } }, { "type": "path", "data": { - "pathName": "Collect - LEFT" + "pathName": "Collect" } }, { "type": "path", "data": { - "pathName": "Overbump to Shoot - LEFT" + "pathName": "Overbump to Shoot" } }, { - "type": "named", + "type": "wait", "data": { - "name": "Shoot" + "waitTime": 1.0 } }, { "type": "path", "data": { - "pathName": "Shoot to Climb - LEFT" + "pathName": "Shoot to Climber" } } ] } }, "resetOdom": true, - "folder": "Comp Autos", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Full Simple Auto.auto b/src/main/deploy/pathplanner/autos/Full Simple Auto.auto index e9d5b03..2265148 100644 --- a/src/main/deploy/pathplanner/autos/Full Simple Auto.auto +++ b/src/main/deploy/pathplanner/autos/Full Simple Auto.auto @@ -26,6 +26,6 @@ } }, "resetOdom": true, - "folder": "Simple Autos", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Plus Auto.auto b/src/main/deploy/pathplanner/autos/Plus Auto.auto index 95f26fd..f8b62f3 100644 --- a/src/main/deploy/pathplanner/autos/Plus Auto.auto +++ b/src/main/deploy/pathplanner/autos/Plus Auto.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "Seed" - } - }, { "type": "path", "data": { @@ -37,7 +31,7 @@ ] } }, - "resetOdom": true, + "resetOdom": false, "folder": "Test Autos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Rotate and Move Auto.auto b/src/main/deploy/pathplanner/autos/Rotate and Move Auto.auto index 791f817..5a9e346 100644 --- a/src/main/deploy/pathplanner/autos/Rotate and Move Auto.auto +++ b/src/main/deploy/pathplanner/autos/Rotate and Move Auto.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "Seed" - } - }, { "type": "path", "data": { @@ -25,7 +19,7 @@ ] } }, - "resetOdom": true, + "resetOdom": false, "folder": "Test Autos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Simple Forward Auto.auto b/src/main/deploy/pathplanner/autos/Simple Forward Auto.auto index 3e3b9fb..8c69b62 100644 --- a/src/main/deploy/pathplanner/autos/Simple Forward Auto.auto +++ b/src/main/deploy/pathplanner/autos/Simple Forward Auto.auto @@ -20,6 +20,6 @@ } }, "resetOdom": true, - "folder": "Simple Autos", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Simple Rotation Auto.auto b/src/main/deploy/pathplanner/autos/Simple Rotation Auto.auto index 879ca98..45c093f 100644 --- a/src/main/deploy/pathplanner/autos/Simple Rotation Auto.auto +++ b/src/main/deploy/pathplanner/autos/Simple Rotation Auto.auto @@ -20,6 +20,6 @@ } }, "resetOdom": true, - "folder": "Simple Autos", + "folder": null, "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Square Auto.auto b/src/main/deploy/pathplanner/autos/Square Auto.auto index 02be52d..91d6e26 100644 --- a/src/main/deploy/pathplanner/autos/Square Auto.auto +++ b/src/main/deploy/pathplanner/autos/Square Auto.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "Seed" - } - }, { "type": "path", "data": { @@ -37,7 +31,7 @@ ] } }, - "resetOdom": true, + "resetOdom": false, "folder": "Test Autos", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Collect - LEFT.path b/src/main/deploy/pathplanner/paths/Collect - LEFT.path deleted file mode 100644 index b93421e..0000000 --- a/src/main/deploy/pathplanner/paths/Collect - LEFT.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 6.5, - "y": 5.6 - }, - "prevControl": null, - "nextControl": { - "x": 6.23250357830652, - "y": 6.717714710948497 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.66188302425107, - "y": 7.146747503566334 - }, - "prevControl": { - "x": 7.417959543415542, - "y": 7.8788594451551655 - }, - "nextControl": { - "x": 7.8860485021398, - "y": 6.473937232524964 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.8, - "y": 2.0 - }, - "prevControl": { - "x": 7.629810024551348, - "y": 0.9438438751506559 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.9650053022269222, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -88.56790381583522 - }, - "reversed": false, - "folder": "Basic Comp Paths - LEFT", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Collect - RIGHT.path b/src/main/deploy/pathplanner/paths/Collect - RIGHT.path deleted file mode 100644 index f7d17fd..0000000 --- a/src/main/deploy/pathplanner/paths/Collect - RIGHT.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 6.5, - "y": 2.4 - }, - "prevControl": null, - "nextControl": { - "x": 6.827975062694294, - "y": 2.4358068711646417 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.674821683315434, - "y": 1.0267617689018789 - }, - "prevControl": { - "x": 6.465836505806936, - "y": 0.8522019141369845 - }, - "nextControl": { - "x": 7.9222558379197965, - "y": 1.0624876577673015 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.8, - "y": 6.0 - }, - "prevControl": { - "x": 7.791269614841827, - "y": 6.383366619115859 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Basic Comp Paths - RIGHT", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Overbump to Shoot - RIGHT.path b/src/main/deploy/pathplanner/paths/Collect.path similarity index 76% rename from src/main/deploy/pathplanner/paths/Overbump to Shoot - RIGHT.path rename to src/main/deploy/pathplanner/paths/Collect.path index b241ce2..d7a08de 100644 --- a/src/main/deploy/pathplanner/paths/Overbump to Shoot - RIGHT.path +++ b/src/main/deploy/pathplanner/paths/Collect.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.8, - "y": 6.0 + "x": 6.5, + "y": 5.6 }, "prevControl": null, "nextControl": { - "x": 8.8, - "y": 6.0 + "x": 7.764965909090909, + "y": 4.838931818181818 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.5, - "y": 5.6 + "x": 7.8, + "y": 2.6 }, "prevControl": { - "x": 1.5, - "y": 5.6 + "x": 7.897767937834849, + "y": 2.8300900483104847 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -40.0 + "rotation": -88.56790381583522 }, "reversed": false, - "folder": "Basic Comp Paths - RIGHT", + "folder": "Basic Comp Paths", "idealStartingState": { "velocity": 0, - "rotation": 90.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Forward Simple.path b/src/main/deploy/pathplanner/paths/Forward Simple.path index a39e6c8..a2af603 100644 --- a/src/main/deploy/pathplanner/paths/Forward Simple.path +++ b/src/main/deploy/pathplanner/paths/Forward Simple.path @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Simple Pahs", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Overbump to Shoot - LEFT.path b/src/main/deploy/pathplanner/paths/Overbump to Shoot - LEFT.path deleted file mode 100644 index b448128..0000000 --- a/src/main/deploy/pathplanner/paths/Overbump to Shoot - LEFT.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.8, - "y": 2.0 - }, - "prevControl": null, - "nextControl": { - "x": 6.985983737990004, - "y": 2.050324291527099 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.5, - "y": 2.4 - }, - "prevControl": { - "x": 2.1995281659399466, - "y": 2.4581032736970116 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 40.0 - }, - "reversed": false, - "folder": "Basic Comp Paths - LEFT", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to Overbump - RIGHT.path b/src/main/deploy/pathplanner/paths/Overbump to Shoot.path similarity index 80% rename from src/main/deploy/pathplanner/paths/Shoot to Overbump - RIGHT.path rename to src/main/deploy/pathplanner/paths/Overbump to Shoot.path index b30bd4f..cc11753 100644 --- a/src/main/deploy/pathplanner/paths/Shoot to Overbump - RIGHT.path +++ b/src/main/deploy/pathplanner/paths/Overbump to Shoot.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.5, + "x": 7.8, "y": 2.4 }, "prevControl": null, "nextControl": { - "x": 3.439650812800243, - "y": 2.426156135912835 + "x": 7.770205608742315, + "y": 2.151781760844244 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.5, + "x": 2.5, "y": 2.4 }, "prevControl": { - "x": 5.62327005800708, - "y": 2.403713255567281 + "x": 2.255053798152365, + "y": 2.4500135801599687 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 37.509098664816776 }, "reversed": false, - "folder": "Basic Comp Paths - RIGHT", + "folder": "Basic Comp Paths", "idealStartingState": { "velocity": 0, - "rotation": 40.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Rotation Simple.path b/src/main/deploy/pathplanner/paths/Rotation Simple.path index 2e8b0ac..73e0707 100644 --- a/src/main/deploy/pathplanner/paths/Rotation Simple.path +++ b/src/main/deploy/pathplanner/paths/Rotation Simple.path @@ -45,7 +45,7 @@ "rotation": -90.0 }, "reversed": false, - "folder": "Simple Pahs", + "folder": null, "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Shoot to Climb - RIGHT.path b/src/main/deploy/pathplanner/paths/Shoot to Climb - RIGHT.path deleted file mode 100644 index b756051..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot to Climb - RIGHT.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.5, - "y": 5.6 - }, - "prevControl": null, - "nextControl": { - "x": 2.7791476627370493, - "y": 5.598677970740438 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.5548359486447931, - "y": 4.235549215406562 - }, - "prevControl": { - "x": 1.3049767748435428, - "y": 4.22715914583852 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": "Basic Comp Paths - RIGHT", - "idealStartingState": { - "velocity": 0, - "rotation": -40.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Shoot to Climb - LEFT.path b/src/main/deploy/pathplanner/paths/Shoot to Climber.path similarity index 96% rename from src/main/deploy/pathplanner/paths/Shoot to Climb - LEFT.path rename to src/main/deploy/pathplanner/paths/Shoot to Climber.path index 6bb14c8..40101a5 100644 --- a/src/main/deploy/pathplanner/paths/Shoot to Climb - LEFT.path +++ b/src/main/deploy/pathplanner/paths/Shoot to Climber.path @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": "Basic Comp Paths - LEFT", + "folder": "Basic Comp Paths", "idealStartingState": { "velocity": 0, "rotation": 40.0 diff --git a/src/main/deploy/pathplanner/paths/Shoot to Overbump - LEFT.path b/src/main/deploy/pathplanner/paths/Shoot to Overbump.path similarity index 96% rename from src/main/deploy/pathplanner/paths/Shoot to Overbump - LEFT.path rename to src/main/deploy/pathplanner/paths/Shoot to Overbump.path index 83725d4..c15fb32 100644 --- a/src/main/deploy/pathplanner/paths/Shoot to Overbump - LEFT.path +++ b/src/main/deploy/pathplanner/paths/Shoot to Overbump.path @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "Basic Comp Paths - LEFT", + "folder": "Basic Comp Paths", "idealStartingState": { "velocity": 0, "rotation": -40.0 diff --git a/src/main/deploy/pathplanner/paths/Start to Shoot - RIGHT.path b/src/main/deploy/pathplanner/paths/Start to Shoot - RIGHT.path deleted file mode 100644 index 04be42c..0000000 --- a/src/main/deploy/pathplanner/paths/Start to Shoot - RIGHT.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6, - "y": 3.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.9370214328880317, - "y": 3.0058081435684487 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.5, - "y": 2.4 - }, - "prevControl": { - "x": 2.2511707277397344, - "y": 2.375834047371835 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 40.0 - }, - "reversed": false, - "folder": "Basic Comp Paths - RIGHT", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start to Shoot - LEFT.path b/src/main/deploy/pathplanner/paths/Start to Shoot.path similarity index 90% rename from src/main/deploy/pathplanner/paths/Start to Shoot - LEFT.path rename to src/main/deploy/pathplanner/paths/Start to Shoot.path index 64d1e7b..676f55a 100644 --- a/src/main/deploy/pathplanner/paths/Start to Shoot - LEFT.path +++ b/src/main/deploy/pathplanner/paths/Start to Shoot.path @@ -20,8 +20,8 @@ "y": 5.6 }, "prevControl": { - "x": 2.3972407268555767, - "y": 5.827904655026679 + "x": 2.397239513949471, + "y": 5.827904108139057 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": -40.0 }, "reversed": false, - "folder": "Basic Comp Paths - LEFT", + "folder": "Basic Comp Paths", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 8daa0f8..6110946 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,16 +3,12 @@ "robotLength": 0.858, "holonomicMode": true, "pathFolders": [ - "Basic Comp Paths - LEFT", - "Basic Comp Paths - RIGHT", + "Basic Comp Paths", "Plus Sign Test", "Rotation + Translation Test", - "Simple Pahs", "Square Test" ], "autoFolders": [ - "Comp Autos", - "Simple Autos", "Test Autos" ], "defaultMaxVel": 3.0, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e3684d9..a815a03 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,10 +13,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.statemachines.LaunchState; import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.drive.DrivetrainSubsystem; @@ -39,18 +39,18 @@ public class RobotContainer { @Logged(name = "Indexer") public final IndexerSubsystem indexer = new IndexerSubsystem(); - @Logged(name = "Climber") - public final ClimberSubsystem climber = new ClimberSubsystem(); - @Logged(name = "Shooter") public final ShooterSubsystem shooter = new ShooterSubsystem(); + @Logged(name = "Climber") + public final ClimberSubsystem climber = new ClimberSubsystem(); + + private final LaunchState launchState = LaunchState.getInstance(); private final SendableChooser autoChooser; public RobotContainer() { NamedCommands.registerCommand("Seed", drivetrain.runOnce(drivetrain::seedFieldCentric)); - NamedCommands.registerCommand("Shoot", new WaitCommand(2)); autoChooser = AutoBuilder.buildAutoChooser("Auto Chooser"); SmartDashboard.putData("Auto Mode", autoChooser); @@ -61,7 +61,6 @@ public RobotContainer() { } public void configureSubsystemDefaultCommands() { - drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest( diff --git a/src/main/java/frc/robot/motor-ids.txt b/src/main/java/frc/robot/motor-ids.txt deleted file mode 100644 index 65a8c0c..0000000 --- a/src/main/java/frc/robot/motor-ids.txt +++ /dev/null @@ -1,6 +0,0 @@ -intake extension = 1 - roller = 2 -indexer = 3 -hood = 4 -flywheel x2 = 5,6 -climb = 7 \ No newline at end of file diff --git a/src/main/java/frc/robot/statemachines/DriveState.java b/src/main/java/frc/robot/statemachines/DriveState.java index 167624e..11ad5d8 100644 --- a/src/main/java/frc/robot/statemachines/DriveState.java +++ b/src/main/java/frc/robot/statemachines/DriveState.java @@ -16,7 +16,8 @@ public class DriveState { private HashMap> concurrentQueueMap; - private SwerveDriveState currentDriveStats = null; + private SwerveDriveState previousDriveStats = new SwerveDriveState(); + private SwerveDriveState currentDriveStats = new SwerveDriveState(); private DriveState() { concurrentQueueMap = new HashMap>(); @@ -57,6 +58,7 @@ public ArrayList grabVisionEstimateList(String cameraName) { } public void adjustCurrentDriveStats(SwerveDriveState newStats) { + previousDriveStats = currentDriveStats; currentDriveStats = newStats; } @@ -68,6 +70,10 @@ public SwerveDriveState getCurrentDriveStats() { return currentDriveStats; } + public SwerveDriveState getPreviousDriveStats() { + return previousDriveStats; + } + public ChassisSpeeds getFieldVelocity() { return ChassisSpeeds.fromRobotRelativeSpeeds( getCurrentDriveStats().Speeds, getCurrentDriveStats().Pose.getRotation()); diff --git a/src/main/java/frc/robot/statemachines/LaunchCalculator.java b/src/main/java/frc/robot/statemachines/LaunchCalculator.java new file mode 100644 index 0000000..a3d6a2c --- /dev/null +++ b/src/main/java/frc/robot/statemachines/LaunchCalculator.java @@ -0,0 +1,133 @@ +package frc.robot.statemachines; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.LinearFilter; +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.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.AngularVelocity; +import frc.robot.subsystems.shooter.LaunchRequest; +import frc.robot.subsystems.shooter.LaunchRequestBuilder; + +public class LaunchCalculator { + + private Pose3d target; + private LaunchRequestBuilder builder; + + protected LaunchCalculator(Pose3d target, LaunchRequestBuilder builder) { + this.target = target; + this.builder = builder; + } + + private double loopPeriodSecs = 0.02; + + private final LinearFilter driveAngleFilter = + LinearFilter.movingAverage((int) (0.8 / loopPeriodSecs)); + + private static final double phaseDelay; + + private static final InterpolatingDoubleTreeMap hubTimeOfFlightMap = + new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap passingTimeOfFlightMap = + new InterpolatingDoubleTreeMap(); + + // TODO: All of this is made up. Need real numbers. + static { + phaseDelay = 0.03; + + hubTimeOfFlightMap.put(5.68, 1.16); + hubTimeOfFlightMap.put(4.55, 1.12); + hubTimeOfFlightMap.put(3.15, 1.11); + hubTimeOfFlightMap.put(1.88, 1.09); + hubTimeOfFlightMap.put(1.38, 0.90); + + passingTimeOfFlightMap.put(6.68, 1.16); + passingTimeOfFlightMap.put(5.55, 1.12); + passingTimeOfFlightMap.put(4.15, 1.11); + passingTimeOfFlightMap.put(2.88, 1.09); + passingTimeOfFlightMap.put(2.38, 0.90); + } + + public LaunchRequest createLaunchRequest() { + + boolean passing = target.getZ() < 0.1; + + // current pose and movement + Pose2d estimatedRobotPose = DriveState.getInstance().getCurrentDriveStats().Pose; + ChassisSpeeds robotSpeeds = DriveState.getInstance().getCurrentDriveStats().Speeds; + + // predicted pose + estimatedRobotPose.exp( + new Twist2d( + robotSpeeds.vxMetersPerSecond * phaseDelay, + robotSpeeds.vyMetersPerSecond * phaseDelay, + robotSpeeds.omegaRadiansPerSecond * phaseDelay)); + + // TODO: for now assume they're the same. calculate offsets later + Pose2d launcherPose = estimatedRobotPose; + double launcherToTargetDistance = + launcherPose.getTranslation().getDistance(target.toPose2d().getTranslation()); + + // Calculate field relative velocity + double xSpeed = DriveState.getInstance().getFieldVelocity().vxMetersPerSecond; + double ySpeed = DriveState.getInstance().getFieldVelocity().vyMetersPerSecond; + + // Account for imparted velocity by robot (launcher) to offset + double timeOfFlight = + passing + ? passingTimeOfFlightMap.get(launcherToTargetDistance) + : hubTimeOfFlightMap.get(launcherToTargetDistance); + Pose2d lookaheadPose = launcherPose; + double lookaheadLauncherToTargetDistance = launcherToTargetDistance; + + for (int i = 0; i < 20; i++) { + timeOfFlight = + passing + ? passingTimeOfFlightMap.get(lookaheadLauncherToTargetDistance) + : hubTimeOfFlightMap.get(lookaheadLauncherToTargetDistance); + double offsetX = xSpeed * timeOfFlight; + double offsetY = ySpeed * timeOfFlight; + lookaheadPose = + new Pose2d( + launcherPose.getTranslation().plus(new Translation2d(offsetX, offsetY)), + launcherPose.getRotation()); + lookaheadLauncherToTargetDistance = + target.getTranslation().toTranslation2d().getDistance(lookaheadPose.getTranslation()); + } + + // calcuate rotation angle + Rotation2d targetRobotAngle = + getDriveAngle(lookaheadPose, target.getTranslation().toTranslation2d()); + + AngularVelocity targetRobotAngularVelocity = + RadiansPerSecond.of( + driveAngleFilter.calculate( + targetRobotAngle + .minus(DriveState.getInstance().getPreviousDriveStats().Pose.getRotation()) + .getRadians() + / loopPeriodSecs)); + + return builder.createLaunchRequest( + passing, lookaheadLauncherToTargetDistance, targetRobotAngularVelocity, targetRobotAngle); + } + + private static Rotation2d getDriveAngle(Pose2d robotPose, Translation2d target) { + Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle(); + Rotation2d hubAngle = + new Rotation2d( + Math.asin( + MathUtil.clamp( + robotPose.getTranslation().getY() + / target.getDistance(robotPose.getTranslation()), + -1.0, + 1.0))); + Rotation2d driveAngle = fieldToHubAngle.plus(hubAngle).plus(robotPose.getRotation()); + return driveAngle; + } +} diff --git a/src/main/java/frc/robot/statemachines/LaunchState.java b/src/main/java/frc/robot/statemachines/LaunchState.java new file mode 100644 index 0000000..33d1f7f --- /dev/null +++ b/src/main/java/frc/robot/statemachines/LaunchState.java @@ -0,0 +1,70 @@ +package frc.robot.statemachines; + +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.Utils; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.shooter.LaunchRequest; +import frc.robot.subsystems.shooter.MappedLaunchRequestBuilder; +import frc.robot.subsystems.shooter.ParabolicLaunchRequestBuilder; + +public class LaunchState { + private static LaunchState single_instance = null; + + private LaunchCalculator currentCalculator = null; + + private LaunchRequest currentRequest = null; + + private LaunchState() {} + + public static synchronized LaunchState getInstance() { + if (single_instance == null) single_instance = new LaunchState(); + return single_instance; + } + + public void activateCalculator(Pose3d target, LaunchType calculatorType) { + if (currentCalculator != null) return; + + if (calculatorType == LaunchType.PARABOLIC) + currentCalculator = new LaunchCalculator(target, new ParabolicLaunchRequestBuilder()); + else if (calculatorType == LaunchType.MAPPED) + currentCalculator = new LaunchCalculator(target, new MappedLaunchRequestBuilder()); + + refreshRequest(); + } + + public void deactivateCalculator() { + currentCalculator = null; + } + + public boolean isActivated() { + return currentCalculator != null; + } + + public LaunchRequest getLaunchRequest() { + if (!isActivated()) return null; + + if (currentRequest.getTimestamp() + 0.02 < Utils.getCurrentTimeSeconds()) refreshRequest(); + return currentRequest; + } + + private void refreshRequest() { + currentRequest = currentCalculator.createLaunchRequest(); + SmartDashboard.putNumber( + "Current Launch Request/Target Robot Angle (degrees)", + currentRequest.getTargetRobotAngle().getDegrees()); + SmartDashboard.putNumber( + "Current Launch Request/Target Flywheel Velocity (rps)", + currentRequest.getFlywheelVelocity().in(RotationsPerSecond)); + SmartDashboard.putNumber( + "Current Launch Request/Target Hood Angle (rotations)", + currentRequest.getHoodTarget().in(Rotations)); + } + + public enum LaunchType { + PARABOLIC, + MAPPED + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainSubsystem.java index e5d6eb7..6e07d96 100644 --- a/src/main/java/frc/robot/subsystems/drive/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DrivetrainSubsystem.java @@ -41,8 +41,6 @@ public DrivetrainSubsystem() { applyDriveGains(); configureAutoBuilder(); configureCANrange(); - // TODO: remove. debugging. - periodic(); } @Override diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java b/src/main/java/frc/robot/subsystems/indexer/IntakePreferences.java similarity index 100% rename from src/main/java/frc/robot/subsystems/indexer/IndexerPreferences.java rename to src/main/java/frc/robot/subsystems/indexer/IntakePreferences.java diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 89cb56a..25b586f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -11,7 +11,7 @@ private IntakeConstants() {} // TODO: Change to actual ports public static final int ROLLER_MOTOR_ID = 2; - public static final int EXTENSION_MOTOR_ID = 1; + public static final int EXTENSION_MOTOR_ID = 3; // TODO: Tune Roller and Extension Motors diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java new file mode 100644 index 0000000..21c135b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/LaunchRequest.java @@ -0,0 +1,59 @@ +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; + +public class LaunchRequest { + private Angle launchHoodTarget; + private AngularVelocity launchVelocity; + private AngularVelocity targetRobotAngularVelocity; + private Rotation2d targetRobotAngle; + private double timestamp; + + public LaunchRequest( + Angle launchHoodTarget, + AngularVelocity launchVelocity, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle, + double timestamp) { + this.launchHoodTarget = + Rotations.of( + launchHoodTarget.in(Degrees) + * ShooterConstants.ROTATIONS_PER_LAUNCH_DEGREE.in( + Rotations)); // converts froms radians to rotations + this.launchVelocity = + RotationsPerSecond.of( + launchVelocity.in(RadiansPerSecond) + / Math.PI + * 2); // converts from Radians Per Second to Rotations Per Second + this.targetRobotAngularVelocity = targetRobotAngularVelocity; + this.targetRobotAngle = targetRobotAngle; + this.timestamp = timestamp; + } + + public Angle getHoodTarget() { + return launchHoodTarget; + } + + public AngularVelocity getFlywheelVelocity() { + return launchVelocity; + } + + public AngularVelocity getTargetRobotAngularVelocity() { + return targetRobotAngularVelocity; + } + + public Rotation2d getTargetRobotAngle() { + return targetRobotAngle; + } + + public double getTimestamp() { + return timestamp; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java index 7264a32..6107652 100644 --- a/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/LaunchRequestBuilder.java @@ -1,22 +1,12 @@ -// Copyright (c) FIRST and other WPILib contributors. -// 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.shooter; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; -/** Add your docs here. */ public interface LaunchRequestBuilder { - - public record LaunchRequest( - Angle launchHoodTarget, - AngularVelocity launchVelocity, - LinearVelocity driveVelocity, - Rotation2d driveAngle) {} - - public LaunchRequest createLaunchRequest(); + public LaunchRequest createLaunchRequest( + boolean passing, + double distance, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle); } diff --git a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java index c065b2e..248d3bf 100644 --- a/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/MappedLaunchRequestBuilder.java @@ -4,69 +4,36 @@ package frc.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; +import com.ctre.phoenix6.Utils; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.math.interpolation.InverseInterpolator; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; -import frc.robot.statemachines.DriveState; -import java.util.function.Supplier; /** Add your docs here. */ public class MappedLaunchRequestBuilder implements LaunchRequestBuilder { - private Supplier targetPose; - - private LaunchRequest currentLaunchRequest; - - public MappedLaunchRequestBuilder(Supplier targetPose) { - this.targetPose = targetPose; - } - - private double loopPeriodSecs = 0.02; - - private double lastHoodAngle; - private Rotation2d lastDriveAngle; - - private final LinearFilter hoodAngleFilter = - LinearFilter.movingAverage((int) (0.1 / loopPeriodSecs)); - private final LinearFilter driveAngleFilter = - LinearFilter.movingAverage((int) (0.8 / loopPeriodSecs)); - private static final double minDistance; private static final double maxDistance; private static final double passingMinDistance; private static final double passingMaxDistance; - private static final double phaseDelay; // Launching Maps private static final InterpolatingTreeMap hoodAngleMap = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate); private static final InterpolatingDoubleTreeMap flywheelSpeedMap = new InterpolatingDoubleTreeMap(); - private static final InterpolatingDoubleTreeMap timeOfFlightMap = - new InterpolatingDoubleTreeMap(); // Passing Maps private static final InterpolatingTreeMap passingHoodAngleMap = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), Rotation2d::interpolate); private static final InterpolatingDoubleTreeMap passingFlywheelSpeedMap = new InterpolatingDoubleTreeMap(); - private static final InterpolatingDoubleTreeMap passingTimeOfFlightMap = - new InterpolatingDoubleTreeMap(); // TODO: All of this is made up. Need real numbers. static { @@ -74,7 +41,6 @@ public MappedLaunchRequestBuilder(Supplier targetPose) { maxDistance = 5.60; passingMinDistance = 0.0; passingMaxDistance = 100000; - phaseDelay = 0.03; hoodAngleMap.put(1.34, Rotation2d.fromDegrees(19.0)); hoodAngleMap.put(1.78, Rotation2d.fromDegrees(19.0)); @@ -98,117 +64,34 @@ public MappedLaunchRequestBuilder(Supplier targetPose) { flywheelSpeedMap.put(5.57, 275.0); flywheelSpeedMap.put(5.60, 290.0); - timeOfFlightMap.put(5.68, 1.16); - timeOfFlightMap.put(4.55, 1.12); - timeOfFlightMap.put(3.15, 1.11); - timeOfFlightMap.put(1.88, 1.09); - timeOfFlightMap.put(1.38, 0.90); - passingHoodAngleMap.put(passingMinDistance, Rotation2d.fromDegrees(0.0)); passingHoodAngleMap.put(passingMaxDistance, Rotation2d.fromDegrees(0.0)); passingFlywheelSpeedMap.put(passingMinDistance, 0.0); passingFlywheelSpeedMap.put(passingMaxDistance, 0.0); - - passingTimeOfFlightMap.put(passingMinDistance, 0.0); - passingTimeOfFlightMap.put(passingMaxDistance, 0.0); } - public LaunchRequest createLaunchRequest() { - - boolean passing = targetPose.get().getZ() == 0.0; // target pose is the floor, so we're passing. - - // current pose and movement - Pose2d estimatedRobotPose = DriveState.getInstance().getCurrentDriveStats().Pose; - ChassisSpeeds robotSpeeds = DriveState.getInstance().getCurrentDriveStats().Speeds; - - // predicted pose - estimatedRobotPose.exp( - new Twist2d( - robotSpeeds.vxMetersPerSecond * phaseDelay, - robotSpeeds.vyMetersPerSecond * phaseDelay, - robotSpeeds.omegaRadiansPerSecond * phaseDelay)); - - // TODO: for now assume they're the same. calculate offsets later - Pose2d launcherPose = estimatedRobotPose; - double launcherToTargetDistance = - launcherPose.getTranslation().getDistance(targetPose.get().toPose2d().getTranslation()); - - // Calculate field relative velocity - double xVelocity = DriveState.getInstance().getFieldVelocity().vxMetersPerSecond; - double yVelocity = DriveState.getInstance().getFieldVelocity().vyMetersPerSecond; - - // Account for imparted velocity by robot (launcher) to offset - double timeOfFlight = - passing - ? passingTimeOfFlightMap.get(launcherToTargetDistance) - : timeOfFlightMap.get(launcherToTargetDistance); - Pose2d lookaheadPose = launcherPose; - double lookaheadLauncherToTargetDistance = launcherToTargetDistance; - - for (int i = 0; i < 20; i++) { - timeOfFlight = - passing - ? passingTimeOfFlightMap.get(lookaheadLauncherToTargetDistance) - : timeOfFlightMap.get(lookaheadLauncherToTargetDistance); - double offsetX = xVelocity * timeOfFlight; - double offsetY = yVelocity * timeOfFlight; - lookaheadPose = - new Pose2d( - launcherPose.getTranslation().plus(new Translation2d(offsetX, offsetY)), - launcherPose.getRotation()); - lookaheadLauncherToTargetDistance = - targetPose - .get() - .getTranslation() - .toTranslation2d() - .getDistance(lookaheadPose.getTranslation()); - } - - // calcuate rotation angle - Rotation2d driveAngle = - getDriveAngle(lookaheadPose, targetPose.get().getTranslation().toTranslation2d()); - // if no last drive angle, default to the robot's current pose info. - if (lastDriveAngle == null) lastDriveAngle = estimatedRobotPose.getRotation(); + public LaunchRequest createLaunchRequest( + boolean passing, + double distance, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle) { // calculate hood angle double hoodAngle = passing - ? passingHoodAngleMap.get(lookaheadLauncherToTargetDistance).getRadians() - : hoodAngleMap.get(lookaheadLauncherToTargetDistance).getRadians(); + ? passingHoodAngleMap.get(distance).getRadians() + : hoodAngleMap.get(distance).getRadians(); // calculate flywheel speed double flywheelSpeed = - passing - ? passingFlywheelSpeedMap.get(lookaheadLauncherToTargetDistance) - : flywheelSpeedMap.get(lookaheadLauncherToTargetDistance); - - // filter for smoothing - double driveVelocity = - driveAngleFilter.calculate(driveAngle.minus(lastDriveAngle).getRadians() / loopPeriodSecs); - lastDriveAngle = driveAngle; - - currentLaunchRequest = - new LaunchRequest( - Angle.ofBaseUnits(hoodAngle, Radians), - AngularVelocity.ofBaseUnits(flywheelSpeed, RadiansPerSecond), - LinearVelocity.ofBaseUnits(driveVelocity, MetersPerSecond), - driveAngle); - - return currentLaunchRequest; - } - - private static Rotation2d getDriveAngle(Pose2d robotPose, Translation2d target) { - Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle(); - Rotation2d hubAngle = - new Rotation2d( - Math.asin( - MathUtil.clamp( - robotPose.getTranslation().getY() - / target.getDistance(robotPose.getTranslation()), - -1.0, - 1.0))); - Rotation2d driveAngle = fieldToHubAngle.plus(hubAngle).plus(robotPose.getRotation()); - return driveAngle; + passing ? passingFlywheelSpeedMap.get(distance) : flywheelSpeedMap.get(distance); + + return new LaunchRequest( + Angle.ofBaseUnits(hoodAngle, Radians), + AngularVelocity.ofBaseUnits(flywheelSpeed, RadiansPerSecond), + targetRobotAngularVelocity, + targetRobotAngle, + Utils.getCurrentTimeSeconds()); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java index 0d06bdd..a7e4418 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java +++ b/src/main/java/frc/robot/subsystems/shooter/ParabolicLaunchRequestBuilder.java @@ -6,35 +6,29 @@ import static edu.wpi.first.units.Units.*; -import edu.wpi.first.math.geometry.Pose3d; +import com.ctre.phoenix6.Utils; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearVelocity; -import frc.robot.statemachines.DriveState; -import java.util.function.Supplier; /** Add your docs here. */ public class ParabolicLaunchRequestBuilder implements LaunchRequestBuilder { - private Supplier targetPose; - - private ParabolicLaunchRequestBuilder(Supplier targetPose) { - this.targetPose = targetPose; - } - - public LaunchRequest createLaunchRequest() { + public LaunchRequest createLaunchRequest( + boolean passing, + double distance, + AngularVelocity targetRobotAngularVelocity, + Rotation2d targetRobotAngle) { double y1 = ShooterConstants.SHOOTER_HEIGHT.in(Meters); - double x2 = - DriveState.getInstance() - .getCurrentDriveStats() - .Pose - .getTranslation() - .getDistance(targetPose.get().getTranslation().toTranslation2d()); - // double y2 = ShooterConstants.GOAL_HEIGHT.in(Meters); - double y2 = targetPose.get().getMeasureZ().in(Meters); + double x2 = distance; + double y2 = passing ? 0 : ShooterConstants.HUB_HEIGHT.in(Meters); + + double slope; + if (passing) slope = ShooterConstants.OPTIMAL_PASSING_ENTRY_SLOPE; + else slope = ShooterConstants.OPTIMAL_HUB_ENTRY_SLOPE; - double slope = ShooterConstants.OPTIMAL_ENTRY_SLOPE; - double a, b, vertex; + double a, b, vertex, hitWallCheckX; Angle theta, motorAngle; do { // system of equations @@ -45,8 +39,11 @@ public LaunchRequest createLaunchRequest() { theta = Radians.of(Math.atan(b)); // launch angle (Hood Angle Conversion: MATH.PI/2 - theta) motorAngle = Radians.of(Math.PI / 2 - theta.in(Radians)); vertex = -1 * b / (2 * a); + hitWallCheckX = x2 - ShooterConstants.FROM_HUB_CENTER_TO_WALL.in(Meters); slope -= 0.05; - } while ((vertex > x2 - ShooterConstants.MIN_VERTEX_DISTANCE.in(Meters)) + } while ((passing + || a * Math.pow(hitWallCheckX, 2) + b * hitWallCheckX + y1 + > ShooterConstants.HUB_HEIGHT.in(Meters)) && !(motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees))); if (motorAngle.in(Degrees) < ShooterConstants.MIN_HOOD_ANGLE.in(Degrees)) return null; @@ -67,9 +64,10 @@ public LaunchRequest createLaunchRequest() { AngularVelocity angularVelocity = RadiansPerSecond.of(angularVelocityMagnitude); return new LaunchRequest( - theta, + motorAngle, angularVelocity, - LinearVelocity.ofBaseUnits(0, MetersPerSecond), - DriveState.getInstance().getCurrentDriveStats().Pose.getRotation()); + targetRobotAngularVelocity, + targetRobotAngle, + Utils.getCurrentTimeSeconds()); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index cf933d1..fb7d3ef 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -94,11 +94,12 @@ public static MotorOutputConfigs createHoodMotorOutputConfigs() { // Lemon Yeeting Constants public static final Distance SHOOTER_HEIGHT = Inch.of(65); // TODO: Get Better Estimate - public static final Distance GOAL_HEIGHT = Inch.of(23); // TODO: Get Better Estimate - public static final Distance OFFSET_DISTANCE = Meter.of(1); // TODO: Get Better Estimate + public static final Distance HUB_HEIGHT = Inch.of(23); // TODO: Get Better Estimate + public static final Distance FROM_HUB_CENTER_TO_WALL = Inch.of(23.5); public static final Distance MIN_VERTEX_DISTANCE = Inch.of(23.5); public static final Angle MIN_HOOD_ANGLE = Degrees.of(0); // TODO: Get Better Estimate - public static final double OPTIMAL_ENTRY_SLOPE = -1; // TODO: Tune + public static final double OPTIMAL_PASSING_ENTRY_SLOPE = -1; // TODO: Tune + public static final double OPTIMAL_HUB_ENTRY_SLOPE = -1; // TODO: Tune // TODO: verify these! // funnel poses. diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 0816ce0..efc8ca0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -12,11 +12,12 @@ import edu.wpi.first.epilogue.Logged.Importance; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.statemachines.LaunchState; +import frc.robot.subsystems.intake.IntakeConstants; @Logged public class ShooterSubsystem extends SubsystemBase { @@ -24,8 +25,6 @@ public class ShooterSubsystem extends SubsystemBase { private final TalonFX flywheelMotorFollower; private final TalonFX hoodMotor; - private final LaunchRequestBuilder launchRequestBuilder; - @Logged(name = "Velocity Target", importance = Importance.CRITICAL) private AngularVelocity velocityTarget; // Rotations Per Second @@ -36,6 +35,8 @@ public class ShooterSubsystem extends SubsystemBase { private PositionTorqueCurrentFOC hoodControl; + private final LaunchState launchState = LaunchState.getInstance(); + final SysIdRoutine m_sysIdRoutineFlywheel = new SysIdRoutine( new SysIdRoutine.Config( @@ -46,31 +47,6 @@ public class ShooterSubsystem extends SubsystemBase { state -> SignalLogger.writeString("SysIdFlywheel_State", state.toString())), new SysIdRoutine.Mechanism(output -> setFlywheelVoltage(output.magnitude()), null, this)); - public class LaunchRequest { - private Angle launchHoodTarget; - private AngularVelocity launchVelocity; - - public LaunchRequest(Angle theta, LinearVelocity velocity) { - // hood angle in degrees (0 degrees is all the way back) is converted to motor rotations - launchHoodTarget = - Rotations.of( - theta.in(Degrees) * ShooterConstants.ROTATIONS_PER_LAUNCH_DEGREE.in(Rotations)); - // meters per second is converted to rotations per second of the flywheel - launchVelocity = - RotationsPerSecond.of( - velocity.in(MetersPerSecond) - / (2 * Math.PI * ShooterConstants.FLYWHEEL_RADIUS.in(Meters))); - } - - public Angle getHoodTarget() { - return launchHoodTarget; - } - - public AngularVelocity getVelocityTarget() { - return launchVelocity; - } - } - public ShooterSubsystem() { flywheelMotorLeader = new TalonFX(ShooterConstants.FLYWHEEL_LEADER_MOTOR_ID); flywheelMotorFollower = new TalonFX(ShooterConstants.FLYWHEEL_FOLLOWER_MOTOR_ID); @@ -91,20 +67,21 @@ public ShooterSubsystem() { velocityControl = new VelocityVoltage(0); hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorSlot0Configs()); - hoodMotor.getConfigurator().apply(ShooterConstants.createHoodMotorOutputConfigs()); hoodMotor.getConfigurator().apply(ShooterConstants.createHoodSoftwareLimitSwitchConfigs()); hoodTarget = Rotations.of(0); hoodControl = new PositionTorqueCurrentFOC(0); - - launchRequestBuilder = new MappedLaunchRequestBuilder(() -> ShooterConstants.BLUE_TARGET); } @Override public void periodic() { + if (launchState.isActivated()) { + hoodTarget = launchState.getLaunchRequest().getHoodTarget(); + velocityTarget = launchState.getLaunchRequest().getFlywheelVelocity(); + } + flywheelMotorLeader.setControl( velocityControl.withVelocity(velocityTarget.in(RotationsPerSecond))); // hoodMotor.setControl(hoodControl.withPosition(hoodTarget.in(Rotations))); - // launchRequestBuilder.createLaunchRequest(); } public Command spinFlywheelCommand() { @@ -128,7 +105,7 @@ private void setFlywheelVoltage(double magnitude) { @Logged(name = "At Hood Setpoint", importance = Importance.CRITICAL) public boolean atHoodSetpoint() { return Math.abs(hoodMotor.getPosition().getValueAsDouble() - hoodTarget.in(Rotations)) - < ShooterConstants.ALLOWABLE_HOOD_ERROR; + < IntakeConstants.ALLOWABLE_EXTENSION_ERROR; } public Command setHoodCommand(Angle position) { diff --git a/src/main/java/frc/robot/subsystems/vision/CameraConstants.java b/src/main/java/frc/robot/subsystems/vision/CameraConstants.java index 7808d93..7281955 100644 --- a/src/main/java/frc/robot/subsystems/vision/CameraConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/CameraConstants.java @@ -15,14 +15,17 @@ public class CameraConstants { public static final String photonCameraName_Front = "FRONT-CAMERA"; - public static final Transform3d photonCameraTransform_Front = + public static final Transform3d photonCameraTransform_Front_STATIC = + new Transform3d(new Translation3d(0.3175, 0.0706, 0.2477), new Rotation3d(0, 0, 0)); + + public static final Transform3d photonCameraTransform_Front_TILTED = new Transform3d( - new Translation3d(-0.1651, 0, 0.7191), new Rotation3d(0, Math.toRadians(-20), 0)); + new Translation3d(0.3124, 0.0706, 0.3543), new Rotation3d(0, Math.toRadians(35), 0)); public static final PhotonCamera photonCamera_Front = new PhotonCamera(photonCameraName_Front); public static final PhotonPoseEstimator photonPoseEstimator_Front = - new PhotonPoseEstimator(layout, photonCameraTransform_Front); + new PhotonPoseEstimator(layout, photonCameraTransform_Front_STATIC); public static final double MAXIMUM_ALLOWED_AMBIGUITY = 0.2; }