From fd4448f31242ee9c51194fe8c31684385105cad1 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 18 Apr 2024 08:03:02 -0400 Subject: [PATCH 01/18] COMP Worlds - Added Loging --- src/main/java/frc/robot/Constants.java | 8 ++ src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/command/shoot/NotePass.java | 77 ++++++++++++++----- .../frc/robot/command/shoot/SmartShoot.java | 1 - 4 files changed, 66 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0cdf1316..7812dda6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -18,6 +18,7 @@ import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -163,6 +164,13 @@ public static class ChaseConstants { public static final double CHASE_PIECE_ALIGNMENT_TOLERANCE = 3d; } + public static class PassConstants { + public static final double POINT_TOLERANCE = 3d; + + public static final PIDController PASS_CONTROLLER = new PIDController(0.1, 0, 0.01); + public static final SimpleMotorFeedforward FEED_FORWARD = new SimpleMotorFeedforward(0.25, 0.5); + } + public static class TunerConstants { // Both sets of gains need to be tuned to your individual robot. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f37b261..54bcaaa4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -107,7 +107,6 @@ public class RobotContainer extends LightningContainer { Telemetry logger; private Boolean triggerInit; - private boolean notePassOnTarget = false; public static double bias = 0d; @Override diff --git a/src/main/java/frc/robot/command/shoot/NotePass.java b/src/main/java/frc/robot/command/shoot/NotePass.java index 70e2ae98..6ce94098 100644 --- a/src/main/java/frc/robot/command/shoot/NotePass.java +++ b/src/main/java/frc/robot/command/shoot/NotePass.java @@ -1,23 +1,20 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.command.shoot; -import java.util.function.BooleanSupplier; - import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.util.datalog.BooleanLogEntry; +import edu.wpi.first.util.datalog.DataLog; +import edu.wpi.first.util.datalog.DoubleLogEntry; +import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; import frc.robot.Constants.DrivetrainConstants; +import frc.robot.Constants.PassConstants; import frc.robot.Constants.ShooterConstants; -import frc.robot.Constants.VisionConstants; import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Indexer; import frc.robot.subsystems.Pivot; @@ -34,13 +31,22 @@ public class NotePass extends Command { private Translation2d targetPose; private double currentHeading; private double targetHeading; + private double distanceToCorner; private double feedForwardOutput; private double pidOutput; - private PIDController pidController = VisionConstants.COMBO_CONTROLLER; // TAG_AIM_CONTROLLER - private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0.25, 0.5); + private PIDController pidController = PassConstants.PASS_CONTROLLER; + private SimpleMotorFeedforward feedforward = PassConstants.FEED_FORWARD; private XboxControllerFilter driver; private Indexer indexer; + + private DoubleLogEntry currentHeadingLog; + private DoubleLogEntry targetHeadingLog; + private DoubleLogEntry pidOutputLog; + private DoubleLogEntry distanceToCornerLog; + private BooleanLogEntry headingOnTargetLog; + private BooleanLogEntry shooterOnTargetLog; + /** * Creates a new NotePass. * @@ -67,6 +73,8 @@ public void initialize() { } else { targetPose = DrivetrainConstants.RED_CORNER_POSE; } + + initLogging(); } @Override @@ -75,6 +83,7 @@ public void execute() { var deltaX = targetPose.getX() - pose.getX(); var deltaY = targetPose.getY() - pose.getY(); + distanceToCorner = drivetrain.distanceToCorner(); currentHeading = (pose.getRotation().getDegrees() + 360) % 360; // Calculate vector to target, add 180 to make it point backwards @@ -90,8 +99,8 @@ public void execute() { drivetrain.setField(-driver.getLeftY(), -driver.getLeftX(), feedForwardOutput); - flywheel.setAllMotorsRPM(ShooterConstants.NOTEPASS_SPEED_MAP.get(drivetrain.distanceToCorner()) + flywheel.getBias()); - pivot.setTargetAngle(ShooterConstants.NOTEPASS_ANGLE_MAP.get(drivetrain.distanceToCorner()) + pivot.getBias()); + flywheel.setAllMotorsRPM(ShooterConstants.NOTEPASS_SPEED_MAP.get(distanceToCorner) + flywheel.getBias()); + pivot.setTargetAngle(ShooterConstants.NOTEPASS_ANGLE_MAP.get(distanceToCorner) + pivot.getBias()); if (flywheel.allMotorsOnTarget() && pivot.onTarget() && inTolerance()) { indexer.indexUp(); @@ -99,12 +108,7 @@ public void execute() { new TimedCommand(RobotContainer.hapticDriverCommand(), 1d).schedule(); } - if (!DriverStation.isFMSAttached()) { - LightningShuffleboard.setBool("Note-Pass", "In tloeracnce", inTolerance()); - LightningShuffleboard.setDouble("Note-Pass", "CurrentHeading", currentHeading); - LightningShuffleboard.setDouble("Note-Pass", "TargetHeading", targetHeading); - LightningShuffleboard.setDouble("Note-Pass", "Distance to Corner", drivetrain.distanceToCorner()); - } + updateLogging(); } @Override @@ -127,6 +131,41 @@ private boolean isBlueAlliance() { private boolean inTolerance() { double difference = Math.abs(currentHeading - targetHeading); difference = difference > 180 ? 360 - difference : difference; - return difference <= VisionConstants.POINTATPOINT_ALIGNMENT_TOLERANCE; // TODO not has Target but if the correct filter is set + return difference <= PassConstants.POINT_TOLERANCE; + } + + /** + * initialize logging + */ + private void initLogging() { + DataLog log = DataLogManager.getLog(); + + currentHeadingLog = new DoubleLogEntry(log, "/NotePass/CurrentHeading"); + targetHeadingLog = new DoubleLogEntry(log, "/NotePass/TargetHeading"); + pidOutputLog = new DoubleLogEntry(log, "/NotePass/PIDoutput"); + distanceToCornerLog = new DoubleLogEntry(log, "/NotePass/DistanceTOCorner"); + + headingOnTargetLog = new BooleanLogEntry(log, "/NotePass/Heading-OnTarget"); + shooterOnTargetLog = new BooleanLogEntry(log, "/NotePass/Shooter-OnTarget"); + + if (!DriverStation.isFMSAttached()) { + LightningShuffleboard.setBoolSupplier("Note-Pass", "In tloeracnce", () -> inTolerance()); + LightningShuffleboard.setDoubleSupplier("Note-Pass", "CurrentHeading", () -> currentHeading); + LightningShuffleboard.setDoubleSupplier("Note-Pass", "TargetHeading", () -> targetHeading); + LightningShuffleboard.setDoubleSupplier("Note-Pass", "Distance to Corner", () -> distanceToCorner); + } } + + /** + * update logging + */ + public void updateLogging() { + currentHeadingLog.append(currentHeading); + targetHeadingLog.append(targetHeading); + pidOutputLog.append(feedForwardOutput); + distanceToCornerLog.append(distanceToCorner); + + headingOnTargetLog.append(inTolerance()); + shooterOnTargetLog.append(flywheel.allMotorsOnTarget() && pivot.onTarget()); + } } diff --git a/src/main/java/frc/robot/command/shoot/SmartShoot.java b/src/main/java/frc/robot/command/shoot/SmartShoot.java index 0e1f2dad..91ebcc36 100644 --- a/src/main/java/frc/robot/command/shoot/SmartShoot.java +++ b/src/main/java/frc/robot/command/shoot/SmartShoot.java @@ -9,7 +9,6 @@ import edu.wpi.first.util.datalog.StringLogEntry; import frc.robot.Constants; import frc.robot.RobotContainer; -import frc.robot.Constants.CandConstants; import frc.robot.Constants.LEDsConstants.LED_STATES; import frc.robot.Constants.ShooterConstants.ShootingState; import frc.robot.Constants.ShooterConstants; From 65d49d7e182282f63fc3bcf59c99b351765f5aa3 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 18 Apr 2024 10:53:49 -0400 Subject: [PATCH 02/18] COMP Worlds - Q9 --- .../frc/robot/command/shoot/NotePass.java | 26 ++++++++++++------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/command/shoot/NotePass.java b/src/main/java/frc/robot/command/shoot/NotePass.java index 6ce94098..cd3f5327 100644 --- a/src/main/java/frc/robot/command/shoot/NotePass.java +++ b/src/main/java/frc/robot/command/shoot/NotePass.java @@ -1,5 +1,7 @@ package frc.robot.command.shoot; +import java.sql.Driver; +import java.util.function.BooleanSupplier; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; @@ -28,6 +30,7 @@ public class NotePass extends Command { private final Swerve drivetrain; private final Flywheel flywheel; private final Pivot pivot; + private Translation2d targetPose; private double currentHeading; private double targetHeading; @@ -39,7 +42,6 @@ public class NotePass extends Command { private XboxControllerFilter driver; private Indexer indexer; - private DoubleLogEntry currentHeadingLog; private DoubleLogEntry targetHeadingLog; private DoubleLogEntry pidOutputLog; @@ -63,7 +65,11 @@ public NotePass(Swerve drivetrain, Flywheel flywheel, Pivot pivot, XboxControlle this.driver = driver; this.indexer = indexer; - addRequirements(flywheel, pivot, drivetrain, indexer); + if (DriverStation.isAutonomous()) { + addRequirements(flywheel, pivot, drivetrain, indexer); + } else { + addRequirements(flywheel, pivot, drivetrain); + } } @Override @@ -103,7 +109,8 @@ public void execute() { pivot.setTargetAngle(ShooterConstants.NOTEPASS_ANGLE_MAP.get(distanceToCorner) + pivot.getBias()); if (flywheel.allMotorsOnTarget() && pivot.onTarget() && inTolerance()) { - indexer.indexUp(); + if (DriverStation.isAutonomous()) + indexer.indexUp(); new TimedCommand(RobotContainer.hapticCopilotCommand(), 1d).schedule(); new TimedCommand(RobotContainer.hapticDriverCommand(), 1d).schedule(); } @@ -115,7 +122,8 @@ public void execute() { public void end(boolean interrupted) { flywheel.coast(true); pivot.setTargetAngle(pivot.getStowAngle()); - indexer.stop(); + if (DriverStation.isAutonomous()) + indexer.stop(); } @Override @@ -131,7 +139,7 @@ private boolean isBlueAlliance() { private boolean inTolerance() { double difference = Math.abs(currentHeading - targetHeading); difference = difference > 180 ? 360 - difference : difference; - return difference <= PassConstants.POINT_TOLERANCE; + return difference <= PassConstants.POINT_TOLERANCE; } /** @@ -157,9 +165,9 @@ private void initLogging() { } /** - * update logging - */ - public void updateLogging() { + * update logging + */ + public void updateLogging() { currentHeadingLog.append(currentHeading); targetHeadingLog.append(targetHeading); pidOutputLog.append(feedForwardOutput); @@ -167,5 +175,5 @@ public void updateLogging() { headingOnTargetLog.append(inTolerance()); shooterOnTargetLog.append(flywheel.allMotorsOnTarget() && pivot.onTarget()); - } + } } From d43d07dedd10a7456e42f8e0707ce167dcca3c17 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 18 Apr 2024 13:22:07 -0400 Subject: [PATCH 03/18] COMP Worlds - Q17 --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/command/SmartCollect.java | 3 ++- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7812dda6..2557dc63 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -373,7 +373,7 @@ public class MusicConstants { public class CollectorConstants { public static final boolean COLLECTOR_MOTOR_INVERTED = true; - public static final int COLLECTOR_MOTOR_STATOR_CURRENT_LIMIT = 70; + public static final int COLLECTOR_MOTOR_STATOR_CURRENT_LIMIT = 90; public static final boolean COLLECTOR_MOTOR_BRAKE_MODE = false; public static final double MOTOR_KP = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 54bcaaa4..f16b97e7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -258,7 +258,7 @@ protected void configureButtonBindings() { /* COPILOT */ new Trigger(() -> (coPilot.getBButton() && !driver.getRightBumper())) .whileTrue(new InstantCommand(() -> flywheel.stop(), flywheel) - .andThen(new SmartCollect(() -> 0.65, () -> 0.9, collector, indexer, pivot, flywheel)) + .andThen(new SmartCollect(() -> 1d, () -> .9, collector, indexer, pivot, flywheel)) .deadlineWith(leds.enableState(LED_STATES.COLLECTING))); // new Trigger(coPilot::getBButton) diff --git a/src/main/java/frc/robot/command/SmartCollect.java b/src/main/java/frc/robot/command/SmartCollect.java index 89036edf..6baf33da 100644 --- a/src/main/java/frc/robot/command/SmartCollect.java +++ b/src/main/java/frc/robot/command/SmartCollect.java @@ -100,9 +100,10 @@ public void execute() { indexer.setPower(0.9 * indexerPower.getAsDouble()); } else if (reversedFromExit) { indexer.stop(); - if(!DriverStation.isAutonomous()){ + if (!DriverStation.isAutonomous()){ flywheel.coast(true); } + new TimedCommand(RobotContainer.hapticDriverCommand(), 1d).schedule(); new TimedCommand(RobotContainer.hapticCopilotCommand(), 1d).schedule(); } break; From b3ee713d1e4e4b7b01dc458d402664872a1cb9b2 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 18 Apr 2024 14:12:38 -0400 Subject: [PATCH 04/18] COMP Worlds - Q27 --- src/main/java/frc/robot/Constants.java | 4 +++- src/main/java/frc/robot/RobotContainer.java | 4 +--- src/main/java/frc/robot/subsystems/Indexer.java | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2557dc63..d3e1de01 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -376,6 +376,8 @@ public class CollectorConstants { public static final int COLLECTOR_MOTOR_STATOR_CURRENT_LIMIT = 90; public static final boolean COLLECTOR_MOTOR_BRAKE_MODE = false; + public static final double COLLECTOR_DEBOUNCE_TIME = 0.5d; + public static final double MOTOR_KP = 0; public static final double MOTOR_KI = 0; public static final double MOTOR_KD = 0; @@ -447,7 +449,7 @@ public enum PieceState { public static final double INDEXER_DEFAULT_POWER = 1d; public static final double INDEXER_MANUAL_POWER = 1d; - public static final double INDEXER_DEBOUNCE_TIME = 0.1d; + public static final double INDEXER_DEBOUNCE_TIME = 0.5d; public static final double INDEXER_SYSTEST_POWER = 0.25d; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f16b97e7..b7e3f5fc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -413,9 +413,7 @@ protected void configureFaultMonitors() { @Override protected void configureSystemTests() { SystemTest.registerTest("Drive Test", - new DrivetrainSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); // to - // be - // tested + new DrivetrainSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); // to be tested SystemTest.registerTest("Azimuth Test", new TurnSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_TURN)); diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index cabcba2f..168b5e2e 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -37,8 +37,8 @@ public class Indexer extends SubsystemBase { private PieceState currentState = PieceState.NONE; private boolean didShoot = false; - private Debouncer entryDebouncer = new Debouncer(0.05); - private Debouncer exitDebouncer = new Debouncer(0.05); + private Debouncer entryDebouncer = new Debouncer(IndexerConstants.INDEXER_DEBOUNCE_TIME); + private Debouncer exitDebouncer = new Debouncer(IndexerConstants.INDEXER_DEBOUNCE_TIME); private DoubleLogEntry indexerPowerLog; private DoubleLogEntry indexerTargetPowerLog; From e616d6deaf3c86e581c53c10bc51cdb6244bff2c Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 18 Apr 2024 16:15:34 -0400 Subject: [PATCH 05/18] COMP Worlds - Q46 --- .../pathplanner/autos/2-SB-[F4]-EF.auto | 2 +- .../pathplanner/autos/3-SB-[F4-F5]-EC.auto | 120 ++++++++ ...F-[C2-F3]-EC.auto => 3-SB-[F4-F5]-EF.auto} | 0 .../pathplanner/autos/3-SB-[F5-F4]-EF.auto | 278 ------------------ src/main/deploy/pathplanner/paths/BS-F4.path | 2 +- .../deploy/pathplanner/paths/BS-F5-Close.path | 52 ++++ src/main/deploy/pathplanner/paths/F3-BS.path | 2 +- .../paths/{F4-BS.path => F4-BS-Close.path} | 12 +- .../pathplanner/paths/Linked Point.path | 2 +- .../java/frc/robot/subsystems/Indexer.java | 2 +- 10 files changed, 183 insertions(+), 289 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/3-SB-[F4-F5]-EC.auto rename src/main/deploy/pathplanner/autos/{3-SF-[C2-F3]-EC.auto => 3-SB-[F4-F5]-EF.auto} (100%) delete mode 100644 src/main/deploy/pathplanner/autos/3-SB-[F5-F4]-EF.auto create mode 100644 src/main/deploy/pathplanner/paths/BS-F5-Close.path rename src/main/deploy/pathplanner/paths/{F4-BS.path => F4-BS-Close.path} (81%) diff --git a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto index fc80a61b..fba1dbe7 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto +++ b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto @@ -43,7 +43,7 @@ { "type": "path", "data": { - "pathName": "F4-BS" + "pathName": null } } ] diff --git a/src/main/deploy/pathplanner/autos/3-SB-[F4-F5]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[F4-F5]-EC.auto new file mode 100644 index 00000000..65a69831 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3-SB-[F4-F5]-EC.auto @@ -0,0 +1,120 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Cand-Sub" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SB-F4" + } + }, + { + "type": "named", + "data": { + "name": "Chase-Pieces" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "F4-BS-Close" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Point-At-Speaker-left" + } + }, + { + "type": "named", + "data": { + "name": "preAim" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.5 + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Smart-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stop-Drive" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "BS-F5-Close" + } + }, + { + "type": "named", + "data": { + "name": "Chase-Pieces" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[F4-F5]-EF.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto rename to src/main/deploy/pathplanner/autos/3-SB-[F4-F5]-EF.auto diff --git a/src/main/deploy/pathplanner/autos/3-SB-[F5-F4]-EF.auto b/src/main/deploy/pathplanner/autos/3-SB-[F5-F4]-EF.auto deleted file mode 100644 index 321bf45b..00000000 --- a/src/main/deploy/pathplanner/autos/3-SB-[F5-F4]-EF.auto +++ /dev/null @@ -1,278 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Cand-Sub" - } - }, - { - "type": "wait", - "data": { - "waitTime": 4.0 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "SB-F5" - } - }, - { - "type": "named", - "data": { - "name": "Chase-Pieces" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "F5-BS" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Stop-Flywheel" - } - }, - { - "type": "path", - "data": { - "pathName": "BS-F4" - } - }, - { - "type": "named", - "data": { - "name": "Chase-Pieces" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "F4-BS" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Has-Piece" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Stop-Flywheel" - } - }, - { - "type": "path", - "data": { - "pathName": "BS-F4" - } - }, - { - "type": "deadline", - "data": { - "commands": [] - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BS-F4.path b/src/main/deploy/pathplanner/paths/BS-F4.path index cb654f20..4b922158 100644 --- a/src/main/deploy/pathplanner/paths/BS-F4.path +++ b/src/main/deploy/pathplanner/paths/BS-F4.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 3.3263053363443364, + "x": 3.326305336344337, "y": 2.1507848092099264 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BS-F5-Close.path b/src/main/deploy/pathplanner/paths/BS-F5-Close.path new file mode 100644 index 00000000..34f24757 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BS-F5-Close.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4311279977766789, + "y": 3.688913511921175 + }, + "prevControl": null, + "nextControl": { + "x": 2.239175719435603, + "y": 2.4007214918852084 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.49, + "y": 0.77 + }, + "prevControl": { + "x": 5.1432537972351255, + "y": 1.030553616028773 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "F5" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 1.5, + "rotation": 0, + "rotateFast": true + }, + "reversed": false, + "folder": "Bottom side paths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F3-BS.path b/src/main/deploy/pathplanner/paths/F3-BS.path index f85e927b..a7993333 100644 --- a/src/main/deploy/pathplanner/paths/F3-BS.path +++ b/src/main/deploy/pathplanner/paths/F3-BS.path @@ -36,7 +36,7 @@ "y": 4.141627021992783 }, "prevControl": { - "x": 3.3263053363443347, + "x": 3.326305336344335, "y": 4.1182053489012205 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/F4-BS.path b/src/main/deploy/pathplanner/paths/F4-BS-Close.path similarity index 81% rename from src/main/deploy/pathplanner/paths/F4-BS.path rename to src/main/deploy/pathplanner/paths/F4-BS-Close.path index e70e96d2..2f6d2b69 100644 --- a/src/main/deploy/pathplanner/paths/F4-BS.path +++ b/src/main/deploy/pathplanner/paths/F4-BS-Close.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 2.520051166228458, - "y": 3.3465825490209546 + "x": 1.2554654495899562, + "y": 4.42669621430541 }, "prevControl": { - "x": 3.684883766163928, - "y": 2.200239355433667 + "x": 2.4733924503512332, + "y": 1.979131376237074 }, "nextControl": null, "isLocked": false, - "linkedName": "BS" + "linkedName": null } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -38.796552098308226, + "rotation": -45.806929455102335, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Linked Point.path b/src/main/deploy/pathplanner/paths/Linked Point.path index 7085de7b..8de89a46 100644 --- a/src/main/deploy/pathplanner/paths/Linked Point.path +++ b/src/main/deploy/pathplanner/paths/Linked Point.path @@ -212,7 +212,7 @@ "y": 4.141627021992783 }, "prevControl": { - "x": 2.218544638798982, + "x": 2.2185446387989822, "y": 4.079045632486238 }, "nextControl": { diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 168b5e2e..412effdb 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -38,7 +38,7 @@ public class Indexer extends SubsystemBase { private boolean didShoot = false; private Debouncer entryDebouncer = new Debouncer(IndexerConstants.INDEXER_DEBOUNCE_TIME); - private Debouncer exitDebouncer = new Debouncer(IndexerConstants.INDEXER_DEBOUNCE_TIME); + private Debouncer exitDebouncer = new Debouncer(0.1); private DoubleLogEntry indexerPowerLog; private DoubleLogEntry indexerTargetPowerLog; From 4e8054acb8ce905764e97b8464a23ca9c57637c5 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 18 Apr 2024 16:23:23 -0400 Subject: [PATCH 06/18] COMP Worlds - Q46 pt2 --- .../pathplanner/autos/2-SB-[F4]-EF.auto | 2 +- .../pathplanner/autos/3-SB-[F4-F5]-EF.auto | 164 ------------------ ...B-[F4-F5]-EC.auto => 3-SF-[C2-F3]-EC.auto} | 0 3 files changed, 1 insertion(+), 165 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/3-SB-[F4-F5]-EF.auto rename src/main/deploy/pathplanner/autos/{3-SB-[F4-F5]-EC.auto => 3-SF-[C2-F3]-EC.auto} (100%) diff --git a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto index fba1dbe7..a5b1502c 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto +++ b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto @@ -43,7 +43,7 @@ { "type": "path", "data": { - "pathName": null + "pathName": "F4-BS-Close" } } ] diff --git a/src/main/deploy/pathplanner/autos/3-SB-[F4-F5]-EF.auto b/src/main/deploy/pathplanner/autos/3-SB-[F4-F5]-EF.auto deleted file mode 100644 index 6987aad8..00000000 --- a/src/main/deploy/pathplanner/autos/3-SB-[F4-F5]-EF.auto +++ /dev/null @@ -1,164 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Cand-Sub" - } - }, - { - "type": "wait", - "data": { - "waitTime": 4.0 - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "SF-C2" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - }, - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "C2-F3" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Flywheel" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Chase-Pieces" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "F3-TS" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - }, - { - "type": "named", - "data": { - "name": "Smart-Collect" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SB-[F4-F5]-EC.auto b/src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/3-SB-[F4-F5]-EC.auto rename to src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto From 5e296e25ee9b3508fbeada7c189d2da80d0554c9 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 18 Apr 2024 18:00:12 -0400 Subject: [PATCH 07/18] COMP Worlds - Start auto worl --- .pathplanner/settings.json | 5 +- .../pathplanner/autos/2-SB-[C3]-EC.auto | 2 +- .../{2-SB-[F4]-EF.auto => 2-SB-[F4]-EC.auto} | 14 +++--- .../pathplanner/autos/2-SF-[C2]-EC.auto | 2 +- .../pathplanner/autos/2-ST-[C1]-EC.auto | 2 +- .../pathplanner/autos/3-SF-[C2-C3]-EC.auto | 2 +- .../deploy/pathplanner/autos/Point-Blank.auto | 2 +- .../deploy/pathplanner/autos/Smart-Shoot.auto | 2 +- .../deploy/pathplanner/autos/nuh-uh-V2.auto | 2 +- src/main/deploy/pathplanner/autos/nuh-uh.auto | 2 +- .../deploy/pathplanner/autos/womp-womp.auto | 2 +- src/main/deploy/pathplanner/paths/F4-BS.path | 49 +++++++++++++++++++ 12 files changed, 69 insertions(+), 17 deletions(-) rename src/main/deploy/pathplanner/autos/{2-SB-[F4]-EF.auto => 2-SB-[F4]-EC.auto} (92%) create mode 100644 src/main/deploy/pathplanner/paths/F4-BS.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index cb48e7ab..e9300c80 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -9,7 +9,10 @@ "Silly", "Top side paths" ], - "autoFolders": [], + "autoFolders": [ + "LOL", + "CAND" + ], "defaultMaxVel": 5.0, "defaultMaxAccel": 4.0, "defaultMaxAngVel": 540.0, diff --git a/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto b/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto index 8ff90a92..bb166786 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-SB-[C3]-EC.auto @@ -121,6 +121,6 @@ ] } }, - "folder": null, + "folder": "CAND", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto similarity index 92% rename from src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto rename to src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto index a5b1502c..07bbe82f 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EF.auto +++ b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto @@ -45,6 +45,12 @@ "data": { "pathName": "F4-BS-Close" } + }, + { + "type": "named", + "data": { + "name": "preAim" + } } ] } @@ -60,7 +66,7 @@ { "type": "named", "data": { - "name": "Point-At-Speaker-left" + "name": "Point-At-Speaker" } }, { @@ -99,12 +105,6 @@ } ] } - }, - { - "type": "path", - "data": { - "pathName": "BS-F4" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto b/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto index e52a987d..166a1854 100644 --- a/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-SF-[C2]-EC.auto @@ -102,6 +102,6 @@ ] } }, - "folder": null, + "folder": "CAND", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto b/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto index 5f925bfe..762bc6ff 100644 --- a/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-ST-[C1]-EC.auto @@ -108,6 +108,6 @@ ] } }, - "folder": null, + "folder": "CAND", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto index 70284020..6c75bd14 100644 --- a/src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto +++ b/src/main/deploy/pathplanner/autos/3-SF-[C2-C3]-EC.auto @@ -140,6 +140,6 @@ ] } }, - "folder": null, + "folder": "CAND", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Point-Blank.auto b/src/main/deploy/pathplanner/autos/Point-Blank.auto index 3a8351e9..28e57214 100644 --- a/src/main/deploy/pathplanner/autos/Point-Blank.auto +++ b/src/main/deploy/pathplanner/autos/Point-Blank.auto @@ -14,6 +14,6 @@ ] } }, - "folder": null, + "folder": "CAND", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Smart-Shoot.auto b/src/main/deploy/pathplanner/autos/Smart-Shoot.auto index b94aaf61..c8076f1d 100644 --- a/src/main/deploy/pathplanner/autos/Smart-Shoot.auto +++ b/src/main/deploy/pathplanner/autos/Smart-Shoot.auto @@ -33,6 +33,6 @@ ] } }, - "folder": null, + "folder": "CAND", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/nuh-uh-V2.auto b/src/main/deploy/pathplanner/autos/nuh-uh-V2.auto index e05bab02..4c9bf165 100644 --- a/src/main/deploy/pathplanner/autos/nuh-uh-V2.auto +++ b/src/main/deploy/pathplanner/autos/nuh-uh-V2.auto @@ -228,6 +228,6 @@ ] } }, - "folder": null, + "folder": "LOL", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/nuh-uh.auto b/src/main/deploy/pathplanner/autos/nuh-uh.auto index 8bb33787..fbf2eeda 100644 --- a/src/main/deploy/pathplanner/autos/nuh-uh.auto +++ b/src/main/deploy/pathplanner/autos/nuh-uh.auto @@ -39,6 +39,6 @@ ] } }, - "folder": null, + "folder": "LOL", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/womp-womp.auto b/src/main/deploy/pathplanner/autos/womp-womp.auto index cb9b6edd..c80e9ad8 100644 --- a/src/main/deploy/pathplanner/autos/womp-womp.auto +++ b/src/main/deploy/pathplanner/autos/womp-womp.auto @@ -51,6 +51,6 @@ ] } }, - "folder": null, + "folder": "LOL", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F4-BS.path b/src/main/deploy/pathplanner/paths/F4-BS.path new file mode 100644 index 00000000..27d6d1d2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F4-BS.path @@ -0,0 +1,49 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.279501243687172, + "y": 2.4313569347859425 + }, + "prevControl": null, + "nextControl": { + "x": 5.132568726675916, + "y": 0.6808190539458011 + }, + "isLocked": false, + "linkedName": "F4-R" + }, + { + "anchor": { + "x": 2.22182059748735, + "y": 3.3646274984857665 + }, + "prevControl": { + "x": 2.7151677380277848, + "y": 2.0819249330806353 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "BS" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -50.63068275763522, + "rotateFast": false + }, + "reversed": false, + "folder": "Return Paths", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file From ad7a45070fbc086f05c9993458f917d7081f182d Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 18 Apr 2024 18:03:36 -0400 Subject: [PATCH 08/18] COMP Worlds - Going on Practice Field --- .../pathplanner/autos/3-SB-[F4-C3]-EC.auto | 2 +- .../pathplanner/autos/3-SF-[C2-F3]-EC.auto | 120 ------------------ .../deploy/pathplanner/paths/F4-BS-Close.path | 12 +- src/main/deploy/pathplanner/paths/SB-F4.path | 4 +- 4 files changed, 9 insertions(+), 129 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto diff --git a/src/main/deploy/pathplanner/autos/3-SB-[F4-C3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SB-[F4-C3]-EC.auto index 4f26199f..7b8fd319 100644 --- a/src/main/deploy/pathplanner/autos/3-SB-[F4-C3]-EC.auto +++ b/src/main/deploy/pathplanner/autos/3-SB-[F4-C3]-EC.auto @@ -185,6 +185,6 @@ ] } }, - "folder": null, + "folder": "LOL", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto b/src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto deleted file mode 100644 index 65a69831..00000000 --- a/src/main/deploy/pathplanner/autos/3-SF-[C2-F3]-EC.auto +++ /dev/null @@ -1,120 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Cand-Sub" - } - }, - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "SB-F4" - } - }, - { - "type": "named", - "data": { - "name": "Chase-Pieces" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "F4-BS-Close" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker-left" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.5 - } - } - ] - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Smart-Shoot" - } - }, - { - "type": "named", - "data": { - "name": "Stop-Drive" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "BS-F5-Close" - } - }, - { - "type": "named", - "data": { - "name": "Chase-Pieces" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F4-BS-Close.path b/src/main/deploy/pathplanner/paths/F4-BS-Close.path index 2f6d2b69..c9bcaae9 100644 --- a/src/main/deploy/pathplanner/paths/F4-BS-Close.path +++ b/src/main/deploy/pathplanner/paths/F4-BS-Close.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.2554654495899562, - "y": 4.42669621430541 + "x": 1.580469314784785, + "y": 4.153982923350462 }, "prevControl": { - "x": 2.4733924503512332, - "y": 1.979131376237074 + "x": 1.8567437134874287, + "y": 2.269396846486001 }, "nextControl": null, "isLocked": false, @@ -39,8 +39,8 @@ }, "goalEndState": { "velocity": 0, - "rotation": -45.806929455102335, - "rotateFast": false + "rotation": -47.41950921665628, + "rotateFast": true }, "reversed": false, "folder": "Return Paths", diff --git a/src/main/deploy/pathplanner/paths/SB-F4.path b/src/main/deploy/pathplanner/paths/SB-F4.path index 3b35cc08..e996a690 100644 --- a/src/main/deploy/pathplanner/paths/SB-F4.path +++ b/src/main/deploy/pathplanner/paths/SB-F4.path @@ -39,8 +39,8 @@ }, "goalEndState": { "velocity": 1.0, - "rotation": 1.301952672578864, - "rotateFast": false + "rotation": 0.0, + "rotateFast": true }, "reversed": false, "folder": "Bottom side paths", From d854561f3d13bc9b63f2400ea55e3dbdaa54e14d Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 18 Apr 2024 19:27:08 -0400 Subject: [PATCH 09/18] COMP Worlds - End of day --- .../pathplanner/autos/2-SB-[F4]-EC.auto | 46 +----- .../pathplanner/autos/3-SB-[F4-F3]-EF.auto | 138 ++++++++++++++++++ src/main/deploy/pathplanner/paths/BB-F3.path | 8 +- src/main/deploy/pathplanner/paths/BF-F3.path | 8 +- src/main/deploy/pathplanner/paths/BS-F3.path | 8 +- src/main/deploy/pathplanner/paths/C1-F2.path | 2 +- src/main/deploy/pathplanner/paths/C1-F3.path | 8 +- src/main/deploy/pathplanner/paths/C2-F3.path | 8 +- src/main/deploy/pathplanner/paths/C3-F3.path | 8 +- .../deploy/pathplanner/paths/F3-BS-Close.path | 71 +++++++++ .../deploy/pathplanner/paths/F4-BS-Close.path | 10 +- .../pathplanner/paths/Linked Point.path | 16 +- src/main/deploy/pathplanner/paths/SB-F3.path | 8 +- src/main/deploy/pathplanner/paths/SF-F3.path | 8 +- 14 files changed, 262 insertions(+), 85 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/3-SB-[F4-F3]-EF.auto create mode 100644 src/main/deploy/pathplanner/paths/F3-BS-Close.path diff --git a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto index 07bbe82f..afb259b9 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto @@ -45,44 +45,6 @@ "data": { "pathName": "F4-BS-Close" } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 3.0 - } } ] } @@ -94,7 +56,7 @@ { "type": "named", "data": { - "name": "Smart-Shoot" + "name": "Cand-Sub" } }, { @@ -105,6 +67,12 @@ } ] } + }, + { + "type": "path", + "data": { + "pathName": "BS-F3" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/3-SB-[F4-F3]-EF.auto b/src/main/deploy/pathplanner/autos/3-SB-[F4-F3]-EF.auto new file mode 100644 index 00000000..527df8b6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3-SB-[F4-F3]-EF.auto @@ -0,0 +1,138 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Cand-Sub" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SB-F4" + } + }, + { + "type": "named", + "data": { + "name": "Chase-Pieces" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "F4-BS-Close" + } + }, + { + "type": "named", + "data": { + "name": "preAim" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Point-At-Speaker" + } + }, + { + "type": "named", + "data": { + "name": "preAim" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Smart-Shoot" + } + }, + { + "type": "named", + "data": { + "name": "Stop-Drive" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SB-F3" + } + }, + { + "type": "named", + "data": { + "name": "Chase-Pieces" + } + }, + { + "type": "path", + "data": { + "pathName": "F3-BS-Close" + } + }, + { + "type": "named", + "data": { + "name": "Cand-Sub" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BB-F3.path b/src/main/deploy/pathplanner/paths/BB-F3.path index 223ffdd1..2f7a936c 100644 --- a/src/main/deploy/pathplanner/paths/BB-F3.path +++ b/src/main/deploy/pathplanner/paths/BB-F3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.3, - "y": 4.084914323674802 + "x": 8.071172319234796, + "y": 4.098792791023527 }, "prevControl": { - "x": 5.128815128898846, - "y": 0.630749897446187 + "x": 5.899987448133642, + "y": 0.6446283647949121 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BF-F3.path b/src/main/deploy/pathplanner/paths/BF-F3.path index 125b1d13..c42749df 100644 --- a/src/main/deploy/pathplanner/paths/BF-F3.path +++ b/src/main/deploy/pathplanner/paths/BF-F3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.3, - "y": 4.084914323674802 + "x": 8.071172319234796, + "y": 4.098792791023527 }, "prevControl": { - "x": 5.128815128898846, - "y": 0.630749897446187 + "x": 5.899987448133642, + "y": 0.6446283647949121 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/BS-F3.path b/src/main/deploy/pathplanner/paths/BS-F3.path index 5904a59b..9fea166b 100644 --- a/src/main/deploy/pathplanner/paths/BS-F3.path +++ b/src/main/deploy/pathplanner/paths/BS-F3.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 7.3, - "y": 4.084914323674802 + "x": 8.071172319234796, + "y": 4.098792791023527 }, "prevControl": { - "x": 6.595606914466325, - "y": 4.098792791023527 + "x": 7.366779233701121, + "y": 4.112671258372252 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C1-F2.path b/src/main/deploy/pathplanner/paths/C1-F2.path index f12a6eef..63f12ccb 100644 --- a/src/main/deploy/pathplanner/paths/C1-F2.path +++ b/src/main/deploy/pathplanner/paths/C1-F2.path @@ -21,7 +21,7 @@ }, "prevControl": { "x": 5.934873634259503, - "y": 6.4085300912838195 + "y": 6.408530091283819 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C1-F3.path b/src/main/deploy/pathplanner/paths/C1-F3.path index 9af4e553..335beb22 100644 --- a/src/main/deploy/pathplanner/paths/C1-F3.path +++ b/src/main/deploy/pathplanner/paths/C1-F3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.3, - "y": 4.084914323674802 + "x": 8.071172319234796, + "y": 4.098792791023527 }, "prevControl": { - "x": 7.3, - "y": 6.356584333442144 + "x": 8.071172319234796, + "y": 6.370462800790869 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C2-F3.path b/src/main/deploy/pathplanner/paths/C2-F3.path index 19e73e8b..dbee4161 100644 --- a/src/main/deploy/pathplanner/paths/C2-F3.path +++ b/src/main/deploy/pathplanner/paths/C2-F3.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 7.3, - "y": 4.084914323674802 + "x": 8.071172319234796, + "y": 4.098792791023527 }, "prevControl": { - "x": 6.2677034911844425, - "y": 4.075371117931965 + "x": 7.038875810419238, + "y": 4.08924958528069 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C3-F3.path b/src/main/deploy/pathplanner/paths/C3-F3.path index 05cb8d0a..a7e772f9 100644 --- a/src/main/deploy/pathplanner/paths/C3-F3.path +++ b/src/main/deploy/pathplanner/paths/C3-F3.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 7.3, - "y": 4.084914323674802 + "x": 8.071172319234796, + "y": 4.098792791023527 }, "prevControl": { - "x": 6.698806189250695, - "y": 4.051949444840401 + "x": 7.469978508485491, + "y": 4.0658279121891265 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/F3-BS-Close.path b/src/main/deploy/pathplanner/paths/F3-BS-Close.path new file mode 100644 index 00000000..3e556b7c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F3-BS-Close.path @@ -0,0 +1,71 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.071172319234796, + "y": 4.098792791023527 + }, + "prevControl": null, + "nextControl": { + "x": 6.314546837367568, + "y": 4.098792791023527 + }, + "isLocked": false, + "linkedName": "F3" + }, + { + "anchor": { + "x": 5.073198163514728, + "y": 3.946551915928368 + }, + "prevControl": { + "x": 5.588474971529114, + "y": 4.321298685393376 + }, + "nextControl": { + "x": 3.678389922484949, + "y": 2.9321459224521655 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.71, + "y": 4.22 + }, + "prevControl": { + "x": 2.9886692583656194, + "y": 2.4007214918852084 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.3, + "rotationDegrees": 0, + "rotateFast": true + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.23000000000002, + "rotateFast": true + }, + "reversed": false, + "folder": "Return Paths", + "previewStartingState": null, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F4-BS-Close.path b/src/main/deploy/pathplanner/paths/F4-BS-Close.path index c9bcaae9..b6b6840f 100644 --- a/src/main/deploy/pathplanner/paths/F4-BS-Close.path +++ b/src/main/deploy/pathplanner/paths/F4-BS-Close.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.580469314784785, - "y": 4.153982923350462 + "x": 0.71, + "y": 4.22 }, "prevControl": { - "x": 1.8567437134874287, - "y": 2.269396846486001 + "x": 2.537562767433542, + "y": 2.7035423301569765 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -47.41950921665628, + "rotation": -59.23000000000002, "rotateFast": true }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Linked Point.path b/src/main/deploy/pathplanner/paths/Linked Point.path index 8de89a46..d338f9fd 100644 --- a/src/main/deploy/pathplanner/paths/Linked Point.path +++ b/src/main/deploy/pathplanner/paths/Linked Point.path @@ -149,27 +149,27 @@ }, "prevControl": { "x": 7.118683523378979, - "y": 5.90254759691669 + "y": 5.9025475969166905 }, "nextControl": { "x": 7.126505594674005, - "y": 5.855132295047945 + "y": 5.8551322950479445 }, "isLocked": false, "linkedName": "F2" }, { "anchor": { - "x": 7.3, - "y": 4.084914323674802 + "x": 8.071172319234796, + "y": 4.098792791023527 }, "prevControl": { - "x": 7.301955517823756, - "y": 4.073060498207616 + "x": 8.073127837058552, + "y": 4.0869389655563415 }, "nextControl": { - "x": 7.298044482176244, - "y": 4.096768149141988 + "x": 8.06921680141104, + "y": 4.110646616490713 }, "isLocked": false, "linkedName": "F3" diff --git a/src/main/deploy/pathplanner/paths/SB-F3.path b/src/main/deploy/pathplanner/paths/SB-F3.path index 87f19370..9b407e01 100644 --- a/src/main/deploy/pathplanner/paths/SB-F3.path +++ b/src/main/deploy/pathplanner/paths/SB-F3.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 7.3, - "y": 4.084914323674802 + "x": 8.071172319234796, + "y": 4.098792791023527 }, "prevControl": { - "x": 6.698806189250695, - "y": 4.051949444840401 + "x": 7.469978508485491, + "y": 4.0658279121891265 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/SF-F3.path b/src/main/deploy/pathplanner/paths/SF-F3.path index e41582de..d86c1859 100644 --- a/src/main/deploy/pathplanner/paths/SF-F3.path +++ b/src/main/deploy/pathplanner/paths/SF-F3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.3, - "y": 4.084914323674802 + "x": 8.071172319234796, + "y": 4.098792791023527 }, "prevControl": { - "x": 5.875878418423503, - "y": 3.6701471601141407 + "x": 6.647050737658299, + "y": 3.6840256274628658 }, "nextControl": null, "isLocked": false, From 81abf5457830a13a0da08822434e917640d68542 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Thu, 18 Apr 2024 19:48:52 -0400 Subject: [PATCH 10/18] COMP Worlds - got to look over my work --- .../pathplanner/autos/2-SB-[F4]-EC.auto | 6 ---- .../pathplanner/autos/3-SB-[F4-F3]-EF.auto | 34 +------------------ 2 files changed, 1 insertion(+), 39 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto index afb259b9..1c8b0e82 100644 --- a/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto +++ b/src/main/deploy/pathplanner/autos/2-SB-[F4]-EC.auto @@ -67,12 +67,6 @@ } ] } - }, - { - "type": "path", - "data": { - "pathName": "BS-F3" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/3-SB-[F4-F3]-EF.auto b/src/main/deploy/pathplanner/autos/3-SB-[F4-F3]-EF.auto index 527df8b6..d0e6659c 100644 --- a/src/main/deploy/pathplanner/autos/3-SB-[F4-F3]-EF.auto +++ b/src/main/deploy/pathplanner/autos/3-SB-[F4-F3]-EF.auto @@ -55,38 +55,6 @@ ] } }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Point-At-Speaker" - } - }, - { - "type": "named", - "data": { - "name": "preAim" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - } - ] - } - }, { "type": "deadline", "data": { @@ -94,7 +62,7 @@ { "type": "named", "data": { - "name": "Smart-Shoot" + "name": "Cand-Sub" } }, { From b51beebdb3aa9c9d412a5907ac715cceac2bcea8 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Fri, 19 Apr 2024 09:39:14 -0400 Subject: [PATCH 11/18] COMP Worlds - Q63 --- src/main/deploy/pathplanner/paths/SB-F4.path | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/SB-F4.path b/src/main/deploy/pathplanner/paths/SB-F4.path index e996a690..56dd5e4c 100644 --- a/src/main/deploy/pathplanner/paths/SB-F4.path +++ b/src/main/deploy/pathplanner/paths/SB-F4.path @@ -20,8 +20,8 @@ "y": 2.318745636064738 }, "prevControl": { - "x": 5.31912573097614, - "y": 0.9485777602083012 + "x": 6.220860145001316, + "y": 0.42159011564813337 }, "nextControl": null, "isLocked": false, From d628f783463daca16387aaab271d2a465b81902a Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Fri, 19 Apr 2024 11:07:56 -0400 Subject: [PATCH 12/18] COMP Worlds - Q74 --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../command/shoot/AutonCand/PointBlankShotAuton.java | 9 +++++++-- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b7e3f5fc..6dc29c09 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -161,7 +161,7 @@ protected void initializeNamedCommands() { leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5)); NamedCommands.registerCommand("Cand-Sub", - new PointBlankShotAuton(flywheel, pivot, indexer) + new PointBlankShotAuton(flywheel, pivot, indexer, collector) .deadlineWith(leds.enableState(LED_STATES.SHOOTING).withTimeout(1))); NamedCommands.registerCommand("Cand-C1", new CandC1(flywheel, pivot, indexer)); NamedCommands.registerCommand("Cand-C2", new CandC2(flywheel, pivot, indexer)); diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java index 53faa8c3..d82a99d3 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.CandConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.subsystems.Collector; import frc.robot.subsystems.Flywheel; import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Indexer; @@ -13,6 +14,7 @@ public class PointBlankShotAuton extends Command { private final Flywheel flywheel; private final Pivot pivot; private final Indexer indexer; + private final Collector collector; private boolean shot = false; private double startTime = 0; @@ -27,12 +29,13 @@ public class PointBlankShotAuton extends Command { * @param pivot subsystem * @param indexer subsystem */ - public PointBlankShotAuton(Flywheel flywheel, Pivot pivot, Indexer indexer) { + public PointBlankShotAuton(Flywheel flywheel, Pivot pivot, Indexer indexer, Collector collector) { this.flywheel = flywheel; this.pivot = pivot; this.indexer = indexer; + this.collector = collector; - addRequirements(flywheel, pivot, indexer); + addRequirements(flywheel, pivot, indexer, collector); } @Override @@ -57,6 +60,7 @@ public void execute() { shot = true; shotTime = Timer.getFPGATimestamp(); indexer.indexUp(); + collector.setPower(1d); } flywheel.setAllMotorsRPM(CandConstants.POINT_BLANK_RPM + pivot.getBias()); @@ -68,6 +72,7 @@ public void end(boolean interrupted) { flywheel.coast(true); pivot.setTargetAngle(pivot.getStowAngle()); indexer.stop(); + collector.stop(); } @Override From 48e856226056fe9561cb119caef5b13baa31db79 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Fri, 19 Apr 2024 12:11:44 -0400 Subject: [PATCH 13/18] COMP Worlds - Q91 --- src/main/deploy/pathplanner/paths/BB-F3.path | 4 ++-- src/main/deploy/pathplanner/paths/BF-F3.path | 4 ++-- src/main/deploy/pathplanner/paths/BS-F3.path | 4 ++-- src/main/deploy/pathplanner/paths/C1-F3.path | 4 ++-- src/main/deploy/pathplanner/paths/C2-F3.path | 4 ++-- src/main/deploy/pathplanner/paths/C3-F3.path | 4 ++-- .../deploy/pathplanner/paths/F3-BS-Close.path | 12 ++++++------ .../deploy/pathplanner/paths/F4-BS-Close.path | 8 ++++---- .../deploy/pathplanner/paths/Linked Point.path | 6 +++--- src/main/deploy/pathplanner/paths/SB-F3.path | 16 ++++++++-------- src/main/deploy/pathplanner/paths/SF-F3.path | 4 ++-- 11 files changed, 35 insertions(+), 35 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/BB-F3.path b/src/main/deploy/pathplanner/paths/BB-F3.path index 2f7a936c..1f797bb6 100644 --- a/src/main/deploy/pathplanner/paths/BB-F3.path +++ b/src/main/deploy/pathplanner/paths/BB-F3.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 8.071172319234796, + "x": 7.274835434121652, "y": 4.098792791023527 }, "prevControl": { - "x": 5.899987448133642, + "x": 5.103650563020498, "y": 0.6446283647949121 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/BF-F3.path b/src/main/deploy/pathplanner/paths/BF-F3.path index c42749df..37e51c08 100644 --- a/src/main/deploy/pathplanner/paths/BF-F3.path +++ b/src/main/deploy/pathplanner/paths/BF-F3.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 8.071172319234796, + "x": 7.274835434121652, "y": 4.098792791023527 }, "prevControl": { - "x": 5.899987448133642, + "x": 5.103650563020498, "y": 0.6446283647949121 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/BS-F3.path b/src/main/deploy/pathplanner/paths/BS-F3.path index 9fea166b..c416f978 100644 --- a/src/main/deploy/pathplanner/paths/BS-F3.path +++ b/src/main/deploy/pathplanner/paths/BS-F3.path @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 8.071172319234796, + "x": 7.274835434121652, "y": 4.098792791023527 }, "prevControl": { - "x": 7.366779233701121, + "x": 6.570442348587977, "y": 4.112671258372252 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/C1-F3.path b/src/main/deploy/pathplanner/paths/C1-F3.path index 335beb22..db8dffc6 100644 --- a/src/main/deploy/pathplanner/paths/C1-F3.path +++ b/src/main/deploy/pathplanner/paths/C1-F3.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 8.071172319234796, + "x": 7.274835434121652, "y": 4.098792791023527 }, "prevControl": { - "x": 8.071172319234796, + "x": 7.274835434121652, "y": 6.370462800790869 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/C2-F3.path b/src/main/deploy/pathplanner/paths/C2-F3.path index dbee4161..c183cc1e 100644 --- a/src/main/deploy/pathplanner/paths/C2-F3.path +++ b/src/main/deploy/pathplanner/paths/C2-F3.path @@ -32,11 +32,11 @@ }, { "anchor": { - "x": 8.071172319234796, + "x": 7.274835434121652, "y": 4.098792791023527 }, "prevControl": { - "x": 7.038875810419238, + "x": 6.2425389253060946, "y": 4.08924958528069 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/C3-F3.path b/src/main/deploy/pathplanner/paths/C3-F3.path index a7e772f9..9a0d1711 100644 --- a/src/main/deploy/pathplanner/paths/C3-F3.path +++ b/src/main/deploy/pathplanner/paths/C3-F3.path @@ -48,11 +48,11 @@ }, { "anchor": { - "x": 8.071172319234796, + "x": 7.274835434121652, "y": 4.098792791023527 }, "prevControl": { - "x": 7.469978508485491, + "x": 6.6736416233723475, "y": 4.0658279121891265 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/F3-BS-Close.path b/src/main/deploy/pathplanner/paths/F3-BS-Close.path index 3e556b7c..613f2cd7 100644 --- a/src/main/deploy/pathplanner/paths/F3-BS-Close.path +++ b/src/main/deploy/pathplanner/paths/F3-BS-Close.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 8.071172319234796, + "x": 7.274835434121652, "y": 4.098792791023527 }, "prevControl": null, "nextControl": { - "x": 6.314546837367568, + "x": 5.518209952254424, "y": 4.098792791023527 }, "isLocked": false, @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 0.71, - "y": 4.22 + "x": 0.78, + "y": 4.44 }, "prevControl": { - "x": 2.9886692583656194, - "y": 2.4007214918852084 + "x": 3.0586692583656196, + "y": 2.620721491885209 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/F4-BS-Close.path b/src/main/deploy/pathplanner/paths/F4-BS-Close.path index b6b6840f..2e9cd0d2 100644 --- a/src/main/deploy/pathplanner/paths/F4-BS-Close.path +++ b/src/main/deploy/pathplanner/paths/F4-BS-Close.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.71, - "y": 4.22 + "x": 0.7753211512129142, + "y": 4.438407050851191 }, "prevControl": { - "x": 2.537562767433542, - "y": 2.7035423301569765 + "x": 2.602883918646456, + "y": 2.9219493810081674 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Linked Point.path b/src/main/deploy/pathplanner/paths/Linked Point.path index d338f9fd..ffab829d 100644 --- a/src/main/deploy/pathplanner/paths/Linked Point.path +++ b/src/main/deploy/pathplanner/paths/Linked Point.path @@ -160,15 +160,15 @@ }, { "anchor": { - "x": 8.071172319234796, + "x": 7.274835434121652, "y": 4.098792791023527 }, "prevControl": { - "x": 8.073127837058552, + "x": 7.276790951945408, "y": 4.0869389655563415 }, "nextControl": { - "x": 8.06921680141104, + "x": 7.272879916297896, "y": 4.110646616490713 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/SB-F3.path b/src/main/deploy/pathplanner/paths/SB-F3.path index 9b407e01..1acd7de4 100644 --- a/src/main/deploy/pathplanner/paths/SB-F3.path +++ b/src/main/deploy/pathplanner/paths/SB-F3.path @@ -36,24 +36,24 @@ "y": 4.098792791023527 }, "prevControl": { - "x": 5.143024138751494, - "y": 4.106194885737151 + "x": 5.142975442867549, + "y": 4.098792791023527 }, "nextControl": { - "x": 6.595606914466325, - "y": 4.087081954477746 + "x": 6.4083311132622045, + "y": 4.098792791023527 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.071172319234796, + "x": 7.274835434121652, "y": 4.098792791023527 }, "prevControl": { - "x": 7.469978508485491, - "y": 4.0658279121891265 + "x": 6.630632970750323, + "y": 4.098792791023527 }, "nextControl": null, "isLocked": false, @@ -64,7 +64,7 @@ "constraintZones": [ { "name": "New Constraints Zone", - "minWaypointRelativePos": 0.5499999999999999, + "minWaypointRelativePos": 0.75, "maxWaypointRelativePos": 1.45, "constraints": { "maxVelocity": 3.0, diff --git a/src/main/deploy/pathplanner/paths/SF-F3.path b/src/main/deploy/pathplanner/paths/SF-F3.path index d86c1859..97ee47f0 100644 --- a/src/main/deploy/pathplanner/paths/SF-F3.path +++ b/src/main/deploy/pathplanner/paths/SF-F3.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 8.071172319234796, + "x": 7.274835434121652, "y": 4.098792791023527 }, "prevControl": { - "x": 6.647050737658299, + "x": 5.850713852545155, "y": 3.6840256274628658 }, "nextControl": null, From 88222568bd1e3c24498f3f9a1b289edbbb7202e6 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Fri, 19 Apr 2024 14:50:33 -0400 Subject: [PATCH 14/18] COMP Worlds - Improve collect --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d3e1de01..69042bbd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -373,7 +373,7 @@ public class MusicConstants { public class CollectorConstants { public static final boolean COLLECTOR_MOTOR_INVERTED = true; - public static final int COLLECTOR_MOTOR_STATOR_CURRENT_LIMIT = 90; + public static final int COLLECTOR_MOTOR_STATOR_CURRENT_LIMIT = 140; public static final boolean COLLECTOR_MOTOR_BRAKE_MODE = false; public static final double COLLECTOR_DEBOUNCE_TIME = 0.5d; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6dc29c09..0be6cfce 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -258,7 +258,7 @@ protected void configureButtonBindings() { /* COPILOT */ new Trigger(() -> (coPilot.getBButton() && !driver.getRightBumper())) .whileTrue(new InstantCommand(() -> flywheel.stop(), flywheel) - .andThen(new SmartCollect(() -> 1d, () -> .9, collector, indexer, pivot, flywheel)) + .andThen(new SmartCollect(() -> 1d, () -> 1d, collector, indexer, pivot, flywheel)) .deadlineWith(leds.enableState(LED_STATES.COLLECTING))); // new Trigger(coPilot::getBButton) From 22691753cf0848a1ae191cd75a3041da74ecfe99 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Fri, 19 Apr 2024 15:31:21 -0400 Subject: [PATCH 15/18] COMP Worlds - 103 --- src/main/java/frc/robot/Constants.java | 4 ++-- src/main/java/frc/robot/command/SmartCollect.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 69042bbd..f5efe291 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -616,8 +616,8 @@ public enum ShootingState { public class CandConstants { // Amp - public static final double AMP_TOP_RPM = 325; - public static final double AMP_BOTTOM_RPM = 625; + public static final double AMP_TOP_RPM = 400; + public static final double AMP_BOTTOM_RPM = 700; public static final double AMP_ANGLE = 92; // PointBlank diff --git a/src/main/java/frc/robot/command/SmartCollect.java b/src/main/java/frc/robot/command/SmartCollect.java index 6baf33da..0430477f 100644 --- a/src/main/java/frc/robot/command/SmartCollect.java +++ b/src/main/java/frc/robot/command/SmartCollect.java @@ -82,7 +82,7 @@ public void execute() { case IN_COLLECT: /* Note has passed beambreak past collector */ if (allowIndex) { // Slow down collector to prevent jamming - collector.setPower(0.65 * collectorPower.getAsDouble()); + collector.setPower(collectorPower.getAsDouble()); indexer.setPower(indexerPower.getAsDouble()); if(!DriverStation.isAutonomous()){ flywheel.setAllMotorsRPM(-500); @@ -100,7 +100,7 @@ public void execute() { indexer.setPower(0.9 * indexerPower.getAsDouble()); } else if (reversedFromExit) { indexer.stop(); - if (!DriverStation.isAutonomous()){ + if (!DriverStation.isAutonomous()) { flywheel.coast(true); } new TimedCommand(RobotContainer.hapticDriverCommand(), 1d).schedule(); From f239e2bc7d46712d0bb0e24729ee0c5898e89fec Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Fri, 19 Apr 2024 17:16:15 -0400 Subject: [PATCH 16/18] COMP Worlds - Change debounce --- src/main/java/frc/robot/subsystems/Collector.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Collector.java b/src/main/java/frc/robot/subsystems/Collector.java index ba536cb7..c59796dd 100644 --- a/src/main/java/frc/robot/subsystems/Collector.java +++ b/src/main/java/frc/robot/subsystems/Collector.java @@ -34,7 +34,7 @@ public class Collector extends SubsystemBase { private DoubleLogEntry collectorPowerLog; private BooleanLogEntry hasPieceLog; - private Debouncer entryDebouncer = new Debouncer(0.05); + private Debouncer entryDebouncer = new Debouncer(0.4); public Collector() { motor = new ThunderBird( From c65616696642147c2ebd97e8a8fe1effdf091a31 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Mon, 22 Apr 2024 12:57:10 -0400 Subject: [PATCH 17/18] COMP Worlds - Elims --- src/main/java/frc/robot/Constants.java | 23 ++++++++++++++++++- src/main/java/frc/robot/RobotContainer.java | 16 ++++++++++--- src/main/java/frc/robot/subsystems/LEDs.java | 4 ++-- .../java/frc/robot/subsystems/Swerve.java | 2 +- 4 files changed, 38 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f5efe291..316abc9a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -717,7 +717,28 @@ public class LEDsConstants { public static final int PINK_HUE = 355; public enum LED_STATES { - CUSTOMCONTROL(0), DISABLED(1), EMERGENCY(2), START(3), GOOD_POSE(4), COLLECT_PLANNED(5), COLLECTED(6), SHOT(7), FINISHED_CLIMB(8), SHOOTING(9), COLLECTING(10), CHASING(11), CLIMBING(12), BAD_POSE(13), HAS_PIECE(14), HAS_VISION(15), DEFAULT(16), PIVOT_BOTTOM_SWITCH(17), PIVOT_TOP_SWITCH(18), COLLECTOR_BEAMBREAK(19), INDEXER_ENTER_BEAMBREAK(20), INDEXER_EXIT_BEAMBREAK(21); + CUSTOMCONTROL(0), + DISABLED(1), + EMERGENCY(2), + START(3), + GOOD_POSE(4), + COLLECT_PLANNED(5), + COLLECTED(6), + SHOT(7), + FINISHED_CLIMB(8), + SHOOTING(9), + COLLECTING(10), + CHASING(11), + CLIMBING(12), + BAD_POSE(13), + HAS_PIECE(14), + HAS_VISION(15), + DEFAULT(16), + PIVOT_BOTTOM_SWITCH(17), + PIVOT_TOP_SWITCH(18), + COLLECTOR_BEAMBREAK(19), + INDEXER_ENTER_BEAMBREAK(20), + INDEXER_EXIT_BEAMBREAK(21); private final int priority; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0be6cfce..c8f8bce2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -302,12 +302,22 @@ protected void configureButtonBindings() { .whileTrue(leds.enableState(LED_STATES.HAS_PIECE)) .whileTrue(leds.enableState(LED_STATES.COLLECTED).withTimeout(2)); - new Trigger(() -> DriverStation.isDisabled() && triggerInit - && !(limelights.getStopMe().hasTarget() || drivetrain.isInField() || drivetrain.isStable())) - .whileTrue(leds.enableState(LED_STATES.BAD_POSE)); + // new Trigger(() -> DriverStation.isDisabled() && triggerInit + // && !(limelights.getStopMe().hasTarget() || drivetrain.isInField() || drivetrain.isStable())) + // .whileTrue(leds.enableState(LED_STATES.BAD_POSE)); + + // new Trigger(() -> !drivetrain.isStable() && DriverStation.isDisabled() && !drivetrain.isInField() + // && !limelights.getStopMe().hasTarget() && triggerInit) + // .whileTrue(leds.enableState(LED_STATES.BAD_POSE)); + + new Trigger(() -> ((!drivetrain.isStable() || !drivetrain.isInField() || !limelights.getStopMe().hasTarget()) + && DriverStation.isDisabled() && triggerInit)) + .whileTrue(leds.enableState(LED_STATES.BAD_POSE)); + new Trigger(() -> drivetrain.isStable() && DriverStation.isDisabled() && drivetrain.isInField() && limelights.getStopMe().hasTarget() && triggerInit) .whileTrue(leds.enableState(LED_STATES.GOOD_POSE)); + triggerInit = true; new Trigger(() -> collector.getEntryBeamBreakState()) diff --git a/src/main/java/frc/robot/subsystems/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs.java index 29e131b8..63a4c77a 100644 --- a/src/main/java/frc/robot/subsystems/LEDs.java +++ b/src/main/java/frc/robot/subsystems/LEDs.java @@ -49,11 +49,11 @@ public void periodic() { break; case BAD_POSE: - pulse(-1, LEDsConstants.BLUE_HUE); + pulse(-1, LEDsConstants.RED_HUE); break; case GOOD_POSE: - blink(-1, LEDsConstants.PINK_HUE); + blink(-1, LEDsConstants.GREEN_HUE); break; case COLLECT_PLANNED: diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 9788b88a..31653bf7 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -295,7 +295,7 @@ public Pose2d getPose() { } public boolean isStable() { - return (Math.abs(rotFilter.lastValue() - getPose().getRotation().getDegrees()) < 1 && Math.abs(xFilter.lastValue() - getPose().getX()) < 1 && Math.abs(yFilter.lastValue() - getPose().getY()) < 1); + return (Math.abs(rotFilter.lastValue() - getPose().getRotation().getDegrees()) < 2 && Math.abs(xFilter.lastValue() - getPose().getX()) < 1 && Math.abs(yFilter.lastValue() - getPose().getY()) < 1); } public ChassisSpeeds getCurrentRobotChassisSpeeds() { From ba36364fac974e2a7b035d5e32a24f90627bb647 Mon Sep 17 00:00:00 2001 From: MattD8957 Date: Tue, 30 Apr 2024 11:32:00 -0400 Subject: [PATCH 18/18] Linter --- .../robot/command/shoot/AutonCand/PointBlankShotAuton.java | 1 + src/main/java/frc/robot/command/shoot/NotePass.java | 6 ++++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java index d82a99d3..19c2393a 100644 --- a/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java +++ b/src/main/java/frc/robot/command/shoot/AutonCand/PointBlankShotAuton.java @@ -28,6 +28,7 @@ public class PointBlankShotAuton extends Command { * @param flywheel subsystem * @param pivot subsystem * @param indexer subsystem + * @param collector subsystem */ public PointBlankShotAuton(Flywheel flywheel, Pivot pivot, Indexer indexer, Collector collector) { this.flywheel = flywheel; diff --git a/src/main/java/frc/robot/command/shoot/NotePass.java b/src/main/java/frc/robot/command/shoot/NotePass.java index cd3f5327..1c8c84c5 100644 --- a/src/main/java/frc/robot/command/shoot/NotePass.java +++ b/src/main/java/frc/robot/command/shoot/NotePass.java @@ -109,8 +109,9 @@ public void execute() { pivot.setTargetAngle(ShooterConstants.NOTEPASS_ANGLE_MAP.get(distanceToCorner) + pivot.getBias()); if (flywheel.allMotorsOnTarget() && pivot.onTarget() && inTolerance()) { - if (DriverStation.isAutonomous()) + if (DriverStation.isAutonomous()) { indexer.indexUp(); + } new TimedCommand(RobotContainer.hapticCopilotCommand(), 1d).schedule(); new TimedCommand(RobotContainer.hapticDriverCommand(), 1d).schedule(); } @@ -122,8 +123,9 @@ public void execute() { public void end(boolean interrupted) { flywheel.coast(true); pivot.setTargetAngle(pivot.getStowAngle()); - if (DriverStation.isAutonomous()) + if (DriverStation.isAutonomous()) { indexer.stop(); + } } @Override