diff --git a/elastic-layout/robot-configuration.json b/elastic-layout/robot-configuration.json index e990a5c5..0c794420 100644 --- a/elastic-layout/robot-configuration.json +++ b/elastic-layout/robot-configuration.json @@ -172,6 +172,46 @@ "show_type": true, "maximize_button_space": false } + }, + { + "title": "DeviceHealth", + "x": 1152.0, + "y": 128.0, + "width": 256.0, + "height": 384.0, + "type": "Alerts", + "properties": { + "topic": "/AdvantageKit/RealOutputs/DeviceHealth", + "period": 0.06 + } + }, + { + "title": "Hub to Depo to Tower Auto", + "x": 384.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Hub to Depo to Tower Auto", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } + }, + { + "title": "Normal Bump Auto Right", + "x": 640.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Normal Bump Auto Right", + "period": 0.06, + "show_type": true, + "maximize_button_space": false + } } ] } diff --git a/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto b/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto new file mode 100644 index 00000000..6241a1d0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HubToDepoToTower.auto @@ -0,0 +1,87 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 4.0 + } + }, + { + "type": "named", + "data": { + "name": "WarmupShooterNear" + } + }, + { + "type": "named", + "data": { + "name": "FireWhenShooterAndHoodReady" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeDeployExtend" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + }, + { + "type": "path", + "data": { + "pathName": "HubToDepoPath" + } + }, + { + "type": "parallel", + "data": { + "commands": [] + } + }, + { + "type": "path", + "data": { + "pathName": "DepoToTowerPath" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "AimAndShootFromHere" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BackUpFromHub.path b/src/main/deploy/pathplanner/paths/BackUpFromHub.path new file mode 100644 index 00000000..366b2dfa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BackUpFromHub.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5574733096085422, + "y": 4.008102016607354 + }, + "prevControl": null, + "nextControl": { + "x": 3.073309608540926, + "y": 3.997342823250296 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.396, + "y": 5.095 + }, + "prevControl": { + "x": 3.5251957295373675, + "y": 5.955516014234875 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.19871266989296 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.33380002981693 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DepoToTowerPath.path b/src/main/deploy/pathplanner/paths/DepoToTowerPath.path new file mode 100644 index 00000000..bb5bb83c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/DepoToTowerPath.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.69, + "y": 5.945 + }, + "prevControl": null, + "nextControl": { + "x": 0.7610709357401938, + "y": 5.6925275432902565 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0828588374851729, + "y": 4.933392645314354 + }, + "prevControl": { + "x": 0.9434991997628981, + "y": 5.4827779536440655 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.1, + "maxWaypointRelativePos": 0.4, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HubToDepoPath.path b/src/main/deploy/pathplanner/paths/HubToDepoPath.path new file mode 100644 index 00000000..3f2287e6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HubToDepoPath.path @@ -0,0 +1,94 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4, + "y": 5.152 + }, + "prevControl": null, + "nextControl": { + "x": 1.6746144721233698, + "y": 6.3966429418742585 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.69, + "y": 5.944756820877817 + }, + "prevControl": { + "x": 1.4323843416370106, + "y": 5.9124792408066424 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.821147356580429, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.09223847019122663, + "maxWaypointRelativePos": 0.3959505061867341, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeDeployExtend", + "waypointRelativePos": 0.10123734533183272, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "CollectorIntake", + "waypointRelativePos": 0.33070866141732724, + "endWaypointRelativePos": 1.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 2.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 132.20841353697384 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/competition/ConfigurePathPlannerLib.java b/src/main/java/competition/ConfigurePathPlannerLib.java index a7601a1d..865237ed 100644 --- a/src/main/java/competition/ConfigurePathPlannerLib.java +++ b/src/main/java/competition/ConfigurePathPlannerLib.java @@ -7,6 +7,7 @@ import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import competition.auto_programs.AimAndShootFromHereCommand; +import competition.command_groups.FireWhenShooterAndHoodReady; import competition.command_groups.PrepareToShootCommandGroup; import competition.command_groups.WaitForRotationAndHoodAndShooterToBeAtGoalCommandGroup; import competition.subsystems.collector_intake.commands.CollectorIntakeCommand; @@ -27,7 +28,8 @@ public ConfigurePathPlannerLib(PoseSubsystem pose, DriveSubsystem drive, CollectorIntakeCommand collectorIntakeCommand, AimAndShootFromHereCommand aimAndShootFromHereCommand, PrepareToShootCommandGroup prepareToShootCommandGroup, - WaitForRotationAndHoodAndShooterToBeAtGoalCommandGroup waitForRotationAndHoodAndShooterToBeAtGoalCommandGroup + WaitForRotationAndHoodAndShooterToBeAtGoalCommandGroup waitForRotationAndHoodAndShooterToBeAtGoalCommandGroup, + FireWhenShooterAndHoodReady fireWhenShooterAndHoodReady ) { NamedCommands.registerCommand("IntakeDeployExtend", intakeDeployExtendCommand); NamedCommands.registerCommand("CollectorIntake", collectorIntakeCommand); @@ -35,6 +37,7 @@ public ConfigurePathPlannerLib(PoseSubsystem pose, DriveSubsystem drive, NamedCommands.registerCommand("WaitForRotationAndHoodAndShooterToBeAtGoal", waitForRotationAndHoodAndShooterToBeAtGoalCommandGroup); NamedCommands.registerCommand("WarmupShooterNear", prepareToShootCommandGroup.setPresetLocation(TrajectoriesCalculation.PresetShootingDistance.NEAR)); + NamedCommands.registerCommand("FireWhenShooterAndHoodReady", fireWhenShooterAndHoodReady); try { AutoBuilder.configure( diff --git a/src/main/java/competition/injection/components/BaseRobotComponent.java b/src/main/java/competition/injection/components/BaseRobotComponent.java index 2c14f08b..6efce764 100644 --- a/src/main/java/competition/injection/components/BaseRobotComponent.java +++ b/src/main/java/competition/injection/components/BaseRobotComponent.java @@ -1,6 +1,8 @@ package competition.injection.components; import competition.ConfigurePathPlannerLib; +import competition.auto_programs.CollectAndShootTwiceCommand; +import competition.auto_programs.ShootFromHubCommandGroup; import competition.auto_programs.ShootFromTrenchCommandGroup; import competition.operator_interface.OperatorCommandMap; import competition.operator_interface.OperatorInterface; @@ -65,4 +67,11 @@ public abstract class BaseRobotComponent extends BaseComponent { public abstract SuperstructureMechanismSubsystem superstructureMechanismSubsystem(); public abstract ConfigurePathPlannerLib configurePathPlannerLib(); + + public abstract CollectAndShootTwiceCommand collectAndShootTwiceCommand(); + + public abstract ShootFromHubCommandGroup shootFromHubCommandGroup(); + + public abstract ShootFromTrenchCommandGroup shootFromTrenchCommandGroup(); + } diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 105607cc..fac9ee31 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -4,6 +4,7 @@ import javax.inject.Provider; import javax.inject.Singleton; +import com.pathplanner.lib.auto.AutoBuilder; import competition.auto_programs.AimAndShootFromHereCommand; import competition.auto_programs.ppl.LeftBumpAutoCommand; import competition.auto_programs.ShootFromHubCommandGroup; @@ -207,6 +208,14 @@ public void setupAutoCommands(Provider setAutonomousComman var collectAndShootTwice = setAutonomousCommandProvider.get(); collectAndShootTwice.setAutoCommand(collectAndShootTwiceCommand, Landmarks.blueStartTrenchToOutpost); collectAndShootTwice.includeOnSmartDashboard("Collect and shoot twice."); + + var hubToDepoToTower = setAutonomousCommandProvider.get(); + hubToDepoToTower.setAutoCommand(AutoBuilder.buildAuto("HubToDepoToTower")); + hubToDepoToTower.includeOnSmartDashboard("Hub to Depo to Tower Auto"); + + var normalBumpAutoRight = setAutonomousCommandProvider.get(); + normalBumpAutoRight.setAutoCommand(AutoBuilder.buildAuto("NormalBumpAutoRight")); + normalBumpAutoRight.includeOnSmartDashboard("Normal Bump Auto Right"); } @Inject diff --git a/src/main/java/competition/subsystems/hood/HoodSubsystem.java b/src/main/java/competition/subsystems/hood/HoodSubsystem.java index 847fd837..1934e458 100644 --- a/src/main/java/competition/subsystems/hood/HoodSubsystem.java +++ b/src/main/java/competition/subsystems/hood/HoodSubsystem.java @@ -101,6 +101,10 @@ public void retract() { setTargetValue(getTargetValue()); } + public boolean isHoodDown() { + return hoodServoLeft.getNormalizedCurrentPosition() <= 0.05; + } + public void runServo() { if (hoodServoLeft != null && hoodServoRight != null) { hoodServoLeft.setNormalizedTargetPosition(servoTargetNormalized.get());