diff --git a/src/main/deploy/pathplanner/paths/A to D.path b/src/main/deploy/pathplanner/paths/A to D.path index 6cc3538e..38f1afc4 100644 --- a/src/main/deploy/pathplanner/paths/A to D.path +++ b/src/main/deploy/pathplanner/paths/A to D.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.9000000000000004, - "y": 6.887856966597124 + "x": 2.883271729453586, + "y": 6.722020177278586 }, "prevControl": null, "nextControl": { - "x": 3.890268068741571, - "y": 7.027030067557189 + "x": 3.8735397981951567, + "y": 6.861193278238651 }, "isLocked": false, "linkedName": "A" diff --git a/src/main/deploy/pathplanner/paths/A to E.path b/src/main/deploy/pathplanner/paths/A to E.path index bd9dae7c..2d06fc84 100644 --- a/src/main/deploy/pathplanner/paths/A to E.path +++ b/src/main/deploy/pathplanner/paths/A to E.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.9000000000000004, - "y": 6.887856966597124 + "x": 2.883271729453586, + "y": 6.722020177278586 }, "prevControl": null, "nextControl": { - "x": 5.01984991243836, - "y": 6.971014628418452 + "x": 5.003121641891946, + "y": 6.805177839099914 }, "isLocked": false, "linkedName": "A" diff --git a/src/main/deploy/pathplanner/paths/B to A.path b/src/main/deploy/pathplanner/paths/B to A.path index f8febcf0..2b33b39f 100644 --- a/src/main/deploy/pathplanner/paths/B to A.path +++ b/src/main/deploy/pathplanner/paths/B to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.9, - "y": 5.27 + "x": 2.883271729453586, + "y": 5.059081387777612 }, "prevControl": null, "nextControl": { - "x": 2.7306942929129034, - "y": 5.892387378210238 + "x": 2.7139660223664896, + "y": 5.68146876598785 }, "isLocked": false, "linkedName": "B" }, { "anchor": { - "x": 2.9000000000000004, - "y": 6.887856966597124 + "x": 2.883271729453586, + "y": 6.722020177278586 }, "prevControl": { - "x": 2.7469122475281997, - "y": 6.452168507627614 + "x": 2.7301839769817855, + "y": 6.286331718309076 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/B to F.path b/src/main/deploy/pathplanner/paths/B to F.path index 280a89d7..7212089f 100644 --- a/src/main/deploy/pathplanner/paths/B to F.path +++ b/src/main/deploy/pathplanner/paths/B to F.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.9, - "y": 5.27 + "x": 2.883271729453586, + "y": 5.059081387777612 }, "prevControl": null, "nextControl": { - "x": 4.511078009317217, - "y": 5.211322262872771 + "x": 4.494349738770803, + "y": 5.000403650650383 }, "isLocked": false, "linkedName": "B" diff --git a/src/main/deploy/pathplanner/paths/C to B Close.path b/src/main/deploy/pathplanner/paths/C to B Close.path index a1916338..a71d499a 100644 --- a/src/main/deploy/pathplanner/paths/C to B Close.path +++ b/src/main/deploy/pathplanner/paths/C to B Close.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.9, - "y": 5.27 + "x": 2.883271729453586, + "y": 5.059081387777612 }, "prevControl": { - "x": 2.5981160273115327, - "y": 5.094712531987342 + "x": 2.581387756765119, + "y": 4.883793919764954 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/C to B.path index 44480865..023d2aa7 100644 --- a/src/main/deploy/pathplanner/paths/C to B.path +++ b/src/main/deploy/pathplanner/paths/C to B.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.9, - "y": 5.27 + "x": 2.883271729453586, + "y": 5.059081387777612 }, "prevControl": { - "x": 2.576402701523351, - "y": 4.817991630285256 + "x": 2.559674430976937, + "y": 4.607073018062868 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/D to Ferry Shot.path b/src/main/deploy/pathplanner/paths/D to Ferry Shot.path index 3fdde5f7..5fad0193 100644 --- a/src/main/deploy/pathplanner/paths/D to Ferry Shot.path +++ b/src/main/deploy/pathplanner/paths/D to Ferry Shot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.7, - "y": 7.529999999999999 + "x": 7.930642280685416, + "y": 7.483224552754384 }, "prevControl": null, "nextControl": { - "x": 6.782397412017047, - "y": 7.253301738161635 + "x": 7.0130396927024625, + "y": 7.20652629091602 }, "isLocked": false, "linkedName": "D Top Ferry" @@ -29,7 +29,19 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Decrease Acceleration", + "minWaypointRelativePos": 0.65, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.5, diff --git a/src/main/deploy/pathplanner/paths/E to Ferry Shot.path b/src/main/deploy/pathplanner/paths/E to Ferry Shot.path index aebabacf..b31a5817 100644 --- a/src/main/deploy/pathplanner/paths/E to Ferry Shot.path +++ b/src/main/deploy/pathplanner/paths/E to Ferry Shot.path @@ -29,7 +29,19 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Decrease Acceleration", + "minWaypointRelativePos": 0.6, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 5.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, diff --git a/src/main/deploy/pathplanner/paths/NTF Start To D.path b/src/main/deploy/pathplanner/paths/NTF Start To D.path index 8c7f0df7..61a669fa 100644 --- a/src/main/deploy/pathplanner/paths/NTF Start To D.path +++ b/src/main/deploy/pathplanner/paths/NTF Start To D.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 7.7, - "y": 7.529999999999999 + "x": 7.930642280685416, + "y": 7.483224552754384 }, "prevControl": { - "x": 5.922230165687515, - "y": 7.685534706860702 + "x": 6.152872446372931, + "y": 7.638759259615087 }, "nextControl": null, "isLocked": false, @@ -54,11 +54,11 @@ "constraintZones": [ { "name": "Speed Up", - "minWaypointRelativePos": 1.0, + "minWaypointRelativePos": 1.75, "maxWaypointRelativePos": 2.0, "constraints": { "maxVelocity": 4.5, - "maxAcceleration": 9.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 9115cf99..2b97ce31 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -322,8 +322,8 @@ public void configureAutons() { AutonConfig ReroutableHGF_RED = new AutonConfig("4 HGF", ReroutableFourPieceHGF::new, "Start to H (HGF) Red", "H to HShoot (HGF) Red", "HShoot to G (HGF) Red", "G to Shoot (HGF) Red", "GShoot to F (HGF)", "F to Shoot (HGF)", "Rerouted H To G", "Rerouted G To F"); - AutonConfig HGF = new AutonConfig("4 HGF", FourPieceHGF::new, - "Start to H (HGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)", "F to Shoot (HGF)"); + AutonConfig HGF = new AutonConfig("4 HGF", FourPieceHGF::new, + "Start to H (HGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)", "F to Shoot (HGF)"); AutonConfig HGF_RED = new AutonConfig("4 HGF", FourPieceHGF::new, "Start to H (HGF) Red", "H to HShoot (HGF) Red", "HShoot to G (HGF) Red", "G to Shoot (HGF) Red", "GShoot to F (HGF)", "F to Shoot (HGF)"); @@ -403,8 +403,8 @@ public void configureAutons() { CBF.registerBlue(autonChooser) .registerRed(autonChooser); - ReroutableCBAEF.registerBlue(autonChooser) - .registerRed(autonChooser); + // ReroutableCBAEF.registerBlue(autonChooser) + // .registerRed(autonChooser); GC.registerBlue(autonChooser); GC_RED.registerRed(autonChooser); diff --git a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java index 9c7e097d..039cf895 100644 --- a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java @@ -37,7 +37,7 @@ public class AmpScoreRoutine extends SequentialCommandGroup { - private static final double SCORE_ALIGN_TIMEOUT = 1.25; + private static final double SCORE_ALIGN_TIMEOUT = 1.75; // 1.25 private static final double AMP_WALL_SETUP_X_TOLERANCE = Units.inchesToMeters(1.0); private static final double AMP_WALL_SETUP_Y_TOLERANCE = Units.inchesToMeters(4.0); @@ -62,6 +62,7 @@ public AmpScoreRoutine() { new ParallelCommandGroup( new WaitUntilCommand(() -> Amper.getInstance().hasNote()), new SwerveDriveToPose(() -> getTargetPose(Alignment.AMP_WALL_SETUP_DISTANCE.get())) + .withMaxSpeed(3.0) .withTolerance(AMP_WALL_SETUP_X_TOLERANCE, AMP_WALL_SETUP_Y_TOLERANCE, AMP_WALL_SETUP_ANGLE_TOLERANCE) .deadlineWith(new LEDSet(LEDInstructions.AMP_ALIGN)) ), diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java index 305b3169..2e86cd29 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java @@ -72,6 +72,7 @@ public static SwerveDriveToPose speakerRelative(double angleToSpeaker) { private double yTolerance; private double thetaTolerance; private double velocityTolerance; + private double maxSpeed; private Pose2d targetPose; @@ -86,6 +87,7 @@ public SwerveDriveToPose(Supplier poseSupplier) { this.poseSupplier = poseSupplier; targetPose2d = odometry.getField().getObject("Target Pose"); + maxSpeed = Swerve.MAX_MODULE_SPEED; controller = new HolonomicController( new PIDController(Translation.kP, Translation.kI, Translation.kD), @@ -138,6 +140,11 @@ public SwerveDriveToPose withTolerance(Number x, Number y, Number theta) { return this; } + public SwerveDriveToPose withMaxSpeed(double speed) { + maxSpeed = speed; + return this; + } + @Override public void initialize() { targetPose = poseSupplier.get(); @@ -155,7 +162,7 @@ public void execute() { controller.update(targetPose, odometry.getPose()); Vector2D speed = new Vector2D(controller.getOutput().vxMetersPerSecond, controller.getOutput().vyMetersPerSecond) - .clamp(Swerve.MAX_MODULE_SPEED); + .clamp(maxSpeed); double rotation = SLMath.clamp(controller.getOutput().omegaRadiansPerSecond, Motion.MAX_ANGULAR_VELOCITY.get()); SmartDashboard.putNumber("Alignment/Translation Target Speed", speed.distance()); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index fbceec41..f62c2c0c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -326,9 +326,9 @@ public interface Shooter { ShooterSpeeds REVERSE = new ShooterSpeeds(-3000, -3000); ShooterSpeeds FERRY = new ShooterSpeeds( - new SmartNumber("Shooter/Ferry Shooter RPM", 5500), + new SmartNumber("Shooter/Ferry Shooter RPM", 6000), // 5500 500, - new SmartNumber("Shooter/Ferry Feeder RPM", 3000)); + new SmartNumber("Shooter/Ferry Feeder RPM", 3500)); // 3000 ShooterSpeeds WING_FERRY = new ShooterSpeeds(2000, 2500); @@ -405,7 +405,7 @@ public interface Alignment { double PODIUM_SHOT_MAX_ANGLE = 80; SmartNumber AMP_WALL_SETUP_DISTANCE = new SmartNumber("Alignment/Amp/Setup Pose Distance to Wall", Units.inchesToMeters(25.5)); - SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5 - 1.5)); // NOTE: remove "- 1.5" for battlecry + SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5 - 1.75)); // NOTE: remove "- 1.5" for battlecry SmartNumber TRAP_SETUP_DISTANCE = new SmartNumber("Alignment/Trap/Setup Pose Distance", Units.inchesToMeters(21.0)); SmartNumber TRAP_CLIMB_DISTANCE = new SmartNumber("Alignment/Trap/Climb Distance", Units.inchesToMeters(18.0)); diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/TheiaTagVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/TheiaTagVision.java index b8f8d13a..15596336 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/TheiaTagVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/TheiaTagVision.java @@ -70,12 +70,9 @@ public void periodic() { super.periodic(); outputs.clear(); - - boolean hasAnyData = false; - + for (TheiaCamera camera : cameras) { - if (camera.getVisionData().isPresent()) - hasAnyData = true; + SmartDashboard.putBoolean(camera.getName() + "/Has Data", camera.getVisionData().isPresent()); camera.getVisionData().ifPresent( (VisionData data) -> { @@ -84,7 +81,7 @@ public void periodic() { }); } - SmartDashboard.putBoolean("Vision/Has Any Data", hasAnyData); + SmartDashboard.putBoolean("Vision/Has Any Data", outputs.size() > 0); } private void updateTelemetry(String prefix, VisionData data) {