comits

SouthwestRoboticsProgramming · Apr 13, 2024 · f736452 · f736452
1 parent a00058d
commit f736452
Show file tree

Hide file tree

Showing 5 changed files with 84 additions and 11 deletions.
diff --git a/Robot/src/main/deploy/pathplanner/autos/Big Amp.auto b/Robot/src/main/deploy/pathplanner/autos/Big Amp.auto
@@ -90,6 +90,18 @@
  "data": {
  "name": "Wait and Shoot"
  }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "Intake Off"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Dash"
+ }
  }
  ]
  }

diff --git a/Robot/src/main/deploy/pathplanner/paths/Amp Big 2.path b/Robot/src/main/deploy/pathplanner/paths/Amp Big 2.path
@@ -75,7 +75,7 @@
  {
  "type": "named",
  "data": {
- "name": "Shoot"
+ "name": "Shoot No Vision"
  }
  }
  ]

diff --git a/Robot/src/main/deploy/pathplanner/paths/Dash.path b/Robot/src/main/deploy/pathplanner/paths/Dash.path
@@ -0,0 +1,52 @@
+{
+ "version": 1.0,
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.239865262137932,
+ "y": 7.028734983395893
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 4.2402544943978775,
+ "y": 7.123512450728605
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 8.316160664728736,
+ "y": 7.415208515602217
+ },
+ "prevControl": {
+ "x": 7.208760783262307,
+ "y": 7.320906123293729
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 5.7,
+ "maxAcceleration": 3.8,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 640.0
+ },
+ "goalEndState": {
+ "velocity": 3.0,
+ "rotation": -172.82839318105817,
+ "rotateFast": false
+ },
+ "reversed": false,
+ "folder": "Amp",
+ "previewStartingState": {
+ "rotation": -163.10149254281322,
+ "velocity": 0
+ },
+ "useDefaultConstraints": true
+}
diff --git a/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java b/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java
@@ -112,10 +112,11 @@ public RobotContainer() {
  NamedCommands.registerCommand("Intake On", new IntakeSetCommand(intake, IntakeSubsystem.State.INTAKE));
  NamedCommands.registerCommand("Intake Off", new IntakeSetCommand(intake, IntakeSubsystem.State.OFF));
  NamedCommands.registerCommand("Intake Until Note", RobotCommands.intakeUntilNote(this));
- NamedCommands.registerCommand("Shoot", RobotCommands.aimAndShoot(this, false));
+ NamedCommands.registerCommand("Shoot", RobotCommands.aimAndShoot(this, false, true));
  NamedCommands.registerCommand("Shoot Quick", RobotCommands.shootQuick(this));
+ NamedCommands.registerCommand("Shoot No Vision", RobotCommands.aimAndShoot(this, false, false));
  NamedCommands.registerCommand("Fling", RobotCommands.flingNote(this));
- NamedCommands.registerCommand("Wait and Shoot", RobotCommands.aimAndShoot(this, true)); // Waits for indexer to have note
+ NamedCommands.registerCommand("Wait and Shoot", RobotCommands.aimAndShoot(this, true, true)); // Waits for indexer to have note
  NamedCommands.registerCommand("Eject Hard", RobotCommands.ejectHard(this));
 
  // Create a chooser to select the autonomous

diff --git a/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java b/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java
@@ -1,5 +1,8 @@
 package com.swrobotics.robot.commands;
 
+import java.util.ArrayList;
+import java.util.List;
+
 import com.swrobotics.robot.RobotContainer;
 import com.swrobotics.robot.config.NTData;
 import com.swrobotics.robot.subsystems.amp.AmpArm2Subsystem;
@@ -20,25 +23,30 @@ public static Command justShoot(RobotContainer robot) {
  );
  }
 
- public static Command aimAndShoot(RobotContainer robot, boolean waitForNote) {
+ public static Command aimAndShoot(RobotContainer robot, boolean waitForNote, boolean useVision) {
  AimTowardsSpeakerCommand aim = new AimTowardsSpeakerCommand(robot.drive, robot.shooter);
- Command shootSeq = Commands.sequence(
- Commands.runOnce(() -> robot.drive.setEstimatorIgnoreVision(false)),
+
+ List<Command> commands = new ArrayList<>();
+ commands.addAll(List.of(
  Commands.waitUntil(robot.shooter::isCalibrated),
  Commands.waitUntil(() -> (!waitForNote || robot.indexer.hasPiece())
  && aim.isInTolerance(NTData.DRIVE_AIM_TOLERANCE.get())
  && robot.shooter.isReadyToShoot())
  .withTimeout(NTData.SHOOTER_AUTO_READY_TIMEOUT.get()),
  Commands.waitSeconds(NTData.SHOOTER_AUTO_AFTER_READY_DELAY.get()),
- new IndexerFeedCommand(robot.indexer),
- Commands.runOnce(() -> robot.drive.setEstimatorIgnoreVision(true))
- );
+ new IndexerFeedCommand(robot.indexer)
+ ));
+
+ if (useVision) {
+ commands.add(0, Comm`ands.runOnce(() -> robot.drive.setEstimatorIgnoreVision(false)));
+ commands.add(Commands.runOnce(() -> robot.drive.setEstimatorIgnoreVision(true)));
+ }
 
- return new ParallelDeadlineGroup(shootSeq, aim);
+ return new ParallelDeadlineGroup(Commands.sequence(commands.toArray(new Command[0])), aim);
  }
 
  public static Command shootQuick(RobotContainer robot) {
- return aimAndShoot(robot, false);
+ return aimAndShoot(robot, false, true);
  // return Commands.sequence(
  // Commands.runOnce(() -> robot.shooter.forcePivotCalibration(70)),
  // Commands.waitUntil(() -> robot.shooter.isReadyToShoot()),