From 00f426f3d9879adc09e9b31d1e30521c42e30517 Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Tue, 12 Mar 2024 16:53:47 -0700 Subject: [PATCH 01/14] Move field constants to fields --- .../cyberknights4911/robot2024/Robot2024.java | 31 ++++--------------- .../java/com/cyberknights4911/util/Field.java | 26 +++++++++++++--- 2 files changed, 27 insertions(+), 30 deletions(-) diff --git a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java index a2fb83e..c404efc 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java +++ b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java @@ -33,12 +33,10 @@ import com.cyberknights4911.robot2024.shooter.ShooterIO; import com.cyberknights4911.robot2024.shooter.ShooterIOReal; import com.cyberknights4911.robot2024.shooter.ShooterIOSim; -import com.cyberknights4911.util.Alliance; +import com.cyberknights4911.util.Field; import com.cyberknights4911.util.GameAlerts; import com.cyberknights4911.util.SparkBurnManager; import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; @@ -71,15 +69,6 @@ public Robot2024() { } private void configureControls() { - double x; - double y; - if (Alliance.isRed()) { - x = 652.73; - y = 218.42; - } else { - x = -1.50; - y = 218.42; - } drive.setDefaultCommand( drive.joystickDrive( Robot2024Constants.CONTROL_CONSTANTS, @@ -94,16 +83,7 @@ private void configureControls() { binding .triggersFor(ButtonAction.ZeroSpeaker) - .onTrue( - Commands.runOnce( - () -> { - drive.setPose( - new Pose2d( - new Translation2d( - Units.inchesToMeters(602.73), Units.inchesToMeters(218.42)), - new Rotation2d())); - }, - drive)); + .onTrue(Commands.runOnce(() -> drive.setPose(Field.subWooferIndex()), drive)); binding .triggersFor(ButtonAction.AmpLockOn) @@ -112,8 +92,9 @@ private void configureControls() { Robot2024Constants.CONTROL_CONSTANTS, binding.supplierFor(StickAction.FORWARD), binding.supplierFor(StickAction.STRAFE), - Math.PI / 2)); + Field.ampOpeningAngle().getRadians())); + Translation2d speakerOpening = Field.speakerOpening(); // TODO: combine with auto-aim. binding .triggersFor(ButtonAction.SpeakerLockOn) @@ -122,8 +103,8 @@ private void configureControls() { Robot2024Constants.CONTROL_CONSTANTS, binding.supplierFor(StickAction.FORWARD), binding.supplierFor(StickAction.STRAFE), - Units.inchesToMeters(x), - Units.inchesToMeters(y))); + Units.inchesToMeters(speakerOpening.getX()), + Units.inchesToMeters(speakerOpening.getY()))); binding.triggersFor(ButtonAction.StowCollector).onTrue(stowEverything()); diff --git a/src/main/java/com/cyberknights4911/util/Field.java b/src/main/java/com/cyberknights4911/util/Field.java index 475141d..0149300 100644 --- a/src/main/java/com/cyberknights4911/util/Field.java +++ b/src/main/java/com/cyberknights4911/util/Field.java @@ -11,7 +11,9 @@ import com.cyberknights4911.logging.Alert.AlertType; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import java.io.UncheckedIOException; import java.util.ArrayList; @@ -33,11 +35,25 @@ public static AprilTagFieldLayout getFieldLayout() { } } - public static Transform2d speakerOpening() { - return new Transform2d(); + public static Translation2d speakerOpening() { + if (Alliance.isRed()) { + return new Translation2d(652.73, 218.42); + } else { + return new Translation2d(-1.50, 218.42); + } + } + + public static Pose2d subWooferIndex() { + if (Alliance.isRed()) { + return new Pose2d(new Translation2d(602.73, 218.42), new Rotation2d()); + } else { + // TODO: get values for blue alliance + return new Pose2d(); + } } - public static Transform2d subWooferIndex() { - return new Transform2d(); + public static Rotation2d ampOpeningAngle() { + // Same angle regardless of alliance + return new Rotation2d(Math.PI / 2); } } From f821c912994d18e9a68b3de0474343abe7d7822d Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Wed, 13 Mar 2024 09:14:59 -0700 Subject: [PATCH 02/14] Move zero angle to fields class need to test though --- .../java/com/cyberknights4911/drive/Drive.java | 9 --------- .../com/cyberknights4911/robot2024/Robot2024.java | 13 ++++++++++++- src/main/java/com/cyberknights4911/util/Field.java | 8 ++++++++ src/main/java/com/cyberknights4911/wham/Wham.java | 14 +++++++++++++- 4 files changed, 33 insertions(+), 11 deletions(-) diff --git a/src/main/java/com/cyberknights4911/drive/Drive.java b/src/main/java/com/cyberknights4911/drive/Drive.java index 1464f77..ecce393 100644 --- a/src/main/java/com/cyberknights4911/drive/Drive.java +++ b/src/main/java/com/cyberknights4911/drive/Drive.java @@ -255,15 +255,6 @@ public Command stopWithX() { this); } - /** - * Resets the robot's current pose rotation to be zero. Will not modify robot pose translation. - */ - public Command zeroPoseToCurrentRotation() { - return Commands.runOnce( - () -> setPose(new Pose2d(getPose().getTranslation(), new Rotation2d())), this) - .ignoringDisable(true); - } - ChassisSpeeds createChassisSpeeds( ControlConstants controlConstants, DoubleSupplier xSupplier, diff --git a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java index 26c4fec..8c9f8a6 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java +++ b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java @@ -37,6 +37,7 @@ import com.cyberknights4911.util.GameAlerts; import com.cyberknights4911.util.SparkBurnManager; import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; @@ -77,7 +78,7 @@ private void configureControls() { binding.supplierFor(StickAction.ROTATE))); // TODO: Maybe remove this or make it harder to do by accident. - binding.triggersFor(ButtonAction.ZeroGyro).onTrue(drive.zeroPoseToCurrentRotation()); + binding.triggersFor(ButtonAction.ZeroGyro).onTrue(zeroPoseToCurrentRotation()); binding.triggersFor(ButtonAction.Brake).whileTrue(drive.stopWithX()); @@ -248,6 +249,16 @@ private Drive createDrive() { } } + /** + * Resets the robot's current pose rotation to be zero. Will not modify robot pose translation. + */ + private Command zeroPoseToCurrentRotation() { + return Commands.runOnce( + () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), Field.forwardAngle())), + drive) + .ignoringDisable(true); + } + private Command rumbleLongOnce() { return Commands.runOnce(() -> binding.setDriverRumble(true)) .withTimeout(1.5) diff --git a/src/main/java/com/cyberknights4911/util/Field.java b/src/main/java/com/cyberknights4911/util/Field.java index 0149300..a452e4e 100644 --- a/src/main/java/com/cyberknights4911/util/Field.java +++ b/src/main/java/com/cyberknights4911/util/Field.java @@ -56,4 +56,12 @@ public static Rotation2d ampOpeningAngle() { // Same angle regardless of alliance return new Rotation2d(Math.PI / 2); } + + public static Rotation2d forwardAngle() { + if (Alliance.isRed()) { + return new Rotation2d(Math.PI / 2); + } else { + return new Rotation2d(); + } + } } diff --git a/src/main/java/com/cyberknights4911/wham/Wham.java b/src/main/java/com/cyberknights4911/wham/Wham.java index dc576bd..2912a48 100644 --- a/src/main/java/com/cyberknights4911/wham/Wham.java +++ b/src/main/java/com/cyberknights4911/wham/Wham.java @@ -20,6 +20,7 @@ import com.cyberknights4911.entrypoint.RobotContainer; import com.cyberknights4911.robot2024.Robot2024Constants; import com.cyberknights4911.util.Alliance; +import com.cyberknights4911.util.Field; import com.cyberknights4911.util.GameAlerts; import com.cyberknights4911.vision.simple.VisionSimple; import com.cyberknights4911.wham.slurpp.Slurpp; @@ -30,6 +31,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import org.littletonrobotics.junction.LoggedRobot; @@ -68,7 +70,7 @@ private void configureControls() { binding.triggersFor(ButtonAction.Brake).whileTrue(drive.stopWithX()); - binding.triggersFor(ButtonAction.ZeroGyro).onTrue(drive.zeroPoseToCurrentRotation()); + binding.triggersFor(ButtonAction.ZeroGyro).onTrue(zeroPoseToCurrentRotation()); binding .triggersFor(ButtonAction.ZeroSpeaker) @@ -219,4 +221,14 @@ public void setupAutos(AutoCommandHandler handler) { Autos autos = new Autos(WhamConstants.DRIVE_CONSTANTS, drive, slurpp); autos.addAllAutos(handler); } + + /** + * Resets the robot's current pose rotation to be zero. Will not modify robot pose translation. + */ + private Command zeroPoseToCurrentRotation() { + return Commands.runOnce( + () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), Field.forwardAngle())), + drive) + .ignoringDisable(true); + } } From 8a148f47b7fefd04cbe4aa43571a24532a080815 Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Sat, 16 Mar 2024 13:14:39 -0700 Subject: [PATCH 03/14] Stop logging protos Also slow down back off speed --- .../cyberknights4911/robot2024/Robot2024Constants.java | 2 +- .../com/cyberknights4911/robot2024/shooter/Shooter.java | 1 + src/main/java/com/cyberknights4911/vision/Vision.java | 2 +- src/main/java/com/cyberknights4911/vision/VisionIO.java | 5 ++++- .../java/com/cyberknights4911/vision/VisionIOPhoton.java | 1 - .../cyberknights4911/vision/simple/VisionIOSimple.java | 9 ++++++++- .../com/cyberknights4911/vision/simple/VisionSimple.java | 6 +++--- 7 files changed, 18 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java b/src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java index a995605..d166dd4 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java +++ b/src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java @@ -136,7 +136,7 @@ private Robot2024Constants() {} .sensorId(2) .aimerGearRatio((60 / 20) * (60 / 20)) .guidePercentOutput(.2) - .guideReversePercentOutput(-0.1) + .guideReversePercentOutput(-0.05) .feedTime(2) .aimTime(.5) .speakerPositionDegrees(54) diff --git a/src/main/java/com/cyberknights4911/robot2024/shooter/Shooter.java b/src/main/java/com/cyberknights4911/robot2024/shooter/Shooter.java index 44bc8cd..aa21449 100644 --- a/src/main/java/com/cyberknights4911/robot2024/shooter/Shooter.java +++ b/src/main/java/com/cyberknights4911/robot2024/shooter/Shooter.java @@ -246,6 +246,7 @@ public Command collectAndWaitForNote() { private Command backNoteUp() { return Commands.runOnce(() -> runGuideOutput(guideReverseOutput.get()), this) .andThen(Commands.waitUntil(() -> inputs.beamBreakValue > beamThreshold.get())) + .withTimeout(.5) .andThen(() -> stopGuide(), this); } diff --git a/src/main/java/com/cyberknights4911/vision/Vision.java b/src/main/java/com/cyberknights4911/vision/Vision.java index b4ff565..3d574d1 100644 --- a/src/main/java/com/cyberknights4911/vision/Vision.java +++ b/src/main/java/com/cyberknights4911/vision/Vision.java @@ -130,7 +130,7 @@ private void updatePose(CameraConfig cameraConfig) { // TODO(rbrewer): this is gross, find an alternative synchronized (cameraConfig.constants()) { - cameraResult = cameraConfig.inputs().lastResult; + // cameraResult = cameraConfig.inputs().lastResult; timestamp = cameraConfig.inputs().lastTimestamp; } diff --git a/src/main/java/com/cyberknights4911/vision/VisionIO.java b/src/main/java/com/cyberknights4911/vision/VisionIO.java index 84e8eaf..b11e4e6 100644 --- a/src/main/java/com/cyberknights4911/vision/VisionIO.java +++ b/src/main/java/com/cyberknights4911/vision/VisionIO.java @@ -18,7 +18,6 @@ public interface VisionIO { public static class VisionIOInputs { public boolean isOnline = false; public double lastTimestamp = 0; - public PhotonPipelineResult lastResult = new PhotonPipelineResult(); } public default void updateInputs(VisionIOInputs inputs) {} @@ -28,4 +27,8 @@ public default void setReferencePose(Pose2d referencePose) {} public default Optional update(PhotonPipelineResult cameraResult) { return Optional.empty(); } + + public default PhotonPipelineResult getLastResult() { + return new PhotonPipelineResult(); + } } diff --git a/src/main/java/com/cyberknights4911/vision/VisionIOPhoton.java b/src/main/java/com/cyberknights4911/vision/VisionIOPhoton.java index 35fa281..c7e16a0 100644 --- a/src/main/java/com/cyberknights4911/vision/VisionIOPhoton.java +++ b/src/main/java/com/cyberknights4911/vision/VisionIOPhoton.java @@ -71,7 +71,6 @@ public VisionIOPhoton(VisionConstants visionConstants, CameraConstants constants @Override public synchronized void updateInputs(VisionIOInputs inputs) { inputs.isOnline = camera.isConnected(); - inputs.lastResult = lastResult; inputs.lastTimestamp = lastTimestamp; } diff --git a/src/main/java/com/cyberknights4911/vision/simple/VisionIOSimple.java b/src/main/java/com/cyberknights4911/vision/simple/VisionIOSimple.java index b46037d..aa15e65 100644 --- a/src/main/java/com/cyberknights4911/vision/simple/VisionIOSimple.java +++ b/src/main/java/com/cyberknights4911/vision/simple/VisionIOSimple.java @@ -23,6 +23,8 @@ public final class VisionIOSimple implements VisionIO { private final PhotonCamera camera; private final PhotonPoseEstimator photonPoseEstimator; + private PhotonPipelineResult lastResult = new PhotonPipelineResult(); + public VisionIOSimple(VisionConstants visionConstants, CameraConstants constants) { camera = new PhotonCamera(constants.photonCameraName()); camera.setDriverMode(false); @@ -43,7 +45,7 @@ public void updateInputs(VisionIOInputs inputs) { PhotonPipelineResult newResult = camera.getLatestResult(); double latestTimestamp = newResult.getTimestampSeconds(); if (Math.abs(latestTimestamp - inputs.lastTimestamp) > 1e-5) { - inputs.lastResult = newResult; + lastResult = newResult; inputs.lastTimestamp = latestTimestamp; } } @@ -58,4 +60,9 @@ public void setReferencePose(Pose2d referencePose) { public Optional update(PhotonPipelineResult cameraResult) { return photonPoseEstimator.update(cameraResult); } + + @Override + public PhotonPipelineResult getLastResult() { + return lastResult; + } } diff --git a/src/main/java/com/cyberknights4911/vision/simple/VisionSimple.java b/src/main/java/com/cyberknights4911/vision/simple/VisionSimple.java index 96ca72e..193c1a1 100644 --- a/src/main/java/com/cyberknights4911/vision/simple/VisionSimple.java +++ b/src/main/java/com/cyberknights4911/vision/simple/VisionSimple.java @@ -102,14 +102,14 @@ private void updatePose(CameraConfig cameraConfig) { return; } lastTimestamp.put(cameraConfig.constants().name(), cameraConfig.inputs().lastTimestamp); - + PhotonPipelineResult lastResult = cameraConfig.visionIO().getLastResult(); cameraConfig .visionIO() - .update(cameraConfig.inputs().lastResult) + .update(lastResult) .ifPresent( estimatedPose -> { var robotPose = estimatedPose.estimatedPose.toPose2d(); - var stdDevs = getEstimationStdDevs(robotPose, cameraConfig.inputs().lastResult); + var stdDevs = getEstimationStdDevs(robotPose, lastResult); Logger.recordOutput( "Vision/" + cameraConfig.constants().name() + "/RobotPose", robotPose); From 3a6ade5da2882575640ca9160c956d881d647ec0 Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Sat, 16 Mar 2024 14:31:49 -0700 Subject: [PATCH 04/14] Maybe fix shoot and leave source --- .../pathplanner/autos/SHOOT_AND_LEAVE_SOURCE.auto | 6 ------ .../com/cyberknights4911/robot2024/Robot2024.java | 14 ++++++++++++-- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE.auto b/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE.auto index 2db5f45..e14026b 100644 --- a/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE.auto +++ b/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE.auto @@ -17,12 +17,6 @@ "name": "SHOOT_SUB" } }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, { "type": "path", "data": { diff --git a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java index b62525a..0e39dd3 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java +++ b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java @@ -164,12 +164,22 @@ public void onRobotPeriodic(LoggedRobot robot) { } } + private Command scoreForAuto() { + return shooter + .aimSubwoofer() + .andThen(Commands.waitSeconds(.5)) + .andThen(shooter.fire()) + .andThen(Commands.waitSeconds(.5)); + } + @Override public void setupAutos(AutoCommandHandler handler) { + NamedCommands.registerCommand("SHOOT_SUB", scoreForAuto()); + NamedCommands.registerCommand("COLLECT", collectNote()); + Autos autos = new Autos(Robot2024Constants.DRIVE_CONSTANTS, climb, collect, shooter, drive); + autos.addAllAutos(handler); - NamedCommands.registerCommand("SHOOT_SUB", shooter.aimSubwoofer().andThen(shooter.fire())); - NamedCommands.registerCommand("COLLECT", collectNote()); } private Climb createClimb() { From 78ca3d5bec6e3c963c350c57c254a86f018f66b4 Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Sat, 16 Mar 2024 19:43:56 -0700 Subject: [PATCH 05/14] More Autos --- .../pathplanner/autos/HopesAndDreams.auto | 86 +++++++++++++++++-- src/main/deploy/pathplanner/autos/Lol.auto | 31 +++++++ .../autos/SHOOT_AND_LEAVE_SOURCE.auto | 6 +- .../autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto | 44 +++++++++- .../autos/SHOOT_WAIT_AND_LEAVE_AMP.auto | 37 ++++++++ .../pathplanner/paths/LeaveSubLeft.path | 52 +++++++++++ src/main/deploy/pathplanner/paths/Lol-Go.path | 52 +++++++++++ .../deploy/pathplanner/paths/Lol-Ready.path | 52 +++++++++++ .../pathplanner/paths/Score Source Side.path | 52 +++++++++++ .../pathplanner/paths/Yoink Note 2.path | 52 +++++++++++ .../com/cyberknights4911/robot2024/Autos.java | 4 + .../cyberknights4911/robot2024/Robot2024.java | 4 +- .../robot2024/Robot2024Constants.java | 4 +- 13 files changed, 458 insertions(+), 18 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Lol.auto create mode 100644 src/main/deploy/pathplanner/autos/SHOOT_WAIT_AND_LEAVE_AMP.auto create mode 100644 src/main/deploy/pathplanner/paths/LeaveSubLeft.path create mode 100644 src/main/deploy/pathplanner/paths/Lol-Go.path create mode 100644 src/main/deploy/pathplanner/paths/Lol-Ready.path create mode 100644 src/main/deploy/pathplanner/paths/Score Source Side.path create mode 100644 src/main/deploy/pathplanner/paths/Yoink Note 2.path diff --git a/src/main/deploy/pathplanner/autos/HopesAndDreams.auto b/src/main/deploy/pathplanner/autos/HopesAndDreams.auto index 2379d6a..839536f 100644 --- a/src/main/deploy/pathplanner/autos/HopesAndDreams.auto +++ b/src/main/deploy/pathplanner/autos/HopesAndDreams.auto @@ -12,9 +12,22 @@ "data": { "commands": [ { - "type": "path", + "type": "parallel", "data": { - "pathName": "CollectMiddle" + "commands": [ + { + "type": "named", + "data": { + "name": "COLLECT" + } + }, + { + "type": "path", + "data": { + "pathName": "CollectMiddle" + } + } + ] } }, { @@ -24,9 +37,28 @@ } }, { - "type": "path", + "type": "named", + "data": { + "name": "SHOOT_SUB" + } + }, + { + "type": "parallel", "data": { - "pathName": "CollectLeft" + "commands": [ + { + "type": "path", + "data": { + "pathName": "CollectLeft" + } + }, + { + "type": "named", + "data": { + "name": "COLLECT" + } + } + ] } }, { @@ -36,9 +68,28 @@ } }, { - "type": "path", + "type": "named", + "data": { + "name": "SHOOT_SUB" + } + }, + { + "type": "parallel", "data": { - "pathName": "CollectRight" + "commands": [ + { + "type": "named", + "data": { + "name": "COLLECT" + } + }, + { + "type": "path", + "data": { + "pathName": "CollectRight" + } + } + ] } }, { @@ -48,9 +99,28 @@ } }, { - "type": "path", + "type": "named", + "data": { + "name": "SHOOT_SUB" + } + }, + { + "type": "parallel", "data": { - "pathName": "CollectCenterLineTop" + "commands": [ + { + "type": "named", + "data": { + "name": "COLLECT" + } + }, + { + "type": "path", + "data": { + "pathName": "CollectCenterLineTop" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/Lol.auto b/src/main/deploy/pathplanner/autos/Lol.auto new file mode 100644 index 0000000..b3ef1f8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Lol.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Lol-Ready" + } + }, + { + "type": "path", + "data": { + "pathName": "Lol-Go" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE.auto b/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE.auto index e14026b..8946dd6 100644 --- a/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE.auto +++ b/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.65, - "y": 4.5 + "x": 0.63, + "y": 6.63 }, - "rotation": 120.0 + "rotation": 100.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto b/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto index fb09456..2e855f7 100644 --- a/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto +++ b/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto @@ -18,15 +18,53 @@ } }, { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 2.0 + "commands": [ + { + "type": "named", + "data": { + "name": "COLLECT" + } + }, + { + "type": "path", + "data": { + "pathName": "LeaveSubRightCollectCenterLineBottom" + } + } + ] } }, { "type": "path", "data": { - "pathName": "LeaveSubRightCollectCenterLineBottom" + "pathName": "Score Source Side" + } + }, + { + "type": "named", + "data": { + "name": "SHOOT_SUB" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Yoink Note 2" + } + }, + { + "type": "named", + "data": { + "name": "COLLECT" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/SHOOT_WAIT_AND_LEAVE_AMP.auto b/src/main/deploy/pathplanner/autos/SHOOT_WAIT_AND_LEAVE_AMP.auto new file mode 100644 index 0000000..3851020 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SHOOT_WAIT_AND_LEAVE_AMP.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.63, + "y": 6.63 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SHOOT_SUB" + } + }, + { + "type": "wait", + "data": { + "waitTime": 8.0 + } + }, + { + "type": "path", + "data": { + "pathName": "LeaveSubLeft" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeaveSubLeft.path b/src/main/deploy/pathplanner/paths/LeaveSubLeft.path new file mode 100644 index 0000000..b39a97d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeaveSubLeft.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6325805940202477, + "y": 6.625287881448246 + }, + "prevControl": null, + "nextControl": { + "x": 0.4976965624321599, + "y": 7.3902532375505094 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.264755469108254, + "y": 7.3828571818217466 + }, + "prevControl": { + "x": 3.0499153615756893, + "y": 7.004072531634997 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.4153694792949, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -119.35775354279127, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Lol-Go.path b/src/main/deploy/pathplanner/paths/Lol-Go.path new file mode 100644 index 0000000..59becc6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Lol-Go.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.88, + "y": 7.4999360736976515 + }, + "prevControl": null, + "nextControl": { + "x": 7.9672288203636805, + "y": 6.67349683692656 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.022324769481754, + "y": 1.005501071404828 + }, + "prevControl": { + "x": 7.985587704329772, + "y": 2.2533902249012563 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 131.08175113593276, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 158.38783654677385, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Lol-Ready.path b/src/main/deploy/pathplanner/paths/Lol-Ready.path new file mode 100644 index 0000000..f4909b5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Lol-Ready.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.44663176574675223, + "y": 7.231343321747046 + }, + "prevControl": null, + "nextControl": { + "x": 1.4466317657467522, + "y": 7.231343321747046 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.877697903046813, + "y": 7.548145029175965 + }, + "prevControl": { + "x": 6.858422844362467, + "y": 7.5688060100952415 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 164.84593194968735, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Score Source Side.path b/src/main/deploy/pathplanner/paths/Score Source Side.path new file mode 100644 index 0000000..cf19313 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Score Source Side.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.180768240731112, + "y": 1.7768443590578453 + }, + "prevControl": null, + "nextControl": { + "x": 2.9534974506190617, + "y": 1.9421322064120636 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7221115113371159, + "y": 4.531641814961482 + }, + "prevControl": { + "x": 0.9907042632864136, + "y": 4.14597017113539 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.2913621709843, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -179.65691560045528, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Yoink Note 2.path b/src/main/deploy/pathplanner/paths/Yoink Note 2.path new file mode 100644 index 0000000..d5dfe30 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Yoink Note 2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.811599811119085, + "y": 1.5495735689457948 + }, + "prevControl": null, + "nextControl": { + "x": 6.8115998111190805, + "y": 1.5495735689457948 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.905245877605849, + "y": 2.4448827421144768 + }, + "prevControl": { + "x": 6.905245877605849, + "y": 2.4448827421144768 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.74535376772715, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 179.08089521628574, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/com/cyberknights4911/robot2024/Autos.java b/src/main/java/com/cyberknights4911/robot2024/Autos.java index fbf22f0..faffd1b 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Autos.java +++ b/src/main/java/com/cyberknights4911/robot2024/Autos.java @@ -67,6 +67,10 @@ public void addAllAutos(AutoCommandHandler handler) { handler.addDefaultOption("Nothing", Commands.none()); handler.addOption("Leave, Bro", new PathPlannerAuto("LEAVE")); handler.addOption("Score+Leave Source", new PathPlannerAuto("SHOOT_AND_LEAVE_SOURCE")); + handler.addOption("Score+Wait+Leave Amp", new PathPlannerAuto("SHOOT_WAIT_AND_LEAVE_AMP")); + handler.addOption("Two Piece Source", new PathPlannerAuto("SHOOT_AND_LEAVE_SOURCE_COLLECT")); + // handler.addOption("Hopez And Dreamz", new PathPlannerAuto("HopesAndDreams")); + // handler.addOption("Translate Test", new PathPlannerAuto("TranslationTest")); // addCharacterization("Climb", climb.getSysId(), handler); diff --git a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java index 0e39dd3..c4b92d1 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java +++ b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java @@ -167,9 +167,9 @@ public void onRobotPeriodic(LoggedRobot robot) { private Command scoreForAuto() { return shooter .aimSubwoofer() - .andThen(Commands.waitSeconds(.5)) + .andThen(Commands.waitSeconds(.3)) .andThen(shooter.fire()) - .andThen(Commands.waitSeconds(.5)); + .andThen(Commands.waitSeconds(.25)); } @Override diff --git a/src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java b/src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java index d166dd4..c1a71bf 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java +++ b/src/main/java/com/cyberknights4911/robot2024/Robot2024Constants.java @@ -139,8 +139,8 @@ private Robot2024Constants() {} .guideReversePercentOutput(-0.05) .feedTime(2) .aimTime(.5) - .speakerPositionDegrees(54) - .podiumPositionDegrees(34) + .speakerPositionDegrees(55) + .podiumPositionDegrees(38) // was 34, did not try 36, trying 38 .collectPositionDegrees(34) .firePercentOutput(.6) .shooterFeedBackValues(new PidValues(0.1, 0, 0)) From f45b6f483ea4daf8aa390a912d296a200fb3ae1d Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Sat, 16 Mar 2024 19:59:39 -0700 Subject: [PATCH 06/14] Move collect into collect paths --- .../pathplanner/autos/HopesAndDreams.auto | 68 +++---------------- .../autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto | 34 ++-------- .../paths/CollectCenterLineTop.path | 36 +++++++--- .../deploy/pathplanner/paths/CollectLeft.path | 28 ++++++-- .../pathplanner/paths/CollectMiddle.path | 24 ++++++- .../pathplanner/paths/CollectRight.path | 10 +-- .../LeaveSubRightCollectCenterLineBottom.path | 20 +++++- .../pathplanner/paths/Score Source Side.path | 10 +-- .../pathplanner/paths/Yoink Note 2.path | 40 ++++++++--- 9 files changed, 141 insertions(+), 129 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/HopesAndDreams.auto b/src/main/deploy/pathplanner/autos/HopesAndDreams.auto index 839536f..33442ab 100644 --- a/src/main/deploy/pathplanner/autos/HopesAndDreams.auto +++ b/src/main/deploy/pathplanner/autos/HopesAndDreams.auto @@ -12,22 +12,9 @@ "data": { "commands": [ { - "type": "parallel", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "COLLECT" - } - }, - { - "type": "path", - "data": { - "pathName": "CollectMiddle" - } - } - ] + "pathName": "CollectMiddle" } }, { @@ -43,22 +30,9 @@ } }, { - "type": "parallel", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CollectLeft" - } - }, - { - "type": "named", - "data": { - "name": "COLLECT" - } - } - ] + "pathName": "CollectLeft" } }, { @@ -74,22 +48,9 @@ } }, { - "type": "parallel", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "COLLECT" - } - }, - { - "type": "path", - "data": { - "pathName": "CollectRight" - } - } - ] + "pathName": "CollectRight" } }, { @@ -105,22 +66,9 @@ } }, { - "type": "parallel", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "COLLECT" - } - }, - { - "type": "path", - "data": { - "pathName": "CollectCenterLineTop" - } - } - ] + "pathName": "CollectCenterLineTop" } } ] diff --git a/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto b/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto index 2e855f7..37be3fa 100644 --- a/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto +++ b/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto @@ -18,22 +18,9 @@ } }, { - "type": "parallel", + "type": "path", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "COLLECT" - } - }, - { - "type": "path", - "data": { - "pathName": "LeaveSubRightCollectCenterLineBottom" - } - } - ] + "pathName": "LeaveSubRightCollectCenterLineBottom" } }, { @@ -49,22 +36,9 @@ } }, { - "type": "parallel", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Yoink Note 2" - } - }, - { - "type": "named", - "data": { - "name": "COLLECT" - } - } - ] + "pathName": "Yoink Note 2" } } ] diff --git a/src/main/deploy/pathplanner/paths/CollectCenterLineTop.path b/src/main/deploy/pathplanner/paths/CollectCenterLineTop.path index 0fe36d9..02fb6ae 100644 --- a/src/main/deploy/pathplanner/paths/CollectCenterLineTop.path +++ b/src/main/deploy/pathplanner/paths/CollectCenterLineTop.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.42181301812268, - "y": 6.687270824206078 + "x": 5.405267186373299, + "y": 7.196908353548251 }, "prevControl": null, "nextControl": { - "x": 5.0471435171058205, - "y": 6.825010697001261 + "x": 6.142175505827522, + "y": 7.313987245424156 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.822601953928739, - "y": 7.417292150020542 + "x": 7.8845848966865715, + "y": 7.403518162741023 }, "prevControl": { - "x": 6.810213888884153, - "y": 7.375970188181988 + "x": 6.872196831641985, + "y": 7.362196200902469 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "COLLECT", + "waypointRelativePos": 0.45, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "COLLECT" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/CollectLeft.path b/src/main/deploy/pathplanner/paths/CollectLeft.path index 9011d46..1bb3542 100644 --- a/src/main/deploy/pathplanner/paths/CollectLeft.path +++ b/src/main/deploy/pathplanner/paths/CollectLeft.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.2647980866431525, - "y": 6.1845202885036645 + "x": 1.9617703664937525, + "y": 5.895266555633783 }, "prevControl": null, "nextControl": { - "x": 2.567825806792553, - "y": 6.4531130404542685 + "x": 2.264798086643152, + "y": 6.163859307584387 }, "isLocked": false, "linkedName": null @@ -30,7 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "COLLECT", + "waypointRelativePos": 0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "COLLECT" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/CollectMiddle.path b/src/main/deploy/pathplanner/paths/CollectMiddle.path index f588534..a403042 100644 --- a/src/main/deploy/pathplanner/paths/CollectMiddle.path +++ b/src/main/deploy/pathplanner/paths/CollectMiddle.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.0444142901708617, + "x": 1.541663754468448, "y": 5.571577854565105 }, "prevControl": null, "nextControl": { - "x": 2.2678904149754606, + "x": 1.765139879273046, "y": 5.5274414307217885 }, "isLocked": false, @@ -30,7 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "COLLECT", + "waypointRelativePos": 0.15, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "COLLECT" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/CollectRight.path b/src/main/deploy/pathplanner/paths/CollectRight.path index bf34967..d3ada41 100644 --- a/src/main/deploy/pathplanner/paths/CollectRight.path +++ b/src/main/deploy/pathplanner/paths/CollectRight.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.693177614543148, - "y": 4.910426465148232 + "x": 1.713838595462425, + "y": 5.048166337943415 }, "prevControl": null, "nextControl": { - "x": 1.9097667457079872, - "y": 4.707889187590455 + "x": 1.930427726627264, + "y": 4.845629060385638 }, "isLocked": false, "linkedName": null @@ -33,7 +33,7 @@ "eventMarkers": [ { "name": "COLLECT", - "waypointRelativePos": 0, + "waypointRelativePos": 0.0, "command": { "type": "parallel", "data": { diff --git a/src/main/deploy/pathplanner/paths/LeaveSubRightCollectCenterLineBottom.path b/src/main/deploy/pathplanner/paths/LeaveSubRightCollectCenterLineBottom.path index 97f6307..945f11d 100644 --- a/src/main/deploy/pathplanner/paths/LeaveSubRightCollectCenterLineBottom.path +++ b/src/main/deploy/pathplanner/paths/LeaveSubRightCollectCenterLineBottom.path @@ -30,7 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.7, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "COLLECT" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Score Source Side.path b/src/main/deploy/pathplanner/paths/Score Source Side.path index cf19313..f758185 100644 --- a/src/main/deploy/pathplanner/paths/Score Source Side.path +++ b/src/main/deploy/pathplanner/paths/Score Source Side.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.180768240731112, - "y": 1.7768443590578453 + "x": 2.0375272965311026, + "y": 2.816780398661469 }, "prevControl": null, "nextControl": { - "x": 2.9534974506190617, - "y": 1.9421322064120636 + "x": 1.8102565064190523, + "y": 2.9820682460156873 }, "isLocked": false, "linkedName": null @@ -45,7 +45,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": -179.65691560045528, + "rotation": 142.0578466498811, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Yoink Note 2.path b/src/main/deploy/pathplanner/paths/Yoink Note 2.path index d5dfe30..12861f9 100644 --- a/src/main/deploy/pathplanner/paths/Yoink Note 2.path +++ b/src/main/deploy/pathplanner/paths/Yoink Note 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.811599811119085, - "y": 1.5495735689457948 + "x": 5.935565696634749, + "y": 1.5771215435048316 }, "prevControl": null, "nextControl": { - "x": 6.8115998111190805, - "y": 1.5495735689457948 + "x": 6.810213888884153, + "y": 1.6666524608216995 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.905245877605849, - "y": 2.4448827421144768 + "x": 7.939680845804644, + "y": 2.265820907480741 }, "prevControl": { - "x": 6.905245877605849, - "y": 2.4448827421144768 + "x": 7.278529456387772, + "y": 1.9972281555301359 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.2, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "COLLECT" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, @@ -39,13 +57,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 179.74535376772715, + "rotation": -161.5650511770781, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 179.08089521628574, + "rotation": -166.60750224624903, "velocity": 0 }, "useDefaultConstraints": true From 8a686f4fba116b653af4d84e34324ff3b4f2d02c Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Sat, 16 Mar 2024 23:21:24 -0700 Subject: [PATCH 07/14] rename --- .../autos/{HopesAndDreams.auto => HOPES_AND_DREAMS.auto} | 0 src/main/deploy/pathplanner/paths/CollectMiddle.path | 2 +- src/main/java/com/cyberknights4911/robot2024/Autos.java | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename src/main/deploy/pathplanner/autos/{HopesAndDreams.auto => HOPES_AND_DREAMS.auto} (100%) diff --git a/src/main/deploy/pathplanner/autos/HopesAndDreams.auto b/src/main/deploy/pathplanner/autos/HOPES_AND_DREAMS.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/HopesAndDreams.auto rename to src/main/deploy/pathplanner/autos/HOPES_AND_DREAMS.auto diff --git a/src/main/deploy/pathplanner/paths/CollectMiddle.path b/src/main/deploy/pathplanner/paths/CollectMiddle.path index a403042..218e867 100644 --- a/src/main/deploy/pathplanner/paths/CollectMiddle.path +++ b/src/main/deploy/pathplanner/paths/CollectMiddle.path @@ -33,7 +33,7 @@ "eventMarkers": [ { "name": "COLLECT", - "waypointRelativePos": 0.15, + "waypointRelativePos": 0.2, "command": { "type": "sequential", "data": { diff --git a/src/main/java/com/cyberknights4911/robot2024/Autos.java b/src/main/java/com/cyberknights4911/robot2024/Autos.java index faffd1b..6bbaaf6 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Autos.java +++ b/src/main/java/com/cyberknights4911/robot2024/Autos.java @@ -69,7 +69,7 @@ public void addAllAutos(AutoCommandHandler handler) { handler.addOption("Score+Leave Source", new PathPlannerAuto("SHOOT_AND_LEAVE_SOURCE")); handler.addOption("Score+Wait+Leave Amp", new PathPlannerAuto("SHOOT_WAIT_AND_LEAVE_AMP")); handler.addOption("Two Piece Source", new PathPlannerAuto("SHOOT_AND_LEAVE_SOURCE_COLLECT")); - // handler.addOption("Hopez And Dreamz", new PathPlannerAuto("HopesAndDreams")); + handler.addOption("Hopez And Dreamz", new PathPlannerAuto("HOPES_AND_DREAMS")); // handler.addOption("Translate Test", new PathPlannerAuto("TranslationTest")); From aa6913058752201152fc47f9a57751a1f1db1ea3 Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Sat, 16 Mar 2024 23:26:24 -0700 Subject: [PATCH 08/14] Add quick firing for autos --- .../java/com/cyberknights4911/robot2024/Robot2024.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java index c4b92d1..b9591b8 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java +++ b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java @@ -144,6 +144,7 @@ private Command collectNote() { .extendCollecter() .andThen(indexer.runIndexAtTunableOutput()) .andThen(shooter.collectAndWaitForNote()) + .andThen(rumbleQuickTwice()) .andThen(stowEverything()); } @@ -172,10 +173,17 @@ private Command scoreForAuto() { .andThen(Commands.waitSeconds(.25)); } + private Command quickscoreForAuto() { + return shooter.fire() + .andThen(Commands.waitSeconds(.25)); + } + @Override public void setupAutos(AutoCommandHandler handler) { NamedCommands.registerCommand("SHOOT_SUB", scoreForAuto()); NamedCommands.registerCommand("COLLECT", collectNote()); + NamedCommands.registerCommand("QUICK_SHOTE", quickscoreForAuto()); + NamedCommands.registerCommand("AIM_SUB", shooter.aimSubwoofer()); Autos autos = new Autos(Robot2024Constants.DRIVE_CONSTANTS, climb, collect, shooter, drive); @@ -286,7 +294,6 @@ private VisionSimple createVision() { drive::addVisionMeasurement, Robot2024Constants.CAMERA_CONSTANTS_FRONT_LEFT, Robot2024Constants.CAMERA_CONSTANTS_FRONT_RIGHT); - // TODO: add other camera after validationg front right one } private Command rumbleLongOnce() { From 581821a4b534e06c12469813943d441fd69c9a32 Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Sun, 17 Mar 2024 08:34:03 -0700 Subject: [PATCH 09/14] Reverse command --- .../com/cyberknights4911/control/ButtonAction.java | 3 ++- .../com/cyberknights4911/robot2024/Robot2024.java | 11 +++++++++-- .../cyberknights4911/robot2024/collect/Collect.java | 9 +++++++++ .../robot2024/control/ControllerBinding.java | 2 ++ .../cyberknights4911/robot2024/indexer/Indexer.java | 4 ++++ .../cyberknights4911/robot2024/shooter/Shooter.java | 4 ++++ 6 files changed, 30 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/cyberknights4911/control/ButtonAction.java b/src/main/java/com/cyberknights4911/control/ButtonAction.java index d1589d6..fc6af6a 100644 --- a/src/main/java/com/cyberknights4911/control/ButtonAction.java +++ b/src/main/java/com/cyberknights4911/control/ButtonAction.java @@ -22,5 +22,6 @@ public enum ButtonAction { ExtendClimber, AimSubwoofer, AimPodium, - AimAmp + AimAmp, + ReverseCollect } diff --git a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java index b9591b8..8f284ad 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java +++ b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java @@ -137,6 +137,10 @@ private void configureControls() { binding.triggersFor(ButtonAction.AimPodium).onTrue(shooter.aimPodium()); binding.triggersFor(ButtonAction.FireNoteSpeaker).onTrue(shooter.fire()); + binding + .triggersFor(ButtonAction.ReverseCollect) + .onTrue(reverseNote()) + .onFalse(stowEverything()); } private Command collectNote() { @@ -148,6 +152,10 @@ private Command collectNote() { .andThen(stowEverything()); } + private Command reverseNote() { + return collect.ejectNote().andThen(indexer.runBackwards()).andThen(shooter.runBackwards()); + } + private Command stowEverything() { return shooter.stow().alongWith(indexer.stop()).alongWith(collect.retractCollecter()); } @@ -174,8 +182,7 @@ private Command scoreForAuto() { } private Command quickscoreForAuto() { - return shooter.fire() - .andThen(Commands.waitSeconds(.25)); + return shooter.fire().andThen(Commands.waitSeconds(.25)); } @Override diff --git a/src/main/java/com/cyberknights4911/robot2024/collect/Collect.java b/src/main/java/com/cyberknights4911/robot2024/collect/Collect.java index 613f77e..3fa6682 100644 --- a/src/main/java/com/cyberknights4911/robot2024/collect/Collect.java +++ b/src/main/java/com/cyberknights4911/robot2024/collect/Collect.java @@ -147,6 +147,15 @@ public double getCharacterizationVelocity() { return inputs.collectVelocityRadPerSec; } + public Command ejectNote() { + return Commands.runOnce( + () -> { + collectIO.setCollecterPosition(true); + collectIO.setCollectOutput(-collectOutput.get()); + }, + this); + } + public Command extendCollecter() { return Commands.runOnce( () -> { diff --git a/src/main/java/com/cyberknights4911/robot2024/control/ControllerBinding.java b/src/main/java/com/cyberknights4911/robot2024/control/ControllerBinding.java index 0f11c2a..f729362 100644 --- a/src/main/java/com/cyberknights4911/robot2024/control/ControllerBinding.java +++ b/src/main/java/com/cyberknights4911/robot2024/control/ControllerBinding.java @@ -71,6 +71,8 @@ public Triggers triggersFor(ButtonAction action) { return new Triggers(operator.povRight()); case FireNoteSpeaker: return new Triggers(operator.rightTrigger()); + case ReverseCollect: + return new Triggers(operator.x()); default: return new Triggers(ALWAYS_FALSE); } diff --git a/src/main/java/com/cyberknights4911/robot2024/indexer/Indexer.java b/src/main/java/com/cyberknights4911/robot2024/indexer/Indexer.java index ddfa072..4d83691 100644 --- a/src/main/java/com/cyberknights4911/robot2024/indexer/Indexer.java +++ b/src/main/java/com/cyberknights4911/robot2024/indexer/Indexer.java @@ -82,4 +82,8 @@ public Command runIndexAtTunableOutput() { public Command stop() { return Commands.runOnce(() -> indexerIO.stop(), this); } + + public Command runBackwards() { + return Commands.runOnce(() -> runOutput(-percent.get()), this); + } } diff --git a/src/main/java/com/cyberknights4911/robot2024/shooter/Shooter.java b/src/main/java/com/cyberknights4911/robot2024/shooter/Shooter.java index aa21449..7ab3cb8 100644 --- a/src/main/java/com/cyberknights4911/robot2024/shooter/Shooter.java +++ b/src/main/java/com/cyberknights4911/robot2024/shooter/Shooter.java @@ -250,6 +250,10 @@ private Command backNoteUp() { .andThen(() -> stopGuide(), this); } + public Command runBackwards() { + return Commands.runOnce(() -> runGuideOutput(-guideOutput.get()), this); + } + public Command stow() { return Commands.runOnce( () -> { From 9f0ed23fdbbf0a57e18ae8967cf0695d144274c8 Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Sun, 17 Mar 2024 08:51:40 -0700 Subject: [PATCH 10/14] AUTOS --- .pathplanner/settings.json | 4 +- .../autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto | 14 ++++++- .../paths/CollectCenterLineBottom.path | 4 +- .../paths/CollectCenterLineSecond.path | 4 +- .../paths/CollectCenterLineTop.path | 4 +- .../deploy/pathplanner/paths/CollectLeft.path | 4 +- .../pathplanner/paths/CollectMiddle.path | 4 +- .../pathplanner/paths/CollectRight.path | 4 +- .../pathplanner/paths/LeaveSubLeft.path | 4 +- .../pathplanner/paths/LeaveSubRight.path | 4 +- .../LeaveSubRightCollectCenterLineBottom.path | 34 +++++++++++---- src/main/deploy/pathplanner/paths/Lol-Go.path | 4 +- .../deploy/pathplanner/paths/Lol-Ready.path | 4 +- .../pathplanner/paths/Score Source Side.path | 42 +++++++++++++------ .../pathplanner/paths/Yoink Note 2.path | 42 +++++++++++++------ 15 files changed, 119 insertions(+), 57 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 701c627..814b6b0 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,8 +4,8 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 4.5, + "defaultMaxAccel": 4.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto b/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto index 37be3fa..022a8e4 100644 --- a/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto +++ b/src/main/deploy/pathplanner/autos/SHOOT_AND_LEAVE_SOURCE_COLLECT.auto @@ -32,7 +32,7 @@ { "type": "named", "data": { - "name": "SHOOT_SUB" + "name": "QUICK_SHOOT" } }, { @@ -40,6 +40,18 @@ "data": { "pathName": "Yoink Note 2" } + }, + { + "type": "path", + "data": { + "pathName": "Score Source Side" + } + }, + { + "type": "named", + "data": { + "name": "QUICK_SHOOT" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/CollectCenterLineBottom.path b/src/main/deploy/pathplanner/paths/CollectCenterLineBottom.path index de100a6..92d0eb6 100644 --- a/src/main/deploy/pathplanner/paths/CollectCenterLineBottom.path +++ b/src/main/deploy/pathplanner/paths/CollectCenterLineBottom.path @@ -43,8 +43,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CollectCenterLineSecond.path b/src/main/deploy/pathplanner/paths/CollectCenterLineSecond.path index 22e543b..c4262b0 100644 --- a/src/main/deploy/pathplanner/paths/CollectCenterLineSecond.path +++ b/src/main/deploy/pathplanner/paths/CollectCenterLineSecond.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CollectCenterLineTop.path b/src/main/deploy/pathplanner/paths/CollectCenterLineTop.path index 02fb6ae..119363d 100644 --- a/src/main/deploy/pathplanner/paths/CollectCenterLineTop.path +++ b/src/main/deploy/pathplanner/paths/CollectCenterLineTop.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CollectLeft.path b/src/main/deploy/pathplanner/paths/CollectLeft.path index 1bb3542..26fa589 100644 --- a/src/main/deploy/pathplanner/paths/CollectLeft.path +++ b/src/main/deploy/pathplanner/paths/CollectLeft.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CollectMiddle.path b/src/main/deploy/pathplanner/paths/CollectMiddle.path index 218e867..50cf8dc 100644 --- a/src/main/deploy/pathplanner/paths/CollectMiddle.path +++ b/src/main/deploy/pathplanner/paths/CollectMiddle.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CollectRight.path b/src/main/deploy/pathplanner/paths/CollectRight.path index d3ada41..756f85f 100644 --- a/src/main/deploy/pathplanner/paths/CollectRight.path +++ b/src/main/deploy/pathplanner/paths/CollectRight.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/LeaveSubLeft.path b/src/main/deploy/pathplanner/paths/LeaveSubLeft.path index b39a97d..0914ae5 100644 --- a/src/main/deploy/pathplanner/paths/LeaveSubLeft.path +++ b/src/main/deploy/pathplanner/paths/LeaveSubLeft.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/LeaveSubRight.path b/src/main/deploy/pathplanner/paths/LeaveSubRight.path index 1bee8ae..331c91c 100644 --- a/src/main/deploy/pathplanner/paths/LeaveSubRight.path +++ b/src/main/deploy/pathplanner/paths/LeaveSubRight.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/LeaveSubRightCollectCenterLineBottom.path b/src/main/deploy/pathplanner/paths/LeaveSubRightCollectCenterLineBottom.path index 945f11d..73821f0 100644 --- a/src/main/deploy/pathplanner/paths/LeaveSubRightCollectCenterLineBottom.path +++ b/src/main/deploy/pathplanner/paths/LeaveSubRightCollectCenterLineBottom.path @@ -8,20 +8,36 @@ }, "prevControl": null, "nextControl": { - "x": 1.4552880713777554, - "y": 3.5413650532048258 + "x": 1.8378044809780891, + "y": 3.023390207854241 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.953454833084162, - "y": 0.7644562940132597 + "x": 3.0705763424949666, + "y": 2.155629009244595 }, "prevControl": { - "x": 5.660085951044385, - "y": 0.998614077765068 + "x": 1.75516055730098, + "y": 2.679040525866286 + }, + "nextControl": { + "x": 4.888676252344807, + "y": 1.4321965843828788 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.919019864885367, + "y": 0.7988912622120541 + }, + "prevControl": { + "x": 5.742729874721494, + "y": 1.3980597088710944 }, "nextControl": null, "isLocked": false, @@ -33,7 +49,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.7, + "waypointRelativePos": 1.6, "command": { "type": "sequential", "data": { @@ -50,8 +66,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Lol-Go.path b/src/main/deploy/pathplanner/paths/Lol-Go.path index 59becc6..f99a2a5 100644 --- a/src/main/deploy/pathplanner/paths/Lol-Go.path +++ b/src/main/deploy/pathplanner/paths/Lol-Go.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Lol-Ready.path b/src/main/deploy/pathplanner/paths/Lol-Ready.path index f4909b5..f394fd7 100644 --- a/src/main/deploy/pathplanner/paths/Lol-Ready.path +++ b/src/main/deploy/pathplanner/paths/Lol-Ready.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Score Source Side.path b/src/main/deploy/pathplanner/paths/Score Source Side.path index f758185..b8febc5 100644 --- a/src/main/deploy/pathplanner/paths/Score Source Side.path +++ b/src/main/deploy/pathplanner/paths/Score Source Side.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0375272965311026, - "y": 2.816780398661469 + "x": 5.839147785678121, + "y": 1.267206829715673 }, "prevControl": null, "nextControl": { - "x": 1.8102565064190523, - "y": 2.9820682460156873 + "x": 3.511343935439548, + "y": 1.9421322064120636 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.7221115113371159, - "y": 4.531641814961482 + "x": 0.653241574939525, + "y": 4.510980834042205 }, "prevControl": { - "x": 0.9907042632864136, - "y": 4.14597017113539 + "x": 1.8378044809780891, + "y": 2.0178891364494147 }, "nextControl": null, "isLocked": false, @@ -30,10 +30,28 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.65, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AIM_SUB" + } + } + ] + } + } + } + ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -45,7 +63,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": 142.0578466498811, + "rotation": 128.85337434035333, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/Yoink Note 2.path b/src/main/deploy/pathplanner/paths/Yoink Note 2.path index 12861f9..5db0eee 100644 --- a/src/main/deploy/pathplanner/paths/Yoink Note 2.path +++ b/src/main/deploy/pathplanner/paths/Yoink Note 2.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 5.935565696634749, - "y": 1.5771215435048316 + "x": 2.843305552382916, + "y": 2.4173347675554413 }, "prevControl": null, "nextControl": { - "x": 6.810213888884153, - "y": 1.6666524608216995 + "x": 3.5388919099985845, + "y": 2.0936460664867638 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.939680845804644, - "y": 2.265820907480741 + "x": 5.129787440782935, + "y": 1.7561833781385674 }, "prevControl": { - "x": 7.278529456387772, - "y": 1.9972281555301359 + "x": 4.519025516143855, + "y": 1.7526933099977722 + }, + "nextControl": { + "x": 6.335011327740776, + "y": 1.7630703717783276 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.877697903046813, + "y": 2.4173347675554413 + }, + "prevControl": { + "x": 6.885970818921503, + "y": 1.7561833781385674 }, "nextControl": null, "isLocked": false, @@ -33,7 +49,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.2, + "waypointRelativePos": 1.25, "command": { "type": "sequential", "data": { @@ -50,20 +66,20 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 4.5, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": -161.5650511770781, + "rotation": -175.10090754621214, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": -166.60750224624903, + "rotation": 138.14495746469808, "velocity": 0 }, "useDefaultConstraints": true From 0f32d2fddf1006181fc78886e69fa1454496696d Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Sun, 17 Mar 2024 09:43:41 -0700 Subject: [PATCH 11/14] speed up --- .../java/com/cyberknights4911/robot2024/Robot2024.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java index 8f284ad..b2790f5 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Robot2024.java +++ b/src/main/java/com/cyberknights4911/robot2024/Robot2024.java @@ -176,20 +176,20 @@ public void onRobotPeriodic(LoggedRobot robot) { private Command scoreForAuto() { return shooter .aimSubwoofer() - .andThen(Commands.waitSeconds(.3)) + .andThen(Commands.waitSeconds(.2)) .andThen(shooter.fire()) - .andThen(Commands.waitSeconds(.25)); + .andThen(Commands.waitSeconds(.2)); } private Command quickscoreForAuto() { - return shooter.fire().andThen(Commands.waitSeconds(.25)); + return shooter.fire().andThen(Commands.waitSeconds(.1)); } @Override public void setupAutos(AutoCommandHandler handler) { NamedCommands.registerCommand("SHOOT_SUB", scoreForAuto()); NamedCommands.registerCommand("COLLECT", collectNote()); - NamedCommands.registerCommand("QUICK_SHOTE", quickscoreForAuto()); + NamedCommands.registerCommand("QUICK_SHOOT", quickscoreForAuto()); NamedCommands.registerCommand("AIM_SUB", shooter.aimSubwoofer()); Autos autos = new Autos(Robot2024Constants.DRIVE_CONSTANTS, climb, collect, shooter, drive); From 453fca7dc8625a5c8e82f498a1dc4d19954beda4 Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Sun, 17 Mar 2024 10:35:41 -0700 Subject: [PATCH 12/14] Clean up logging for debugging purposes --- .../com/cyberknights4911/drive/PointToAngleDrive.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/cyberknights4911/drive/PointToAngleDrive.java b/src/main/java/com/cyberknights4911/drive/PointToAngleDrive.java index 044944e..e7dcfd6 100644 --- a/src/main/java/com/cyberknights4911/drive/PointToAngleDrive.java +++ b/src/main/java/com/cyberknights4911/drive/PointToAngleDrive.java @@ -33,6 +33,8 @@ static PointToAngleDrive createDriveFacingPoint( DoubleSupplier ySupplier, double x, double y) { + Logger.recordOutput("Drive/PointAt/XDesired", x); + Logger.recordOutput("Drive/PointAt/yDesired", y); DoubleSupplier angleSupplier = () -> { @@ -42,7 +44,6 @@ static PointToAngleDrive createDriveFacingPoint( if (currentX > x) { desiredAngle += Math.PI; } - Logger.recordOutput("Drive/PointAt/Desired", desiredAngle); return desiredAngle; }; return new PointToAngleDrive(drive, controlConstants, xSupplier, ySupplier, angleSupplier); @@ -75,9 +76,9 @@ private PointToAngleDrive( double desiredAngle = desiredAngleSupplier.getAsDouble(); double output = pointController.calculate(currentRotation, desiredAngle); output = MathUtil.clamp(output, -1, 1); - Logger.recordOutput("Drive/PointAt/Current", currentRotation); - Logger.recordOutput("Drive/PointAt/Desired", desiredAngle); - Logger.recordOutput("Drive/PointAt/OmegaOutput", output); + Logger.recordOutput("Drive/PointAt/AngleCurrent", currentRotation); + Logger.recordOutput("Drive/PointAt/AngleDesired", desiredAngle); + // Logger.recordOutput("Drive/PointAt/OmegaOutput", output); return output; }; From e8ecb6295c5d3f0991adc302a39c6d628c22710d Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Sun, 17 Mar 2024 10:39:04 -0700 Subject: [PATCH 13/14] Better logging --- .../drive/PointToAngleDrive.java | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/cyberknights4911/drive/PointToAngleDrive.java b/src/main/java/com/cyberknights4911/drive/PointToAngleDrive.java index e7dcfd6..9e40131 100644 --- a/src/main/java/com/cyberknights4911/drive/PointToAngleDrive.java +++ b/src/main/java/com/cyberknights4911/drive/PointToAngleDrive.java @@ -11,6 +11,9 @@ import com.cyberknights4911.logging.LoggedTunableNumber; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -33,8 +36,8 @@ static PointToAngleDrive createDriveFacingPoint( DoubleSupplier ySupplier, double x, double y) { - Logger.recordOutput("Drive/PointAt/XDesired", x); - Logger.recordOutput("Drive/PointAt/yDesired", y); + Logger.recordOutput( + "Drive/PointAt/Desired", new Pose2d(new Translation2d(x, y), new Rotation2d())); DoubleSupplier angleSupplier = () -> { @@ -76,9 +79,16 @@ private PointToAngleDrive( double desiredAngle = desiredAngleSupplier.getAsDouble(); double output = pointController.calculate(currentRotation, desiredAngle); output = MathUtil.clamp(output, -1, 1); - Logger.recordOutput("Drive/PointAt/AngleCurrent", currentRotation); - Logger.recordOutput("Drive/PointAt/AngleDesired", desiredAngle); - // Logger.recordOutput("Drive/PointAt/OmegaOutput", output); + Logger.recordOutput( + "Drive/PointAt/AngleCurrent", + new Pose2d( + new Translation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()), + new Rotation2d(currentRotation))); + Logger.recordOutput( + "Drive/PointAt/AngleDesired", + new Pose2d( + new Translation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble()), + new Rotation2d(desiredAngle))); return output; }; From 5c89cf56d8b5786ec5c378e6a6d892f938c04d54 Mon Sep 17 00:00:00 2001 From: Riley Brewer Date: Wed, 20 Mar 2024 16:14:09 -0700 Subject: [PATCH 14/14] Last autos --- .../autos/FINALSAndIDidntStudy.auto | 55 +++++++++++++++ .../autos/SHOOT_WAIT_AND_LEAVE_AMP.auto | 8 +-- .../deploy/pathplanner/paths/EnglishExam.path | 70 +++++++++++++++++++ .../pathplanner/paths/LeaveSubLeft.path | 38 +++++++--- .../deploy/pathplanner/paths/MathExam.path | 70 +++++++++++++++++++ .../pathplanner/paths/SpanishFinal.path | 70 +++++++++++++++++++ .../com/cyberknights4911/robot2024/Autos.java | 13 ++-- 7 files changed, 305 insertions(+), 19 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/FINALSAndIDidntStudy.auto create mode 100644 src/main/deploy/pathplanner/paths/EnglishExam.path create mode 100644 src/main/deploy/pathplanner/paths/MathExam.path create mode 100644 src/main/deploy/pathplanner/paths/SpanishFinal.path diff --git a/src/main/deploy/pathplanner/autos/FINALSAndIDidntStudy.auto b/src/main/deploy/pathplanner/autos/FINALSAndIDidntStudy.auto new file mode 100644 index 0000000..371fb31 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FINALSAndIDidntStudy.auto @@ -0,0 +1,55 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.3915358166286795, + "y": 1.990341161890377 + }, + "rotation": 178.0585136090857 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "EnglishExam" + } + }, + { + "type": "named", + "data": { + "name": "QUICK_SHOOT" + } + }, + { + "type": "path", + "data": { + "pathName": "MathExam" + } + }, + { + "type": "path", + "data": { + "pathName": "SpanishFinal" + } + }, + { + "type": "named", + "data": { + "name": "QUICK_SHOOT" + } + }, + { + "type": "path", + "data": { + "pathName": "Yoink Note 2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SHOOT_WAIT_AND_LEAVE_AMP.auto b/src/main/deploy/pathplanner/autos/SHOOT_WAIT_AND_LEAVE_AMP.auto index 3851020..e1baa13 100644 --- a/src/main/deploy/pathplanner/autos/SHOOT_WAIT_AND_LEAVE_AMP.auto +++ b/src/main/deploy/pathplanner/autos/SHOOT_WAIT_AND_LEAVE_AMP.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.63, - "y": 6.63 + "x": 1.2799579961576024, + "y": 5.61289981640366 }, - "rotation": -120.0 + "rotation": -179.18154453831133 }, "command": { "type": "sequential", @@ -20,7 +20,7 @@ { "type": "wait", "data": { - "waitTime": 8.0 + "waitTime": 10.0 } }, { diff --git a/src/main/deploy/pathplanner/paths/EnglishExam.path b/src/main/deploy/pathplanner/paths/EnglishExam.path new file mode 100644 index 0000000..cc89572 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/EnglishExam.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9080603396106114, + "y": 2.4242217611952004 + }, + "prevControl": null, + "nextControl": { + "x": 0.9218343268864185, + "y": 3.181791061566694 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7772074604551887, + "y": 4.517867827681964 + }, + "prevControl": { + "x": 0.9011733459708525, + "y": 3.491705775357859 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.1, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AIM_SUB" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.06858282186245, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 173.90722834266396, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeaveSubLeft.path b/src/main/deploy/pathplanner/paths/LeaveSubLeft.path index 0914ae5..1beed73 100644 --- a/src/main/deploy/pathplanner/paths/LeaveSubLeft.path +++ b/src/main/deploy/pathplanner/paths/LeaveSubLeft.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.6325805940202477, - "y": 6.625287881448246 + "x": 1.2799579961576024, + "y": 5.61289981640366 }, "prevControl": null, "nextControl": { - "x": 0.4976965624321599, - "y": 7.3902532375505094 + "x": 1.2524100215985658, + "y": 8.15420046947477 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.264755469108254, - "y": 7.3828571818217466 + "x": 7.919019864885367, + "y": 7.424179143660301 }, "prevControl": { - "x": 3.0499153615756893, - "y": 7.004072531634997 + "x": 3.704179757352803, + "y": 7.045394493473552 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,25 @@ ], "rotationTargets": [], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.8, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "COLLECT" + } + } + ] + } + } + } + ], "globalConstraints": { "maxVelocity": 4.5, "maxAcceleration": 4.0, @@ -45,7 +63,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": -119.35775354279127, + "rotation": -178.51213247117235, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/deploy/pathplanner/paths/MathExam.path b/src/main/deploy/pathplanner/paths/MathExam.path new file mode 100644 index 0000000..2915628 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MathExam.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.5058428640347206, + "y": 1.0123880650445858 + }, + "prevControl": null, + "nextControl": { + "x": 3.5058428640347192, + "y": 1.0123880650445858 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.919019864885367, + "y": 0.7506823067337407 + }, + "prevControl": { + "x": 6.919019864885367, + "y": 0.7506823067337407 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "COLLECT" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 179.8488241515751, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 178.43064519525095, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SpanishFinal.path b/src/main/deploy/pathplanner/paths/SpanishFinal.path new file mode 100644 index 0000000..c18b4d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SpanishFinal.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.8501925460226754, + "y": 1.8663752763747146 + }, + "prevControl": null, + "nextControl": { + "x": 2.05818827745038, + "y": 2.8581023605000224 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7772074604551887, + "y": 4.517867827681964 + }, + "prevControl": { + "x": 1.693177614543148, + "y": 3.3815138771217144 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AIM_SUB" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.5, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.06858282186245, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 173.90722834266396, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/com/cyberknights4911/robot2024/Autos.java b/src/main/java/com/cyberknights4911/robot2024/Autos.java index 6bbaaf6..0451361 100644 --- a/src/main/java/com/cyberknights4911/robot2024/Autos.java +++ b/src/main/java/com/cyberknights4911/robot2024/Autos.java @@ -64,12 +64,15 @@ public Autos( } public void addAllAutos(AutoCommandHandler handler) { - handler.addDefaultOption("Nothing", Commands.none()); + handler.addDefaultOption( + "Score+Wait+Leave Amp", new PathPlannerAuto("SHOOT_WAIT_AND_LEAVE_AMP")); handler.addOption("Leave, Bro", new PathPlannerAuto("LEAVE")); - handler.addOption("Score+Leave Source", new PathPlannerAuto("SHOOT_AND_LEAVE_SOURCE")); - handler.addOption("Score+Wait+Leave Amp", new PathPlannerAuto("SHOOT_WAIT_AND_LEAVE_AMP")); - handler.addOption("Two Piece Source", new PathPlannerAuto("SHOOT_AND_LEAVE_SOURCE_COLLECT")); - handler.addOption("Hopez And Dreamz", new PathPlannerAuto("HOPES_AND_DREAMS")); + handler.addOption("Finals and I didnt study", new PathPlannerAuto("FINALSAndIDidntStudy")); + + handler.addOption("Nothing", Commands.none()); + // handler.addOption("Score+Leave Source", new PathPlannerAuto("SHOOT_AND_LEAVE_SOURCE")); + // handler.addOption("Two Piece Source", new PathPlannerAuto("SHOOT_AND_LEAVE_SOURCE_COLLECT")); + // handler.addOption("Hopez And Dreamz", new PathPlannerAuto("HOPES_AND_DREAMS")); // handler.addOption("Translate Test", new PathPlannerAuto("TranslationTest"));