diff --git a/src/main/deploy/pathplanner/paths/a7CollectLoop.path b/src/main/deploy/pathplanner/paths/a7CollectLoop.path index 805fe79..65eb0bb 100644 --- a/src/main/deploy/pathplanner/paths/a7CollectLoop.path +++ b/src/main/deploy/pathplanner/paths/a7CollectLoop.path @@ -33,47 +33,47 @@ { "anchor": { "x": 6.708995782632744, - "y": 2.9864684043141585 + "y": 3.7499931899889374 }, "prevControl": { "x": 7.697838255014892, - "y": 2.580282702370341 + "y": 3.34380748804512 }, "nextControl": { "x": 5.825374028251257, - "y": 3.349432711148226 + "y": 4.112957496823005 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.129280835176992, - "y": 4.461494607300884 + "x": 6.008044558904868, + "y": 4.801829300331858 }, "prevControl": { - "x": 6.156139549466578, - "y": 4.0372859451716305 + "x": 6.034903273194454, + "y": 4.377620638202604 }, "nextControl": { - "x": 6.1024221208874065, - "y": 4.885703269430138 + "x": 5.981185844615283, + "y": 5.2260379624611115 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.654466848727876, - "y": 5.592790652654867 + "x": 4.634185011061947, + "y": 5.56311065403761 }, "prevControl": { - "x": 5.071713567647901, - "y": 5.52944676656619 + "x": 5.051431729981973, + "y": 5.499766767948933 }, "nextControl": { - "x": 4.067828395426444, - "y": 5.681850572544342 + "x": 4.047546557760516, + "y": 5.652170573927084 }, "isLocked": false, "linkedName": null @@ -102,8 +102,12 @@ "rotationDegrees": -56.98406371610071 }, { - "waypointRelativePos": 1.7967115097160609, - "rotationDegrees": 165.52779753636696 + "waypointRelativePos": 1.375409915123457, + "rotationDegrees": -78.72123223305731 + }, + { + "waypointRelativePos": 1.9728009259259265, + "rotationDegrees": -177.36077850349736 }, { "waypointRelativePos": 3.0, @@ -136,6 +140,19 @@ "nominalVoltage": 12.0, "unlimited": false } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.8977962551299594, + "maxWaypointRelativePos": 3.0263284456224335, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 300.0, + "nominalVoltage": 12.0, + "unlimited": false + } } ], "pointTowardsZones": [], diff --git a/src/main/deploy/pathplanner/paths/a7DepotCollectFast.path b/src/main/deploy/pathplanner/paths/a7DepotCollectFast.path index dc95a05..1432c13 100644 --- a/src/main/deploy/pathplanner/paths/a7DepotCollectFast.path +++ b/src/main/deploy/pathplanner/paths/a7DepotCollectFast.path @@ -56,8 +56,8 @@ "minWaypointRelativePos": 0, "maxWaypointRelativePos": 0.800786593707251, "constraints": { - "maxVelocity": 0.8, - "maxAcceleration": 1.5, + "maxVelocity": 2.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/a7StartToCollect.path b/src/main/deploy/pathplanner/paths/a7StartToCollect.path index d6df2e3..d4ee0b3 100644 --- a/src/main/deploy/pathplanner/paths/a7StartToCollect.path +++ b/src/main/deploy/pathplanner/paths/a7StartToCollect.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.5, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/tuning/arc.json b/src/main/deploy/tuning/arc.json index 822a538..2cab95f 100755 --- a/src/main/deploy/tuning/arc.json +++ b/src/main/deploy/tuning/arc.json @@ -113,7 +113,7 @@ }, { "distance": 5.3, - "velocity": 68.5 + "velocity": 70.0 } ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 09a39b0..29ce4f2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -303,6 +303,9 @@ private void configureBindings() { gamepad_.rightTrigger().or(operatorGamepad_.rightTrigger()) .whileTrue(RobotCommands.shoot(shooter_, hopper_, intake_, drivebase_)); + gamepad_.rightBumper() + .whileTrue(intake_.hopperEjectSequence().alongWith(hopper_.reverseFeed())); + // When the shooter isnt shooting, get it ready to shoot. shooter_.setDefaultCommand(shooter_.spinUpVelocityCommand(() -> { return RotationsPerSecond.zero(); diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index bd5fcb4..4f2b15e 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -70,7 +70,11 @@ public static void periodic() { inAllianceZone = currentPose.getMeasureX().minus(allianceWall).abs(Meters) < ShooterConstants.Positions.spinUpZone.in(Meters); // Other logging - Logger.recordOutput("ShiftInfo", HubShiftUtil.getOfficialShiftInfo()); + var shift = HubShiftUtil.getOfficialShiftInfo(); + Logger.recordOutput("ShiftInfo", shift); + Logger.recordOutput("Shift/Active", shift.active()); + Logger.recordOutput("Shift/Name", shift.currentShift()); + Logger.recordOutput("Shift/SecondsRemaining", shift.remainingTime()); LoggedTracer.record("RobotState"); } diff --git a/src/main/java/frc/robot/commands/AutoCommands.java b/src/main/java/frc/robot/commands/AutoCommands.java index 8e012ec..c4643a9 100644 --- a/src/main/java/frc/robot/commands/AutoCommands.java +++ b/src/main/java/frc/robot/commands/AutoCommands.java @@ -167,7 +167,7 @@ public static Command a7CoopDepot(Drive drive, Shooter shooter, IntakeSubsystem .andThen(shooter.spinUpForDistanceHoodParked(() -> Meters.of(2))) ), - RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(5)), + RobotCommands.shoot(shooter, hopper, intake, drive).withTimeout(Seconds.of(4.5)), DriveCommands.followPathCommand("a7CollectLoop").deadlineFor( RobotCommands.intake(intake, hopper) diff --git a/src/main/java/frc/robot/commands/robot/RobotCommands.java b/src/main/java/frc/robot/commands/robot/RobotCommands.java index 095ce21..c569c29 100644 --- a/src/main/java/frc/robot/commands/robot/RobotCommands.java +++ b/src/main/java/frc/robot/commands/robot/RobotCommands.java @@ -70,10 +70,10 @@ public static Command shootHubNoAim(Shooter shooter, Hopper hopper, IntakeSubsys () -> !drive.rotationIsNear(RobotState.rotationToHub(), ShooterConstants.defenseTolerance); return Commands.repeatingSequence( - Commands.waitUntil(shouldShoot).deadlineFor(shooter.spinUpForDistance(RobotState::hubDistance)), - shooter.shootAtDistance(RobotState::hubDistance, hopper, intake) + Commands.waitUntil(shouldShoot), + hopper.feedForShooting(shouldShoot, intake) .until(shouldStopShooting) - ); + ).alongWith(shooter.shootAtDistance(RobotState::hubDistance)); } /** @@ -109,7 +109,7 @@ public static Command ferry(Shooter shooter, Hopper hopper, IntakeSubsystem inta return DriveCommands.joystickDriveAtAngle(targetingAngle) .alongWith( - shooter.shootAtDistance(targetDistance, hopper, intake), + shooter.shootAtDistance(targetDistance), Commands.runOnce(() -> { Logger.recordOutput("Ferry/Target", target.get()); Logger.recordOutput("Ferry/IsFerrying", true); diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index 61144a6..fe8580c 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -6,6 +6,7 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; +import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.AutoLogOutput; @@ -16,9 +17,11 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.Mode; +import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.util.LoggedTracer; import frc.robot.util.MapleSimUtil; @@ -110,6 +113,19 @@ private void stopAll() { // Commands + public Command feedForShooting(BooleanSupplier shouldFeed, IntakeSubsystem intake) { + return Commands.parallel( + preShoot().until(shouldFeed) + .andThen(forwardFeed()), + + Commands.run(() -> { + Logger.recordOutput("Command/ReadyForShoot", shouldFeed); + }), + + intake.shakeBalls() + ); + } + /** * Runs the scrambler at its idling speed until the command ends. * @return diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 6c11ebc..1bcd245 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -38,7 +38,6 @@ import frc.robot.Constants.Mode; import frc.robot.RobotState; import frc.robot.subsystems.hopper.Hopper; -import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.ShooterTuning.ShooterParams; import frc.robot.util.LoggedTracer; import frc.robot.util.MapleSimUtil; @@ -245,7 +244,7 @@ public Command hoodToPosCmd(Angle pos) { * * */ - public Command shootAtDistance(Supplier distance, Hopper hopper, IntakeSubsystem intake) { + public Command shootAtDistance(Supplier distance) { Supplier shooterParams = () -> getTuning().getShooterParams(distance.get().in(Meters)); @@ -254,41 +253,10 @@ public Command shootAtDistance(Supplier distance, Hopper hopper, Intak runDynamicSetpoints( () -> RotationsPerSecond.of(shooterParams.get().velocity), () -> Degrees.of(shooterParams.get().hood) - ), - - hopper.preShoot().until(this::isShooterReady).andThen( - hopper.forwardFeed().deadlineFor(Commands.run(() -> { - var diff = shooterTarget.minus(shooterInputs.wheelVelocity).abs(RadiansPerSecond); - // Ball counting? - if (diff > 15.0) { - if (!isCounted) { - addBallToCount(); - } - isCounted = true; - } else { - isCounted = false; - } - - Logger.recordOutput("Stats/IsCounted", isCounted); - }).onlyIf(() -> Constants.getMode() == Mode.REPLAY)) - ), - - Commands.run(() -> { - Logger.recordOutput("Command/ReadyForShoot", isShooterReady()); - }), - - intake.shakeBalls() + ) ).finallyDo(interrupted -> Logger.recordOutput("Command/ShootAtDistance", false)); } - private int ballsShot = 0; - private boolean isCounted = false; - - private void addBallToCount() { - ballsShot++; - Logger.recordOutput("Stats/BallsShot", ballsShot); - } - /** * Spins the shooter up to shoot, without actually shooting. * @param distance