diff --git a/src/main/deploy/pathplanner/autos/Middle Split 2.auto b/src/main/deploy/pathplanner/autos/Middle Split 2.auto index fc3c55c..2fc5f2c 100644 --- a/src/main/deploy/pathplanner/autos/Middle Split 2.auto +++ b/src/main/deploy/pathplanner/autos/Middle Split 2.auto @@ -76,7 +76,7 @@ { "type": "named", "data": { - "name": "InstantShoot" + "name": "AutoAimAndShoot" } } ] diff --git a/src/main/deploy/pathplanner/paths/Middle Split 2 A.path b/src/main/deploy/pathplanner/paths/Middle Split 2 A.path index 1633b4a..18c26f1 100644 --- a/src/main/deploy/pathplanner/paths/Middle Split 2 A.path +++ b/src/main/deploy/pathplanner/paths/Middle Split 2 A.path @@ -17,47 +17,47 @@ { "anchor": { "x": 2.9, - "y": 6.23 + "y": 6.25 }, "prevControl": { "x": 2.05, - "y": 6.23 + "y": 6.25 }, "nextControl": { - "x": 5.9, - "y": 6.23 + "x": 3.9, + "y": 6.25 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.05, - "y": 7.33 + "x": 5.836521079212586, + "y": 6.25 }, "prevControl": { - "x": 8.050493794188853, - "y": 8.179999856568987 + "x": 4.868260539606293, + "y": 6.340625 }, "nextControl": { - "x": 8.049709532830086, - "y": 6.830000084371184 + "x": 6.804781618818879, + "y": 6.159375 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.0, - "y": 5.95 + "x": 8.023042158425174, + "y": 5.6 }, "prevControl": { - "x": 8.0, - "y": 6.45 + "x": 8.023042158425174, + "y": 5.85 }, "nextControl": { - "x": 8.0, - "y": 5.05 + "x": 8.023042158425174, + "y": 5.35 }, "isLocked": false, "linkedName": null @@ -114,15 +114,10 @@ "rotationDegrees": 0, "rotateFast": false }, - { - "waypointRelativePos": 2.0, - "rotationDegrees": -8.841524793534786, - "rotateFast": false - }, { "waypointRelativePos": 3.0, - "rotationDegrees": -60.0, - "rotateFast": true + "rotationDegrees": 0.0, + "rotateFast": false }, { "waypointRelativePos": 5.0, @@ -133,6 +128,11 @@ "waypointRelativePos": 4.0, "rotationDegrees": 0.0, "rotateFast": false + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 0, + "rotateFast": false } ], "constraintZones": [ @@ -149,8 +149,8 @@ }, { "name": "Slow Poop", - "minWaypointRelativePos": 1.9, - "maxWaypointRelativePos": 2.8499999999999996, + "minWaypointRelativePos": 2.8, + "maxWaypointRelativePos": 3.45, "constraints": { "maxVelocity": 1.5, "maxAcceleration": 4.25, @@ -196,7 +196,7 @@ }, { "name": "Poop Stop", - "waypointRelativePos": 2.55, + "waypointRelativePos": 1.4, "command": { "type": "parallel", "data": { @@ -213,7 +213,7 @@ }, { "name": "Collect", - "waypointRelativePos": 2.65, + "waypointRelativePos": 2.45, "command": { "type": "parallel", "data": { @@ -247,7 +247,7 @@ }, { "name": "Shoot Prep", - "waypointRelativePos": 3.5, + "waypointRelativePos": 3.75, "command": { "type": "parallel", "data": { @@ -264,7 +264,7 @@ }, { "name": "Enable LL Update", - "waypointRelativePos": 1.65, + "waypointRelativePos": 1.85, "command": { "type": "parallel", "data": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d182bba..3edf17d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -357,6 +358,20 @@ private void configureCoDriverController() { new ParallelDeadlineGroup( new IntakeNoteSequence(SHOOTER, INTAKE, WRIST, ELEVATOR), new DriveToNoteAuto(DRIVETRAIN, INTAKE_PHOTON, SHOOTER, INTAKE, WRIST, ELEVATOR))); + + CO_DRIVER_CONTROLLER + .leftStick() + .onTrue( + new InstantCommand( + () -> { + if (CommandSwerveDrivetrain.getAlliance() == Alliance.Blue) { + DRIVETRAIN.seedFieldRelative( + new Pose2d(1.37, 5.52, Rotation2d.fromDegrees(0.0))); + } else { + DRIVETRAIN.seedFieldRelative( + new Pose2d(15.2, 5.5, Rotation2d.fromDegrees(-180))); + } + })); } private void configureDrivetrain() { @@ -497,6 +512,13 @@ public void configureShuffleboard() { DRIVETRAIN.seedFieldRelative( new Pose2d(15.2, 5.5, Rotation2d.fromDegrees(-180)))) .ignoringDisable(true)); + COMMANDS_TAB.add( + "SetPoseBlueSubStart", + new InstantCommand( + () -> + DRIVETRAIN.seedFieldRelative( + new Pose2d(1.37, 5.52, Rotation2d.fromDegrees(0.0)))) + .ignoringDisable(true)); AUTO_CHOOSER.addOption( "SYSID-QUAS-F", DRIVETRAIN.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); diff --git a/src/main/java/frc/robot/commands/control/auto/Madtown.java b/src/main/java/frc/robot/commands/control/auto/Madtown.java index ed54265..0fe4ad9 100644 --- a/src/main/java/frc/robot/commands/control/auto/Madtown.java +++ b/src/main/java/frc/robot/commands/control/auto/Madtown.java @@ -31,13 +31,18 @@ public Madtown() { new ParallelRaceGroup( AutoBuilder.buildAuto("Madtown Initial"), new WaitUntilCommand( - () -> { - final boolean shouldInterrupt = SHOULD_WATCH_FOR_NOTE && RobotContainer.INTAKE_PHOTON.hasTargets() && Math.abs(RobotContainer.INTAKE_PHOTON.getYawVal()) < 10.0; + () -> { + final boolean shouldInterrupt = + SHOULD_WATCH_FOR_NOTE + && RobotContainer.INTAKE_PHOTON.hasTargets() + && Math.abs(RobotContainer.INTAKE_PHOTON.getYawVal()) < 20.0; INITIAL_WAS_INTERRUPTED = shouldInterrupt; return shouldInterrupt; })), new ConditionalCommand( - new AutoCollectNote(2.75) // TODO if this fails near note 4, then try for note 5, probably won't have time in 15 seconds + new AutoCollectNote( + 2.75) // TODO if this fails near note 4, then try for note 5, probably won't + // have time in 15 seconds .withTimeout(1.0) .andThen(Util.PathFindToAutoSourceCloseShot()) .andThen( @@ -48,10 +53,13 @@ public Madtown() { new AutoCollectNote(2.5), Util.PathFindToAutoMadtownShot(), new AutoAimAndShoot(RobotContainer.DRIVETRAIN, RobotContainer.SHOOTER), + // new InstantShoot(RobotContainer.SHOOTER), new RotateUntilNote(false), new AutoCollectNote(2.75), Util.PathFindToAutoMadtownShot(), - new AutoAimAndShoot(RobotContainer.DRIVETRAIN, RobotContainer.SHOOTER)), + new AutoAimAndShoot(RobotContainer.DRIVETRAIN, RobotContainer.SHOOTER) + // new InstantShoot(RobotContainer.SHOOTER) + ), new AutoAimLockWrist(RobotContainer.WRIST, RobotContainer::getAutoState), new AutoIdleShooter(RobotContainer.SHOOTER, RobotContainer::getAutoState)); } diff --git a/src/main/java/frc/robot/commands/control/auto/RotateUntilNote.java b/src/main/java/frc/robot/commands/control/auto/RotateUntilNote.java index 63b4278..acfb29d 100644 --- a/src/main/java/frc/robot/commands/control/auto/RotateUntilNote.java +++ b/src/main/java/frc/robot/commands/control/auto/RotateUntilNote.java @@ -5,9 +5,6 @@ package frc.robot.commands.control.auto; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; - -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; @@ -33,7 +30,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - final double velocityScale = RobotContainer.INTAKE_PHOTON.hasTargets() ? 0.5 : 1.0; + final double velocityScale = RobotContainer.INTAKE_PHOTON.hasTargets() ? 0.5 : 1.5; RobotContainer.DRIVETRAIN.setControl( swerveRequest.withSpeeds( new ChassisSpeeds(0.0, 0.0, (Math.PI * velocityScale) * shouldTurnClockwise))); @@ -48,6 +45,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return RobotContainer.INTAKE_PHOTON.hasTargets() && Math.abs(RobotContainer.INTAKE_PHOTON.getYawVal()) < 7.5; + return RobotContainer.INTAKE_PHOTON.hasTargets() + && Math.abs(RobotContainer.INTAKE_PHOTON.getYawVal()) < 7.5; } } diff --git a/src/main/java/frc/robot/util/Util.java b/src/main/java/frc/robot/util/Util.java index c11a4e0..2ee0a8e 100644 --- a/src/main/java/frc/robot/util/Util.java +++ b/src/main/java/frc/robot/util/Util.java @@ -121,12 +121,13 @@ public static double getDistanceToAllianceLob(Pose2d opose) { // BLUE_AUTO_SOURCE_SHOOTING_POSE.getRotation().plus(Rotation2d.fromDegrees(180.0))); // // new Pose2d(13.04, 3.22, Rotation2d.fromDegrees(-150)); - - public static final Pose2d BLUE_AUTO_SOURCE_CLOSE_SHOT = new Pose2d(3.7, 3.15, Rotation2d.fromDegrees(-30)); - public static final Pose2d RED_AUTO_SOURCE_CLOSE_SHOT = new Pose2d( - 16.56 - BLUE_AUTO_SOURCE_CLOSE_SHOT.getX(), - BLUE_AUTO_SOURCE_CLOSE_SHOT.getY(), - Rotation2d.fromDegrees(-150)); + public static final Pose2d BLUE_AUTO_SOURCE_CLOSE_SHOT = + new Pose2d(3.7, 3.15, Rotation2d.fromDegrees(-30)); + public static final Pose2d RED_AUTO_SOURCE_CLOSE_SHOT = + new Pose2d( + 16.56 - BLUE_AUTO_SOURCE_CLOSE_SHOT.getX(), + BLUE_AUTO_SOURCE_CLOSE_SHOT.getY(), + Rotation2d.fromDegrees(-150)); public static Command PathFindToAutoSourceCloseShot() { return new InstantCommand(() -> RobotContainer.setAutoState(AutoState.SHOOT_PREP))