From e76b92b793c93a8b42f1267fcc65eb2aced79bbd Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Sat, 12 Jul 2025 19:40:30 -0400 Subject: [PATCH 01/38] Improve 3d algae tracking - Use Autopilot for speed calculations, better than PID - Less nesting in CommandSwerveDrivetrain.followObject(), but still a lot (may want to consider changing if possible). - Compensate for camera position not at origin of robot. --- ThriftyTest/simgui.json | 16 ++--- .../java/frc/robot/binding/DriveBindings.java | 6 -- .../drivetrain/CommandSwerveDrivetrain.java | 66 +++++++++++-------- .../frc/robot/utils/MonitoredSupplier.java | 36 ---------- .../java/frc/robot/utils/OnboardLogger.java | 40 +++++------ .../robot/vision/tracking/AlgaeTracker.java | 33 +++++++--- .../vision/tracking/TrackingConstants.java | 4 +- 7 files changed, 90 insertions(+), 111 deletions(-) delete mode 100644 ThriftyTest/src/main/java/frc/robot/utils/MonitoredSupplier.java diff --git a/ThriftyTest/simgui.json b/ThriftyTest/simgui.json index c7dcf2a3..031d1437 100644 --- a/ThriftyTest/simgui.json +++ b/ThriftyTest/simgui.json @@ -25,6 +25,7 @@ "/SmartDashboard/Elevator/Subsystem": "Subsystem", "/SmartDashboard/PathPlanner": "Alerts", "/SmartDashboard/PhotonAlerts": "Alerts", + "/SmartDashboard/Prep/Set Angle": "Command", "/SmartDashboard/Prep/Set Center": "Command", "/SmartDashboard/Prep/Set Left": "Command", "/SmartDashboard/Prep/Set Right": "Command", @@ -54,16 +55,6 @@ "visible": true } }, - "/SmartDashboard/Prep/Set Left": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Prep/Set Right": { - "window": { - "visible": true - } - }, "/SmartDashboard/Super Field": { "Algae": { "image": "../resources/algae.jpg", @@ -203,6 +194,11 @@ "window": { "visible": true } + }, + "/SmartDashboard/Test/Follow Algae": { + "window": { + "visible": true + } } } }, diff --git a/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java index 1799ff09..7a9d92be 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java @@ -1,14 +1,10 @@ package frc.robot.binding; import java.util.function.DoubleSupplier; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Constants.FieldConstants; import frc.robot.binding.BindingConstants.Driver; -import com.therekrab.autopilot.APTarget; import frc.robot.superstructure.Superstructure; -import frc.robot.superstructure.states.Align; import frc.robot.superstructure.states.DeferredAlign; import frc.robot.superstructure.states.DeferredAlign.AlignLocation; import frc.robot.superstructure.states.HeadingReset; @@ -35,8 +31,6 @@ public void bind(Superstructure superstructure) { superstructure.setDrive(superstructure.enterWithoutProxy(new TeleopDrive(m_x, m_y, m_rot))); m_resetHeading.onTrue(superstructure.enter(new HeadingReset())); - // m_smartAlign.and(superstructure.holdingAlgae()).whileTrue(superstructure.enter(new Align( - // new APTarget(FieldConstants.k_processor).withEntryAngle(Rotation2d.kCW_Pi_2)))); m_smartAlign.and(superstructure.holdingAlgae().negate()).whileTrue(superstructure.enter( new DeferredAlign(AlignLocation.Center))); m_leftAlign.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Left))); diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 3eda403f..4238ab04 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.drivetrain; import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Volts; import java.io.IOException; @@ -25,7 +24,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.util.DriveFeedforwards; -import com.pathplanner.lib.util.FlippingUtil; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.swerve.SwerveSetpoint; import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; @@ -40,7 +38,6 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; @@ -128,6 +125,7 @@ public CommandSwerveDrivetrain( m_ologger.registerDouble("Velocity", this::getVelocity); m_ologger.registerPose("Estimated Pose", this::getPose); m_ologger.registerBoolean("Received Vision Update", () -> m_hasReceivedVisionUpdate); + m_ologger.registerBoolean("Valid Object Estimate", seesAlgae()); RobotObserver.setVelocitySupplier(this::getVelocity); RobotObserver.setNoElevatorZoneSupplier(dangerZone()); @@ -221,7 +219,8 @@ private void setPose(Pose2d pose) { } public Command setLocalHeading(Rotation2d heading) { - return Commands.runOnce(() -> resetRotation(FieldUtils.getLocalRotation(heading))).ignoringDisable(true); + return Commands.runOnce(() -> resetRotation(FieldUtils.getLocalRotation(heading))) + .ignoringDisable(true); } private ChassisSpeeds getRobotRelativeSpeeds() { @@ -402,6 +401,10 @@ public Trigger inReefZone() { }); } + public Trigger seesAlgae() { + return new Trigger(() -> m_objectStatus.isPresent()); + } + /** * Drives the robot from given x, y, and rotatational velocity suppliers. */ @@ -441,6 +444,14 @@ private AngularVelocity getMaxRotationalRate() { } } + private void setVelocity(Transform2d goal) { + setControl(m_veloRequest + .withVelocityX(goal.getX()) + .withVelocityY(goal.getY()) + .withTargetDirection(goal.getRotation()) + .withMaxAbsRotationalRate(getMaxRotationalRate())); + } + /** * Drives to a certain point on the field */ @@ -453,11 +464,7 @@ public Command align(Autopilot autopilot, APTarget target) { run(() -> { Translation2d velocities = getVelocityComponents(); Transform2d output = autopilot.calculate(m_estimatedPose, velocities, target); - setControl(m_veloRequest - .withVelocityX(output.getX()) - .withVelocityY(output.getY()) - .withTargetDirection(output.getRotation()) - .withMaxAbsRotationalRate(getMaxRotationalRate())); + setVelocity(output); })) .until(() -> { return autopilot.atTarget(m_estimatedPose, target); @@ -477,34 +484,37 @@ public Command seedLocal(Pose2d pose) { public Command followObject() { return run(new Runnable() { private final PIDController thetaController = new PIDController(8, 0, 0); - private final PIDController driveController = new PIDController(1, 0, 0); @Override public void run() { - m_objectStatus.ifPresentOrElse(status -> { - double output = -this.thetaController.calculate(status.yaw().getRadians(), 0); + if (m_objectStatus.isEmpty()) { + stop(); + return; + } + ObjectTrackingStatus status = m_objectStatus.get(); + if (status.pose().isEmpty()) { + // Drive towards algae. + // We invert the yaw because the input is actually the angle is from the robot to the + // target, so turning in that direction is good. (basically our input is funny). + double rotationalRate = thetaController.calculate(-status.yaw().getRadians(), 0); double speed = DriveConstants.kObjectTrackSpeed.in(MetersPerSecond); - if (status.pose().isPresent()) { - double distance = m_estimatedPose - .relativeTo(status.pose().get()) - .getTranslation() - .getNorm(); - speed = Math.min( - DriveConstants.kMaxObjectTrackingSpeed.in(MetersPerSecond), - -this.driveController.calculate(distance, 0)); - } - /* - * we invert the calculation because it outputs a negative number, because measurement > - * ref, so err < 0 - */ double vx = speed * status.yaw().getCos(); double vy = speed * status.yaw().getSin(); setControl(m_trackingRequest - .withRotationalRate(output) + .withRotationalRate(rotationalRate) .withVelocityX(vx) .withVelocityY(vy)); - }, () -> stop()); + return; + } + // If we have a pose estimate, for algae, use Autopilot to go there. + APTarget target = new APTarget(status.pose().get() + .transformBy(new Transform2d(Translation2d.kZero, status.yaw()))); + Transform2d output = DriveConstants.kTightAutopilot.calculate( + m_estimatedPose, + getVelocityComponents(), + target); + setVelocity(output); } - }); + }).onlyWhile(seesAlgae()); } } diff --git a/ThriftyTest/src/main/java/frc/robot/utils/MonitoredSupplier.java b/ThriftyTest/src/main/java/frc/robot/utils/MonitoredSupplier.java deleted file mode 100644 index 4ce161dd..00000000 --- a/ThriftyTest/src/main/java/frc/robot/utils/MonitoredSupplier.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.utils; - -import java.util.function.BooleanSupplier; -import java.util.function.Consumer; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - -public class MonitoredSupplier implements Supplier { - private T m_prev; - - private final Supplier m_supplier; - - public MonitoredSupplier(Supplier supplier) { - m_supplier = supplier; - } - - public void ifChanged(Consumer action) { - T curr = m_supplier.get(); - if (!curr.equals(m_prev)) { - m_prev = curr; - action.accept(curr); - } - } - - public static MonitoredSupplier fromDoubleSuplier(DoubleSupplier supplier) { - return new MonitoredSupplier<>(supplier::getAsDouble); - } - - public static MonitoredSupplier fromBooleanSupplier(BooleanSupplier supplier) { - return new MonitoredSupplier(supplier::getAsBoolean); - } - - public T get() { - return m_supplier.get(); - } -} diff --git a/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java b/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java index 5f2add24..4c55a4e6 100644 --- a/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java +++ b/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java @@ -24,11 +24,11 @@ public class OnboardLogger { private final String m_name; - private final List, DoubleLogEntry>> m_doubleEntries; - private final List, BooleanLogEntry>> m_booleanEntries; - private final List, StringLogEntry>> m_stringEntries; - private final List, StructLogEntry>> m_poseEntries; - private final List, StructArrayLogEntry>> m_poseListEntries; + private final List, DoubleLogEntry>> m_doubleEntries; + private final List, BooleanLogEntry>> m_booleanEntries; + private final List, StringLogEntry>> m_stringEntries; + private final List, StructLogEntry>> m_poseEntries; + private final List, StructArrayLogEntry>> m_poseListEntries; public OnboardLogger(String name) { m_name = name; @@ -41,46 +41,46 @@ public OnboardLogger(String name) { public void registerDouble(String name, DoubleSupplier supplier) { DoubleLogEntry entry = new DoubleLogEntry(dataLog, m_name + "/" + name); - m_doubleEntries.add(new Pair<>(MonitoredSupplier.fromDoubleSuplier(supplier), entry)); + m_doubleEntries.add(new Pair<>(supplier::getAsDouble, entry)); } public void registerBoolean(String name, BooleanSupplier supplier) { BooleanLogEntry entry = new BooleanLogEntry(dataLog, m_name + "/" + name); - m_booleanEntries.add(new Pair<>(MonitoredSupplier.fromBooleanSupplier(supplier), entry)); + m_booleanEntries.add(new Pair<>(supplier::getAsBoolean, entry)); } public void registerString(String name, Supplier supplier) { StringLogEntry entry = new StringLogEntry(dataLog, m_name + "/" + name); - m_stringEntries.add(new Pair<>(new MonitoredSupplier<>(supplier), entry)); + m_stringEntries.add(new Pair<>(supplier, entry)); } public void registerPose(String name, Supplier supplier) { StructLogEntry entry = StructLogEntry.create(dataLog, m_name + "/" + name, Pose2d.struct); - m_poseEntries.add(new Pair<>(new MonitoredSupplier<>(supplier), entry)); + m_poseEntries.add(new Pair<>(supplier, entry)); } public void registerPoses(String name, Supplier supplier) { StructArrayLogEntry entry = StructArrayLogEntry.create(dataLog, m_name + "/" + name, Pose2d.struct); - m_poseListEntries.add(new Pair<>(new MonitoredSupplier<>(supplier), entry)); + m_poseListEntries.add(new Pair<>(supplier, entry)); } public void log() { - for (Pair, DoubleLogEntry> pair : m_doubleEntries) { - pair.getFirst().ifChanged(d -> pair.getSecond().append(d)); + for (Pair, DoubleLogEntry> pair : m_doubleEntries) { + pair.getSecond().update(pair.getFirst().get()); } - for (Pair, BooleanLogEntry> pair : m_booleanEntries) { - pair.getFirst().ifChanged(b -> pair.getSecond().append(b)); + for (Pair, BooleanLogEntry> pair : m_booleanEntries) { + pair.getSecond().update(pair.getFirst().get()); } - for (Pair, StringLogEntry> pair : m_stringEntries) { - pair.getFirst().ifChanged(s -> pair.getSecond().append(s)); + for (Pair, StringLogEntry> pair : m_stringEntries) { + pair.getSecond().update(pair.getFirst().get()); } - for (Pair, StructLogEntry> pair : m_poseEntries) { - pair.getFirst().ifChanged(p -> pair.getSecond().append(p)); + for (Pair, StructLogEntry> pair : m_poseEntries) { + pair.getSecond().update(pair.getFirst().get()); } - for (Pair, StructArrayLogEntry> pair : m_poseListEntries) { - pair.getFirst().ifChanged(ps -> pair.getSecond().append(ps)); + for (Pair, StructArrayLogEntry> pair : m_poseListEntries) { + pair.getSecond().update(pair.getFirst().get()); } } } diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java index c987005b..abdad8a4 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java @@ -21,6 +21,7 @@ import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.Timer; import frc.robot.Robot; +import frc.robot.utils.OnboardLogger; import frc.robot.vision.CameraIO; import frc.robot.vision.CameraIO.CameraIOInputs; import frc.robot.vision.CameraIOHardware; @@ -28,6 +29,8 @@ public class AlgaeTracker implements Runnable { + private final OnboardLogger m_ologger = new OnboardLogger("Tracking"); + public static final boolean enabled = TrackingConstants.kEnabled; public record ObjectTrackingStatus(Rotation2d yaw, double time, Optional pose) { @@ -47,6 +50,8 @@ public boolean isExpired() { private final Supplier m_robotPose; + private Pose2d m_lastSeenAlgae = Pose2d.kZero; + public AlgaeTracker(Supplier robotPose, Consumer action) { if (Robot.isSimulation()) { m_io = new CameraIOTrackingSim( @@ -59,6 +64,7 @@ public AlgaeTracker(Supplier robotPose, Consumer a m_action = action; m_robotPose = robotPose; m_inputsLogger = new CameraIOInputsLogger(m_inputs, TrackingConstants.kCameraName); + m_ologger.registerPose("Last Seen", () -> m_lastSeenAlgae); } public void run() { @@ -77,21 +83,30 @@ public void run() { Optional estimatedDistance = estimateDistance(target); estimatedDistance.ifPresentOrElse(dist -> { - Transform2d robotOffset = - new Transform2d(new Translation2d(yaw.getCos(), yaw.getSin()).times(dist), Rotation2d.kZero); - Pose2d algae = m_robotPose.get().transformBy(robotOffset); + // Determine camera-relative position of algae + Transform2d cameraOffset = + new Transform2d(yaw.getCos(), yaw.getSin(), Rotation2d.kZero).times(dist); + // Determine robot-relative position of algae + Pose2d robot = m_robotPose.get(); + Pose2d camera = robot + .transformBy(new Transform2d( + TrackingConstants.kRobotToCamera.getTranslation().toTranslation2d(), + TrackingConstants.kRobotToCamera.getRotation().toRotation2d())); + Pose2d algae = camera.transformBy(cameraOffset); + m_lastSeenAlgae = algae; m_action.accept(new ObjectTrackingStatus( - yaw, - Timer.getTimestamp(), - Optional.of(algae))); + algae.relativeTo(robot).getTranslation().getAngle(), + Timer.getTimestamp(), + Optional.of(algae))); }, () -> { m_action.accept(new ObjectTrackingStatus( - yaw, - Timer.getTimestamp(), - Optional.empty())); + yaw, + Timer.getTimestamp(), + Optional.empty())); }); }); + m_ologger.log(); } private Optional estimateDistance(PhotonTrackedTarget target) { diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java index deda9daf..ff354339 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java @@ -9,7 +9,7 @@ import edu.wpi.first.units.measure.Time; class TrackingConstants { - public static final boolean kEnabled = false; + public static final boolean kEnabled = true; protected static final String kCameraName = "camera"; /* it is very important that the robot's position be ON THE GROUND (z = 0) */ @@ -33,7 +33,7 @@ class TrackingConstants { protected static final Distance kGroundAlgaeHeight = Inches.of(8.125); protected static final Distance kLollipopAlgaeHeight = Inches.of(14.0625); - protected static final boolean kDistanceEstimationEnabled = false; + protected static final boolean kDistanceEstimationEnabled = true; protected static final Time kExpirationTime = Milliseconds.of(300); } From 32f51684a6cae89aaef5638742a80683698ae31c Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Sat, 12 Jul 2025 20:05:27 -0400 Subject: [PATCH 02/38] Reduce nesting in AlgaeTracker --- .../robot/vision/tracking/AlgaeTracker.java | 70 ++++++++++--------- 1 file changed, 37 insertions(+), 33 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java index abdad8a4..1bc3c0aa 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java @@ -67,45 +67,49 @@ public AlgaeTracker(Supplier robotPose, Consumer a m_ologger.registerPose("Last Seen", () -> m_lastSeenAlgae); } + private void addResult(PhotonPipelineResult result) { + if (!result.hasTargets()) { + return; + } + PhotonTrackedTarget target = result.getBestTarget(); + Rotation2d yaw = Rotation2d.fromDegrees(-result.getBestTarget().yaw); + + Optional estimatedDistance = estimateDistance(target); + + if (estimatedDistance.isEmpty()) { + m_action.accept(new ObjectTrackingStatus( + yaw, + Timer.getTimestamp(), + Optional.empty())); + return; + } + + double dist = estimatedDistance.get(); + // Determine camera-relative position of algae + Transform2d cameraOffset = new Transform2d( + yaw.getCos(), + yaw.getSin(), + Rotation2d.kZero).times(dist); + // Determine robot-relative position of algae + Pose2d robot = m_robotPose.get(); + Pose2d camera = robot.transformBy(new Transform2d( + TrackingConstants.kRobotToCamera.getTranslation().toTranslation2d(), + TrackingConstants.kRobotToCamera.getRotation().toRotation2d())); + Pose2d algae = camera.transformBy(cameraOffset); + m_lastSeenAlgae = algae; + m_action.accept(new ObjectTrackingStatus( + algae.relativeTo(robot).getTranslation().getAngle(), + Timer.getTimestamp(), + Optional.of(algae))); + } + public void run() { m_io.updateInputs(m_inputs); m_inputsLogger.log(); List results = m_inputs.unreadResults; - results.forEach(result -> { - if (!result.hasTargets()) { - return; - } - PhotonTrackedTarget target = result.getBestTarget(); - Rotation2d yaw = Rotation2d.fromDegrees(-result.getBestTarget().yaw); - - Optional estimatedDistance = estimateDistance(target); - - estimatedDistance.ifPresentOrElse(dist -> { - // Determine camera-relative position of algae - Transform2d cameraOffset = - new Transform2d(yaw.getCos(), yaw.getSin(), Rotation2d.kZero).times(dist); - // Determine robot-relative position of algae - Pose2d robot = m_robotPose.get(); - Pose2d camera = robot - .transformBy(new Transform2d( - TrackingConstants.kRobotToCamera.getTranslation().toTranslation2d(), - TrackingConstants.kRobotToCamera.getRotation().toRotation2d())); - Pose2d algae = camera.transformBy(cameraOffset); - m_lastSeenAlgae = algae; - m_action.accept(new ObjectTrackingStatus( - algae.relativeTo(robot).getTranslation().getAngle(), - Timer.getTimestamp(), - Optional.of(algae))); - }, () -> { - m_action.accept(new ObjectTrackingStatus( - yaw, - Timer.getTimestamp(), - Optional.empty())); - }); - - }); + results.forEach(this::addResult); m_ologger.log(); } From 57834b749e1bd1e1e869aed8e4faed3548fdbc14 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Sat, 12 Jul 2025 20:32:09 -0400 Subject: [PATCH 03/38] Add algae backup --- .../robot/subsystems/drivetrain/CommandSwerveDrivetrain.java | 4 +++- .../java/frc/robot/subsystems/drivetrain/DriveConstants.java | 5 +++++ .../main/java/frc/robot/vision/tracking/AlgaeTracker.java | 1 - 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 4238ab04..04a20533 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -494,6 +494,7 @@ public void run() { ObjectTrackingStatus status = m_objectStatus.get(); if (status.pose().isEmpty()) { // Drive towards algae. + // We invert the yaw because the input is actually the angle is from the robot to the // target, so turning in that direction is good. (basically our input is funny). double rotationalRate = thetaController.calculate(-status.yaw().getRadians(), 0); @@ -508,7 +509,8 @@ public void run() { } // If we have a pose estimate, for algae, use Autopilot to go there. APTarget target = new APTarget(status.pose().get() - .transformBy(new Transform2d(Translation2d.kZero, status.yaw()))); + .transformBy(new Transform2d(Translation2d.kZero, status.yaw())) + .transformBy(DriveConstants.kAlgaeOffset)); Transform2d output = DriveConstants.kTightAutopilot.calculate( m_estimatedPose, getVelocityComponents(), diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java index f8e8baa6..f53a2d88 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java @@ -14,6 +14,9 @@ import com.therekrab.autopilot.APConstraints; import com.therekrab.autopilot.APProfile; import com.therekrab.autopilot.Autopilot; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.LinearAcceleration; @@ -87,5 +90,7 @@ public static class HeadingPID { protected static final LinearVelocity kObjectTrackSpeed = MetersPerSecond.of(2); protected static final LinearVelocity kMaxObjectTrackingSpeed = MetersPerSecond.of(4); + protected static final Transform2d kAlgaeOffset = new Transform2d( + new Translation2d(-0.5, 0), Rotation2d.kZero); } diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java index 1bc3c0aa..3b613f4b 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java @@ -28,7 +28,6 @@ import frc.robot.vision.CameraIOInputsLogger; public class AlgaeTracker implements Runnable { - private final OnboardLogger m_ologger = new OnboardLogger("Tracking"); public static final boolean enabled = TrackingConstants.kEnabled; From dc0ffb25fc8bfe2227cc01d0be0041b0cc0b19ca Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Sat, 12 Jul 2025 21:29:51 -0400 Subject: [PATCH 04/38] Add bindings for algae tracking --- .../main/java/frc/robot/binding/BindingConstants.java | 1 + .../main/java/frc/robot/binding/OperatorBindings.java | 9 ++++++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java index fbf8a519..c445acf6 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java @@ -55,6 +55,7 @@ public static class Operator { public static final int kRightFunnel = 11; public static final int kLeftFunnel = 12; + public static final int kTrackAlgae = Button.kL2.value; } } diff --git a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java index 3632871e..f4ae1936 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java @@ -26,6 +26,7 @@ import frc.robot.superstructure.states.ProcessorPrep; import frc.robot.superstructure.states.DeferredAlign; import frc.robot.superstructure.states.Stow; +import frc.robot.superstructure.states.TrackAlgae; public class OperatorBindings implements Binder { private final CommandPS5Controller m_controller = @@ -43,7 +44,10 @@ public class OperatorBindings implements Binder { private final Trigger m_coralIntake = m_controller.button(Operator.kCoralIntake); private final Trigger m_ejectCoral = m_controller.button(Operator.kEjectCoral); - private final Trigger m_algae = m_controller.button(Operator.kAlgae); + private final Trigger m_untrackedAlgae = m_controller.button(Operator.kAlgae); + private final Trigger m_trackAlgae = m_controller.button(Operator.kTrackAlgae); + + private final Trigger m_algae = m_untrackedAlgae.or(m_trackAlgae); private final Trigger m_algaeGround = m_controller.pov(Operator.kGroundAlgaeIntake); private final Trigger m_algaeHighGround = m_controller.pov(Operator.kHighGroundAlgaeIntake); @@ -65,6 +69,9 @@ public class OperatorBindings implements Binder { private final Trigger m_zeroElevator = m_controller.button(Operator.kCalibrateElevator); public void bind(Superstructure superstructure) { + /* algae tracking */ + m_trackAlgae.whileTrue(superstructure.enter(new TrackAlgae())); + /* algae intake */ m_algae.and(m_algaeGround).whileTrue(superstructure.enter(new GroundAlgaeIntake())); m_algae.and(m_algaeHighGround).whileTrue(superstructure.enter(new HighGroundAlgaeIntake())); From d359db67b32155610fa662150391b18cd2ee8058 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Sun, 13 Jul 2025 14:14:22 -0400 Subject: [PATCH 05/38] Improve comment --- .../robot/subsystems/drivetrain/CommandSwerveDrivetrain.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 04a20533..17c4b350 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -494,9 +494,10 @@ public void run() { ObjectTrackingStatus status = m_objectStatus.get(); if (status.pose().isEmpty()) { // Drive towards algae. - + // We invert the yaw because the input is actually the angle is from the robot to the - // target, so turning in that direction is good. (basically our input is funny). + // target, so turning in that direction is good. (basically our input is reverse + // coordinate system). double rotationalRate = thetaController.calculate(-status.yaw().getRadians(), 0); double speed = DriveConstants.kObjectTrackSpeed.in(MetersPerSecond); double vx = speed * status.yaw().getCos(); From 83dee56370931e83611cd8e106db4717dfb33232 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Sun, 13 Jul 2025 15:15:53 -0400 Subject: [PATCH 06/38] Add steal autons Lots of changes here: - Remove slow() option, handle that internally for drivetrain like spin rate. - Add max distance for algae detection (but not logging) - Auton to steal algae, very cool. --- ThriftyTest/autons/k_steal.txt | 5 ++ ThriftyTest/simgui.json | 5 ++ .../main/deploy/pathplanner/autos/StealK.auto | 1 + .../src/main/java/frc/robot/Constants.java | 3 ++ .../robot/binding/NamedCommandBindings.java | 47 +++++++++++++++---- .../drivetrain/CommandSwerveDrivetrain.java | 24 +++++++++- .../subsystems/drivetrain/DriveConstants.java | 15 ++---- .../robot/superstructure/Superstructure.java | 1 + .../robot/superstructure/states/Align.java | 5 -- .../robot/vision/tracking/AlgaeTracker.java | 18 +++++-- 10 files changed, 94 insertions(+), 30 deletions(-) create mode 100644 ThriftyTest/autons/k_steal.txt create mode 100644 ThriftyTest/src/main/deploy/pathplanner/autos/StealK.auto diff --git a/ThriftyTest/autons/k_steal.txt b/ThriftyTest/autons/k_steal.txt new file mode 100644 index 00000000..e856cb24 --- /dev/null +++ b/ThriftyTest/autons/k_steal.txt @@ -0,0 +1,5 @@ +Align G + L4 +Prepare Steal Inside + (Lollipop * Track) +Align Barge Outside + Net +Prepare Steal Outside + (Lollipop * Track) +Align Barge Center + Net diff --git a/ThriftyTest/simgui.json b/ThriftyTest/simgui.json index 031d1437..7cdd1ccd 100644 --- a/ThriftyTest/simgui.json +++ b/ThriftyTest/simgui.json @@ -55,6 +55,11 @@ "visible": true } }, + "/SmartDashboard/Command Scheduler": { + "window": { + "visible": true + } + }, "/SmartDashboard/Super Field": { "Algae": { "image": "../resources/algae.jpg", diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/StealK.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/StealK.auto new file mode 100644 index 00000000..b41479e5 --- /dev/null +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/StealK.auto @@ -0,0 +1 @@ +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align G"}},{"type":"named","data":{"name":"L4"}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Prepare Steal Inside"}},{"type":"race","data":{"commands":[{"type":"named","data":{"name":"Track"}},{"type":"named","data":{"name":"Lollipop"}}]}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align Barge Center"}},{"type":"named","data":{"name":"Net"}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Prepare Steal Outside"}},{"type":"race","data":{"commands":[{"type":"named","data":{"name":"Track"}},{"type":"named","data":{"name":"Lollipop"}}]}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align Barge Center"}},{"type":"named","data":{"name":"Net"}}]}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/java/frc/robot/Constants.java b/ThriftyTest/src/main/java/frc/robot/Constants.java index b147a5f5..8791bde6 100644 --- a/ThriftyTest/src/main/java/frc/robot/Constants.java +++ b/ThriftyTest/src/main/java/frc/robot/Constants.java @@ -48,6 +48,9 @@ public static class FieldConstants { public static final Pose2d kStartLeft = new Pose2d(7.076, 6.6, Rotation2d.kPi); public static final Rotation2d kStartHeading = Rotation2d.kPi; // facing our alliance + + public static final Pose2d kInsideSteal = new Pose2d(7.642, 1.226, Rotation2d.kZero); + public static final Pose2d kOutsideSteal = new Pose2d(7.642, 2.4, Rotation2d.kZero); } public static final class StateSpaceConstants { diff --git a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java index ce786668..ded0a35f 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -3,20 +3,25 @@ import static edu.wpi.first.units.Units.Meters; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants; import frc.robot.Constants.CoralLevel; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ScoringLocations; +import frc.robot.subsystems.drivetrain.DriveConstants; import com.therekrab.autopilot.APTarget; +import com.therekrab.autopilot.Autopilot; import frc.robot.superstructure.Superstructure; import frc.robot.superstructure.states.AlgaeStow; import frc.robot.superstructure.states.Align; import frc.robot.superstructure.states.CoralScore; import frc.robot.superstructure.states.CoralWait; import frc.robot.superstructure.states.ElevatorZero; +import frc.robot.superstructure.states.HighGroundAlgaeIntake; import frc.robot.superstructure.states.IntakeComplete; import frc.robot.superstructure.states.LowerReefAlgaeIntake; import frc.robot.superstructure.states.Net; +import frc.robot.superstructure.states.TrackAlgae; import frc.robot.superstructure.states.UpperReefAlgaeIntake; public class NamedCommandBindings implements Binder { @@ -38,6 +43,10 @@ public void bind(Superstructure superstructure) { NamedCommands.registerCommand("Upper Algae", superstructure.enter(new UpperReefAlgaeIntake())); NamedCommands.registerCommand("Net", superstructure.enter(new Net())); NamedCommands.registerCommand("Algae Stow", superstructure.enter(new AlgaeStow())); + NamedCommands.registerCommand("Lollipop", superstructure.enter(new HighGroundAlgaeIntake())); + + /* steal hehe */ + NamedCommands.registerCommand("Track", superstructure.enter(new TrackAlgae())); /* align */ for (ScoringLocations location : Constants.ScoringLocations.values()) { @@ -46,6 +55,8 @@ public void bind(Superstructure superstructure) { new Align(new APTarget(location.value).withEntryAngle(location.value.getRotation())) .allianceRelative())); } + + // HP stations APTarget lIntake = new APTarget(FieldConstants.kLeftIntake) .withRotationRadius(Meters.of(2.0)); APTarget rIntake = new APTarget(FieldConstants.kRightIntake) @@ -66,12 +77,17 @@ public void bind(Superstructure superstructure) { new Align(rIntake) .allianceRelative() .fast())); + + // Halfway points NamedCommands.registerCommand("Align IJ", superstructure.enter( - new Align(new APTarget(FieldConstants.kIJ).withEntryAngle(FieldConstants.kIJ.getRotation())) - .allianceRelative())); + new Align(new APTarget(FieldConstants.kIJ) + .withEntryAngle(FieldConstants.kIJ.getRotation())) + .allianceRelative())); NamedCommands.registerCommand("Align GH", superstructure.enter( - new Align(new APTarget(FieldConstants.kGH).withEntryAngle(FieldConstants.kGH.getRotation())) - .allianceRelative())); + new Align(new APTarget(FieldConstants.kGH) + .withEntryAngle(FieldConstants.kGH.getRotation())) + .allianceRelative())); + // Barge APTarget bargeFromCenter = new APTarget(FieldConstants.kBargeFromCenter) .withEntryAngle(Rotation2d.fromDegrees(55.0)) .withRotationRadius(Meters.of(1.5)); @@ -79,12 +95,27 @@ public void bind(Superstructure superstructure) { .withEntryAngle(Rotation2d.fromDegrees(35.0)) .withRotationRadius(Meters.of(1.5)); NamedCommands.registerCommand("Align Barge Center", superstructure.enter( - new Align(bargeFromCenter).allianceRelative().slow())); + new Align(bargeFromCenter).allianceRelative()) + .onlyIf(superstructure.holdingAlgae())); NamedCommands.registerCommand("Align Barge Left", superstructure.enter( - new Align(bargeFromLeft).allianceRelative().slow())); + new Align(bargeFromLeft).allianceRelative()) + .onlyIf(superstructure.holdingAlgae())); NamedCommands.registerCommand("Beeline Barge Center", superstructure.enter( - new Align(bargeFromCenter.withoutEntryAngle()).allianceRelative().slow())); + new Align(bargeFromCenter.withoutEntryAngle()).allianceRelative()) + .onlyIf(superstructure.holdingAlgae())); NamedCommands.registerCommand("Beeline Barge Left", superstructure.enter( - new Align(bargeFromLeft.withoutEntryAngle()).allianceRelative().slow())); + new Align(bargeFromLeft.withoutEntryAngle()).allianceRelative()) + .onlyIf(superstructure.holdingAlgae())); + + // Steal! + APTarget insideSteal = new APTarget(FieldConstants.kInsideSteal) + .withRotationRadius(Meters.of(1)); + APTarget outsideSteal = new APTarget(FieldConstants.kOutsideSteal) + .withRotationRadius(Meters.of(1)); + NamedCommands.registerCommand("Prepare Steal Inside", + superstructure.enter(new Align(insideSteal).allianceRelative())); + NamedCommands.registerCommand("Prepare Steal Outside", + superstructure.enter(new Align(outsideSteal).allianceRelative())); } + } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 17c4b350..be22d001 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -36,6 +36,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; @@ -77,6 +79,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private boolean m_aligned; private Trigger m_tippyTrigger = new Trigger(() -> false); + private Trigger m_slowTrigger = new Trigger(() -> false); private boolean m_hasReceivedVisionUpdate; @@ -402,7 +405,8 @@ public Trigger inReefZone() { } public Trigger seesAlgae() { - return new Trigger(() -> m_objectStatus.isPresent()); + return new Trigger(() -> m_objectStatus.isPresent() + && m_objectStatus.get().isWithin(DriveConstants.kObjectDistanceLimit)); } /** @@ -436,6 +440,10 @@ public void setTippyTrigger(Trigger tippyTrigger) { m_tippyTrigger = tippyTrigger; } + public void setSlowTrigger(Trigger slowTrigger) { + m_slowTrigger = slowTrigger; + } + private AngularVelocity getMaxRotationalRate() { if (m_tippyTrigger.getAsBoolean()) { return DriveConstants.kMaxTippyAngularSpeed; @@ -444,7 +452,19 @@ private AngularVelocity getMaxRotationalRate() { } } + private LinearVelocity getMaxSpeed() { + if (m_slowTrigger.getAsBoolean()) { + return DriveConstants.kMaxTippySpeed; + } + return DriveConstants.kMaxLinearSpeed; + } + private void setVelocity(Transform2d goal) { + double norm = goal.getTranslation().getNorm(); + double max = getMaxSpeed().in(MetersPerSecond); + if (norm > max) { + goal = goal.times(max / norm); + } setControl(m_veloRequest .withVelocityX(goal.getX()) .withVelocityY(goal.getY()) @@ -493,7 +513,7 @@ public void run() { } ObjectTrackingStatus status = m_objectStatus.get(); if (status.pose().isEmpty()) { - // Drive towards algae. + // Drive towards algae, based on camera location. NOT OPTIMAL. // We invert the yaw because the input is actually the angle is from the robot to the // target, so turning in that direction is good. (basically our input is reverse diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java index f53a2d88..eb782b1b 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.units.measure.LinearVelocity; import frc.robot.generated.TunerConstants; @@ -54,18 +55,6 @@ public static class HeadingPID { public static final Autopilot kFastAutopilot = new Autopilot(kFastProfile); - private static final APConstraints kSlowAPConstraints = new APConstraints() - .withAcceleration(3) - .withJerk(1.5) - .withVelocity(3); - - private static final APProfile kSlowProfile = new APProfile(kSlowAPConstraints) - .withErrorXY(Inches.of(2)) - .withErrorTheta(Degrees.of(3)) - .withBeelineRadius(Centimeters.of(20)); - - public static final Autopilot kSlowAutopilot = new Autopilot(kSlowProfile); - protected static final PPHolonomicDriveController kPathplannerHolonomicDriveController = new PPHolonomicDriveController(kTranslationPID, kRotationPID); @@ -77,6 +66,7 @@ public static class HeadingPID { protected static final LinearVelocity kMaxLinearSpeed = MetersPerSecond.of(4); protected static final LinearAcceleration kMaxLinearAcceleration = MetersPerSecondPerSecond.of(3); + public static final LinearVelocity kMaxTippySpeed = MetersPerSecond.of(3); protected static final AngularVelocity kMaxAngularSpeed = RotationsPerSecond.of(2); protected static final AngularVelocity kMaxTippyAngularSpeed = RotationsPerSecond.of(0.5); protected static final AngularAcceleration kMaxAngularAcceleration = @@ -88,6 +78,7 @@ public static class HeadingPID { protected static final double k_closedLoopOverrideToleranceTranslation = 0.05; protected static final double k_closedLoopOverrideToleranceRotation = 0.05; + public static final Distance kObjectDistanceLimit = Meters.of(2); protected static final LinearVelocity kObjectTrackSpeed = MetersPerSecond.of(2); protected static final LinearVelocity kMaxObjectTrackingSpeed = MetersPerSecond.of(4); protected static final Transform2d kAlgaeOffset = new Transform2d( diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/Superstructure.java b/ThriftyTest/src/main/java/frc/robot/superstructure/Superstructure.java index 84f3ceca..22965010 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/Superstructure.java @@ -39,6 +39,7 @@ public Superstructure( leds); drivetrain.setTippyTrigger(tippy()); + drivetrain.setSlowTrigger(elevator.unsafe()); RobotObserver.setFFEnabledSupplier(elevator.unsafe().and(() -> !DriverStation.isAutonomous())); } diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Align.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Align.java index 3c028779..716dc799 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Align.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Align.java @@ -53,11 +53,6 @@ public Align fast() { return this; } - public Align slow() { - m_autopilot = DriveConstants.kSlowAutopilot; - return this; - } - private APTarget target() { if (m_flip) { return FieldUtils.flipTargetConditionally(m_target); diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java index 3b613f4b..39b8e994 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java @@ -16,7 +16,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.Timer; @@ -32,10 +31,21 @@ public class AlgaeTracker implements Runnable { public static final boolean enabled = TrackingConstants.kEnabled; - public record ObjectTrackingStatus(Rotation2d yaw, double time, Optional pose) { + public record ObjectTrackingStatus( + Rotation2d yaw, + double time, + Optional pose, + Optional distance) { public boolean isExpired() { return Timer.getTimestamp() - time() >= TrackingConstants.kExpirationTime.in(Seconds); } + + public boolean isWithin(Distance cutoff) { + if (distance.isEmpty()) { + return true; + } + return distance().get() <= cutoff.in(Meters); + } } @SuppressWarnings("unused") @@ -79,6 +89,7 @@ private void addResult(PhotonPipelineResult result) { m_action.accept(new ObjectTrackingStatus( yaw, Timer.getTimestamp(), + Optional.empty(), Optional.empty())); return; } @@ -99,7 +110,8 @@ private void addResult(PhotonPipelineResult result) { m_action.accept(new ObjectTrackingStatus( algae.relativeTo(robot).getTranslation().getAngle(), Timer.getTimestamp(), - Optional.of(algae))); + Optional.of(algae), + Optional.of(algae.relativeTo(robot).getTranslation().getNorm()))); } public void run() { From eb473a8bd83232f4bbf26536867be215358d12d6 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Sun, 13 Jul 2025 15:37:21 -0400 Subject: [PATCH 07/38] Add secondary steal, just for shoot and move --- ThriftyTest/autons/k_steal2.txt | 5 +++ .../deploy/pathplanner/autos/Steal2K.auto | 1 + .../src/main/java/frc/robot/Constants.java | 3 ++ .../robot/binding/NamedCommandBindings.java | 23 ++++++++++---- .../frc/robot/subsystems/algae/Algae.java | 31 ++++++++++++++++--- .../subsystems/algae/AlgaeConstants.java | 2 ++ .../states/ManualAlgaeEject.java | 11 +++++++ 7 files changed, 65 insertions(+), 11 deletions(-) create mode 100644 ThriftyTest/autons/k_steal2.txt create mode 100644 ThriftyTest/src/main/deploy/pathplanner/autos/Steal2K.auto create mode 100644 ThriftyTest/src/main/java/frc/robot/superstructure/states/ManualAlgaeEject.java diff --git a/ThriftyTest/autons/k_steal2.txt b/ThriftyTest/autons/k_steal2.txt new file mode 100644 index 00000000..4660017e --- /dev/null +++ b/ThriftyTest/autons/k_steal2.txt @@ -0,0 +1,5 @@ +Align G + L4 +Prepare Steal Inside + (Lollipop * Track) +Shoot Outside + Eject Algae +Prepare Steal Outside + (Lollipop * Track) +Shoot Inside + Eject Algae diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Steal2K.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Steal2K.auto new file mode 100644 index 00000000..16b84912 --- /dev/null +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Steal2K.auto @@ -0,0 +1 @@ +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align G"}},{"type":"named","data":{"name":"L4"}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Prepare Steal Inside"}},{"type":"race","data":{"commands":[{"type":"named","data":{"name":"Track"}},{"type":"named","data":{"name":"Lollipop"}}]}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Shoot Outside"}},{"type":"named","data":{"name":"Eject Algae"}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Prepare Steal Outside"}},{"type":"race","data":{"commands":[{"type":"named","data":{"name":"Track"}},{"type":"named","data":{"name":"Lollipop"}}]}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Shoot Inside"}},{"type":"named","data":{"name":"Eject Algae"}}]}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/java/frc/robot/Constants.java b/ThriftyTest/src/main/java/frc/robot/Constants.java index 8791bde6..5d96e14d 100644 --- a/ThriftyTest/src/main/java/frc/robot/Constants.java +++ b/ThriftyTest/src/main/java/frc/robot/Constants.java @@ -51,6 +51,9 @@ public static class FieldConstants { public static final Pose2d kInsideSteal = new Pose2d(7.642, 1.226, Rotation2d.kZero); public static final Pose2d kOutsideSteal = new Pose2d(7.642, 2.4, Rotation2d.kZero); + + public static final Pose2d kInsideShoot = new Pose2d(6, 1.2, Rotation2d.kPi); + public static final Pose2d kOutsideShoot = new Pose2d(6, 2.4, Rotation2d.kPi); } public static final class StateSpaceConstants { diff --git a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java index ded0a35f..de822499 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -2,15 +2,12 @@ import static edu.wpi.first.units.Units.Meters; import com.pathplanner.lib.auto.NamedCommands; +import com.therekrab.autopilot.APTarget; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants; import frc.robot.Constants.CoralLevel; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ScoringLocations; -import frc.robot.subsystems.drivetrain.DriveConstants; -import com.therekrab.autopilot.APTarget; -import com.therekrab.autopilot.Autopilot; import frc.robot.superstructure.Superstructure; import frc.robot.superstructure.states.AlgaeStow; import frc.robot.superstructure.states.Align; @@ -20,6 +17,7 @@ import frc.robot.superstructure.states.HighGroundAlgaeIntake; import frc.robot.superstructure.states.IntakeComplete; import frc.robot.superstructure.states.LowerReefAlgaeIntake; +import frc.robot.superstructure.states.ManualAlgaeEject; import frc.robot.superstructure.states.Net; import frc.robot.superstructure.states.TrackAlgae; import frc.robot.superstructure.states.UpperReefAlgaeIntake; @@ -44,6 +42,7 @@ public void bind(Superstructure superstructure) { NamedCommands.registerCommand("Net", superstructure.enter(new Net())); NamedCommands.registerCommand("Algae Stow", superstructure.enter(new AlgaeStow())); NamedCommands.registerCommand("Lollipop", superstructure.enter(new HighGroundAlgaeIntake())); + NamedCommands.registerCommand("Eject Algae", superstructure.enter(new ManualAlgaeEject())); /* steal hehe */ NamedCommands.registerCommand("Track", superstructure.enter(new TrackAlgae())); @@ -109,13 +108,25 @@ public void bind(Superstructure superstructure) { // Steal! APTarget insideSteal = new APTarget(FieldConstants.kInsideSteal) - .withRotationRadius(Meters.of(1)); + .withRotationRadius(Meters.of(1)); APTarget outsideSteal = new APTarget(FieldConstants.kOutsideSteal) - .withRotationRadius(Meters.of(1)); + .withRotationRadius(Meters.of(1)); NamedCommands.registerCommand("Prepare Steal Inside", superstructure.enter(new Align(insideSteal).allianceRelative())); NamedCommands.registerCommand("Prepare Steal Outside", superstructure.enter(new Align(outsideSteal).allianceRelative())); + + // Shoot! + APTarget outsideShoot = new APTarget(FieldConstants.kOutsideShoot) + .withRotationRadius(Meters.of(0.5)); + APTarget insideShoot = new APTarget(FieldConstants.kInsideShoot) + .withRotationRadius(Meters.of(0.5)); + NamedCommands.registerCommand("Shoot Outside", + superstructure.enter(new Align(outsideShoot).allianceRelative()) + .onlyIf(superstructure.holdingAlgae())); + NamedCommands.registerCommand("Shoot Inside", + superstructure.enter(new Align(insideShoot).allianceRelative()) + .onlyIf(superstructure.holdingAlgae())); } } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java index 70cb2eb0..70450c75 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java @@ -31,7 +31,8 @@ public class Algae extends PassiveSubsystem { private AlgaeIOInputsLogger m_inputsLogger; private boolean m_hasAlgae; - private SuperDebouncer m_debouncer = new SuperDebouncer(AlgaeConstants.kAlgaeDebounceTime.in(Seconds), DebounceType.kFalling); + private SuperDebouncer m_debouncer = + new SuperDebouncer(AlgaeConstants.kAlgaeDebounceTime.in(Seconds), DebounceType.kFalling); private MedianFilter m_filter = new MedianFilter(10); @@ -42,12 +43,12 @@ public Algae() { } else { m_io = new AlgaeIOSim(); } - resetDebouncer(); + resetDebouncer(false); m_inputs = new AlgaeIOInputs(); m_inputsLogger = new AlgaeIOInputsLogger(m_inputs); m_ologger = new OnboardLogger("Algae"); m_ologger.registerBoolean("Holding", holdingAlgae()); - + RobotObserver.setAlgaePieceHeldSupplier(this.holdingAlgae()); m_timer = new LoopTimer("Algae"); } @@ -86,7 +87,8 @@ public void periodic() { m_timer.reset(); m_io.updateInputs(m_inputs); m_inputsLogger.log(); - m_hasAlgae = m_debouncer.calculate(getTorqueCurrent() >= AlgaeConstants.kTorqueCurrentThreshold); + m_hasAlgae = + m_debouncer.calculate(getTorqueCurrent() >= AlgaeConstants.kTorqueCurrentThreshold); m_ologger.log(); m_timer.log(); } @@ -107,7 +109,13 @@ public Command intake() { .unless(holdingAlgae()); } - private void resetDebouncer() { + /** + * Resets the debouncer to be willing to accept any new inputs as accurate + */ + private void resetDebouncer(boolean ignore) { + if (ignore) { + return; + } m_debouncer.overrideTimer(); } @@ -133,6 +141,19 @@ public Command processorScore() { Commands.waitSeconds(AlgaeConstants.kProcessorScoreTime)) .finallyDo(this::keep) + .finallyDo(this::resetDebouncer) .onlyIf(holdingAlgae()); } + + /** Ejects an algae fast */ + public Command eject() { + return Commands.sequence( + runOnce(() -> setVoltage(AlgaeConstants.kManualEjectVoltage)), + Commands.waitSeconds(AlgaeConstants.kManualEjectTime)) + + .finallyDo(this::keep) + .finallyDo(this::resetDebouncer) + .onlyIf(holdingAlgae()); + } + } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java index 7189b9f7..86a49666 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java @@ -17,6 +17,7 @@ public final class AlgaeConstants { protected static final double kNetEjectVoltage = -3.0; protected static final double kProcessorEjectVoltage = -3.2; protected static final double kHoldVoltage = 6.0; + protected static final double kManualEjectVoltage = -6.0; protected static final double kTorqueCurrentThreshold = 75; // We should consider 40-55 range as well. protected static final Time kAlgaeDebounceTime = Seconds.of(0.8); @@ -26,6 +27,7 @@ public final class AlgaeConstants { protected static final double kProcessorScoreTime = 2.0; protected static final double kNetScoreTime = 0.4; + protected static final double kManualEjectTime = 0.3; protected static final TalonFXConfiguration kMotorConfig = new TalonFXConfiguration() .withMotorOutput(new MotorOutputConfigs() diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/ManualAlgaeEject.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ManualAlgaeEject.java new file mode 100644 index 00000000..d6352c2f --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ManualAlgaeEject.java @@ -0,0 +1,11 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class ManualAlgaeEject implements EnterableState { + public Command build(Subsystems subsystems) { + return subsystems.algae().eject(); + } +} From a6ae7c7a1717efc4b482b9640f99eae2d87dc1b6 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Mon, 14 Jul 2025 18:34:29 -0400 Subject: [PATCH 08/38] 3d algae --- .../drivetrain/CommandSwerveDrivetrain.java | 3 +- .../java/frc/robot/utils/OnboardLogger.java | 38 +++++++++++++++---- .../robot/vision/tracking/AlgaeTracker.java | 34 +++++++++++------ 3 files changed, 54 insertions(+), 21 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index be22d001..eb88ea5f 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -36,7 +36,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -529,7 +528,7 @@ public void run() { return; } // If we have a pose estimate, for algae, use Autopilot to go there. - APTarget target = new APTarget(status.pose().get() + APTarget target = new APTarget(status.pose().get().toPose2d() .transformBy(new Transform2d(Translation2d.kZero, status.yaw())) .transformBy(DriveConstants.kAlgaeOffset)); Transform2d output = DriveConstants.kTightAutopilot.calculate( diff --git a/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java b/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java index 4c55a4e6..9d400585 100644 --- a/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java +++ b/ThriftyTest/src/main/java/frc/robot/utils/OnboardLogger.java @@ -7,6 +7,7 @@ import java.util.function.Supplier; import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.util.datalog.BooleanLogEntry; import edu.wpi.first.util.datalog.DataLog; import edu.wpi.first.util.datalog.DoubleLogEntry; @@ -27,16 +28,20 @@ public class OnboardLogger { private final List, DoubleLogEntry>> m_doubleEntries; private final List, BooleanLogEntry>> m_booleanEntries; private final List, StringLogEntry>> m_stringEntries; - private final List, StructLogEntry>> m_poseEntries; - private final List, StructArrayLogEntry>> m_poseListEntries; + private final List, StructLogEntry>> m_pose2dEntries; + private final List, StructArrayLogEntry>> m_pose2dArrayEntries; + private final List, StructLogEntry>> m_pose3dEntries; + private final List, StructArrayLogEntry>> m_pose3dArrayEntries; public OnboardLogger(String name) { m_name = name; m_doubleEntries = new ArrayList<>(); m_booleanEntries = new ArrayList<>(); m_stringEntries = new ArrayList<>(); - m_poseEntries = new ArrayList<>(); - m_poseListEntries = new ArrayList<>(); + m_pose2dEntries = new ArrayList<>(); + m_pose2dArrayEntries = new ArrayList<>(); + m_pose3dEntries = new ArrayList<>(); + m_pose3dArrayEntries = new ArrayList<>(); } public void registerDouble(String name, DoubleSupplier supplier) { @@ -57,13 +62,24 @@ public void registerString(String name, Supplier supplier) { public void registerPose(String name, Supplier supplier) { StructLogEntry entry = StructLogEntry.create(dataLog, m_name + "/" + name, Pose2d.struct); - m_poseEntries.add(new Pair<>(supplier, entry)); + m_pose2dEntries.add(new Pair<>(supplier, entry)); + } + + public void registerPose3d(String name, Supplier supplier) { + StructLogEntry entry = StructLogEntry.create(dataLog, m_name + "/" + name, Pose3d.struct); + m_pose3dEntries.add(new Pair<>(supplier, entry)); } public void registerPoses(String name, Supplier supplier) { StructArrayLogEntry entry = StructArrayLogEntry.create(dataLog, m_name + "/" + name, Pose2d.struct); - m_poseListEntries.add(new Pair<>(supplier, entry)); + m_pose2dArrayEntries.add(new Pair<>(supplier, entry)); + } + + public void registerPoses3d(String name, Supplier supplier) { + StructArrayLogEntry entry = + StructArrayLogEntry.create(dataLog, m_name + "/" + name, Pose3d.struct); + m_pose3dArrayEntries.add(new Pair<>(supplier, entry)); } public void log() { @@ -76,10 +92,16 @@ public void log() { for (Pair, StringLogEntry> pair : m_stringEntries) { pair.getSecond().update(pair.getFirst().get()); } - for (Pair, StructLogEntry> pair : m_poseEntries) { + for (Pair, StructLogEntry> pair : m_pose2dEntries) { + pair.getSecond().update(pair.getFirst().get()); + } + for (Pair, StructArrayLogEntry> pair : m_pose2dArrayEntries) { + pair.getSecond().update(pair.getFirst().get()); + } + for (Pair, StructLogEntry> pair : m_pose3dEntries) { pair.getSecond().update(pair.getFirst().get()); } - for (Pair, StructArrayLogEntry> pair : m_poseListEntries) { + for (Pair, StructArrayLogEntry> pair : m_pose3dArrayEntries) { pair.getSecond().update(pair.getFirst().get()); } } diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java index 39b8e994..ca7bd219 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java @@ -14,8 +14,12 @@ import org.slf4j.LoggerFactory; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.Timer; @@ -34,7 +38,7 @@ public class AlgaeTracker implements Runnable { public record ObjectTrackingStatus( Rotation2d yaw, double time, - Optional pose, + Optional pose, Optional distance) { public boolean isExpired() { return Timer.getTimestamp() - time() >= TrackingConstants.kExpirationTime.in(Seconds); @@ -59,7 +63,7 @@ public boolean isWithin(Distance cutoff) { private final Supplier m_robotPose; - private Pose2d m_lastSeenAlgae = Pose2d.kZero; + private Pose3d m_lastSeenAlgae = Pose3d.kZero; public AlgaeTracker(Supplier robotPose, Consumer action) { if (Robot.isSimulation()) { @@ -73,7 +77,8 @@ public AlgaeTracker(Supplier robotPose, Consumer a m_action = action; m_robotPose = robotPose; m_inputsLogger = new CameraIOInputsLogger(m_inputs, TrackingConstants.kCameraName); - m_ologger.registerPose("Last Seen", () -> m_lastSeenAlgae); + m_ologger.registerPose3d("Last Seen", () -> m_lastSeenAlgae); + m_ologger.registerPose("seen last", () -> m_lastSeenAlgae.toPose2d()); } private void addResult(PhotonPipelineResult result) { @@ -96,22 +101,22 @@ private void addResult(PhotonPipelineResult result) { double dist = estimatedDistance.get(); // Determine camera-relative position of algae - Transform2d cameraOffset = new Transform2d( + Transform3d cameraOffset = new Transform3d( yaw.getCos(), yaw.getSin(), - Rotation2d.kZero).times(dist); + getAlgaeHeight(target).in(Meters), + Rotation3d.kZero).times(dist); // Determine robot-relative position of algae Pose2d robot = m_robotPose.get(); - Pose2d camera = robot.transformBy(new Transform2d( - TrackingConstants.kRobotToCamera.getTranslation().toTranslation2d(), - TrackingConstants.kRobotToCamera.getRotation().toRotation2d())); - Pose2d algae = camera.transformBy(cameraOffset); + Pose3d camera = new Pose3d(robot).transformBy(TrackingConstants.kRobotToCamera); + Pose3d algae = camera.transformBy(cameraOffset); + Translation2d relative = algae.toPose2d().relativeTo(robot).getTranslation(); m_lastSeenAlgae = algae; m_action.accept(new ObjectTrackingStatus( - algae.relativeTo(robot).getTranslation().getAngle(), + relative.getAngle(), Timer.getTimestamp(), Optional.of(algae), - Optional.of(algae.relativeTo(robot).getTranslation().getNorm()))); + Optional.of(relative.getNorm()))); } public void run() { @@ -124,6 +129,13 @@ public void run() { m_ologger.log(); } + private Distance getAlgaeHeight(PhotonTrackedTarget target) { + if (target.pitch >= 0) { + return TrackingConstants.kLollipopAlgaeHeight; + } + return TrackingConstants.kGroundAlgaeHeight; + } + private Optional estimateDistance(PhotonTrackedTarget target) { if (!TrackingConstants.kDistanceEstimationEnabled) { return Optional.empty(); From 41a29b42c3c2261a79f5a92b860347c4312a2337 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Mon, 14 Jul 2025 19:28:18 -0400 Subject: [PATCH 09/38] improvements to algae tracking --- .../drivetrain/CommandSwerveDrivetrain.java | 3 +++ .../robot/subsystems/drivetrain/DriveConstants.java | 2 +- .../frc/robot/superstructure/states/TrackAlgae.java | 3 ++- .../java/frc/robot/vision/tracking/AlgaeTracker.java | 6 +++++- .../frc/robot/vision/tracking/TrackingConstants.java | 12 +++++++----- 5 files changed, 18 insertions(+), 8 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index eb88ea5f..ef4eb5c6 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -349,6 +349,9 @@ public void addPoseEstimate(TimestampedPoseEstimate estimate) { } public void addObjectTrackingData(ObjectTrackingStatus status) { + if (status.distance().isPresent() && status.distance().get() < 0.2) { + return; + } m_objectStatus = Optional.of(status); } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java index eb782b1b..3d36f3a5 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java @@ -78,7 +78,7 @@ public static class HeadingPID { protected static final double k_closedLoopOverrideToleranceTranslation = 0.05; protected static final double k_closedLoopOverrideToleranceRotation = 0.05; - public static final Distance kObjectDistanceLimit = Meters.of(2); + public static final Distance kObjectDistanceLimit = Meters.of(3); protected static final LinearVelocity kObjectTrackSpeed = MetersPerSecond.of(2); protected static final LinearVelocity kMaxObjectTrackingSpeed = MetersPerSecond.of(4); protected static final Transform2d kAlgaeOffset = new Transform2d( diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/TrackAlgae.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/TrackAlgae.java index 0e8a9c66..bbada704 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/TrackAlgae.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/TrackAlgae.java @@ -6,6 +6,7 @@ public class TrackAlgae implements EnterableState { public Command build(Subsystems subsystems) { - return subsystems.drivetrain().followObject(); + return subsystems.drivetrain().followObject() + .until(subsystems.algae().holdingAlgae()); } } diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java index ca7bd219..20342d5e 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java @@ -23,7 +23,9 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; +import frc.robot.RobotObserver; import frc.robot.utils.OnboardLogger; import frc.robot.vision.CameraIO; import frc.robot.vision.CameraIO.CameraIOInputs; @@ -112,11 +114,12 @@ private void addResult(PhotonPipelineResult result) { Pose3d algae = camera.transformBy(cameraOffset); Translation2d relative = algae.toPose2d().relativeTo(robot).getTranslation(); m_lastSeenAlgae = algae; + double distance = relative.getNorm(); m_action.accept(new ObjectTrackingStatus( relative.getAngle(), Timer.getTimestamp(), Optional.of(algae), - Optional.of(relative.getNorm()))); + Optional.of(distance))); } public void run() { @@ -127,6 +130,7 @@ public void run() { results.forEach(this::addResult); m_ologger.log(); + RobotObserver.getField().getObject("Algae").setPose(m_lastSeenAlgae.toPose2d()); } private Distance getAlgaeHeight(PhotonTrackedTarget target) { diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java index ff354339..3b2985c4 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java @@ -5,20 +5,22 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; class TrackingConstants { public static final boolean kEnabled = true; - protected static final String kCameraName = "camera"; + protected static final String kCameraName = "algaeman"; /* it is very important that the robot's position be ON THE GROUND (z = 0) */ - protected static final Transform3d kRobotToCamera = new Transform3d(0, 0, 0.3, Rotation3d.kZero); + // protected static final Transform3d kRobotToCamera = new Transform3d(Units.inchesToMeters(9), Units.inchesToMeters(10), Units.inchesToMeters(11), Rotation3d.kZero); + protected static final Transform3d kRobotToCamera = new Transform3d(Units.inchesToMeters(8.304 + 0.75), Units.inchesToMeters(9.75), Units.inchesToMeters(11), Rotation3d.kZero); /* simulation */ - protected static final double kFPS = 14; - protected static final int kResWidth = 640; - protected static final int kResHeight = 480; + protected static final double kFPS = 30; + protected static final int kResWidth = 320; + protected static final int kResHeight = 320; protected static final Rotation2d kFOVDiag = Rotation2d.fromDegrees(100); protected static final double kCalibError = 0.5; From 3b4132531b2884fe7bd2d01c56cd88fbbae36810 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Mon, 14 Jul 2025 19:35:08 -0400 Subject: [PATCH 10/38] remove unnecessary thigns --- .../robot/subsystems/drivetrain/CommandSwerveDrivetrain.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index ef4eb5c6..eb88ea5f 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -349,9 +349,6 @@ public void addPoseEstimate(TimestampedPoseEstimate estimate) { } public void addObjectTrackingData(ObjectTrackingStatus status) { - if (status.distance().isPresent() && status.distance().get() < 0.2) { - return; - } m_objectStatus = Optional.of(status); } From 26cf81ea6068f6c3179db8325004eb75951ddd19 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Mon, 14 Jul 2025 19:50:32 -0400 Subject: [PATCH 11/38] start on simple pose filterer --- ThriftyTest/simgui.json | 1 - .../vision/tracking/CameraIOTrackingSim.java | 5 ++- .../vision/tracking/SimplePoseFilter.java | 39 +++++++++++++++++++ 3 files changed, 43 insertions(+), 2 deletions(-) create mode 100644 ThriftyTest/src/main/java/frc/robot/vision/tracking/SimplePoseFilter.java diff --git a/ThriftyTest/simgui.json b/ThriftyTest/simgui.json index 7cdd1ccd..1e1246dd 100644 --- a/ThriftyTest/simgui.json +++ b/ThriftyTest/simgui.json @@ -97,7 +97,6 @@ }, "Tracked Object": { "arrows": false, - "image": "/Users/rekrab/hackbots/2025_Reefscape/resources/algae.jpg", "length": 0.800000011920929, "width": 0.800000011920929 }, diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java index 117eb9cf..1f08e1f8 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java @@ -3,6 +3,9 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Seconds; import java.util.function.Supplier; + +import javax.sound.midi.Track; + import org.photonvision.PhotonCamera; import org.photonvision.estimation.TargetModel; import org.photonvision.simulation.PhotonCameraSim; @@ -30,7 +33,7 @@ public class CameraIOTrackingSim implements CameraIO { // simulation stuff only happens once private static final VisionSystemSim visionSim = new VisionSystemSim("tracking"); private static final TargetModel targetModel = new TargetModel(0.5); - private static final Pose3d initialTargetPose = new Pose3d(2, 2, 0, Rotation3d.kZero); + private static final Pose3d initialTargetPose = new Pose3d(2, 2, TrackingConstants.kLollipopAlgaeHeight.in(Meters), Rotation3d.kZero); private static final VisionTargetSim targetSim = new VisionTargetSim(initialTargetPose, targetModel); private static final SimCameraProperties simProps = new SimCameraProperties() diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/SimplePoseFilter.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/SimplePoseFilter.java new file mode 100644 index 00000000..2247277a --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/SimplePoseFilter.java @@ -0,0 +1,39 @@ +package frc.robot.vision.tracking; + +import java.util.ArrayList; +import java.util.HashSet; +import java.util.List; +import java.util.Optional; +import java.util.Set; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.vision.tracking.AlgaeTracker.ObjectTrackingStatus; + +public class SimplePoseFilter { + private record PoseData(ObjectTrackingStatus status, double trust) {}; + + private final Set m_data; + + public SimplePoseFilter() { + m_data = new HashSet<>(); + } + + public Optional calculate(ObjectTrackingStatus status) { + // Add new value with calculated trust + m_data.add(new PoseData(status, calculateTrust(status))); + // Filter old values from m_data + filterData(); + // If no data, return Optional.empty() + return weighAll(); + } + + private double calculateTrust(ObjectTrackingStatus status) { + double timeDiff = Timer.getTimestamp() - status.time(); + if (timeDiff == 0) { + return 1; + } + return Math.min(1, 1/timeDiff); + } +} \ No newline at end of file From fcb75ae8d2b13b79c222e1e474cdffe336e680f5 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Tue, 15 Jul 2025 00:03:21 -0400 Subject: [PATCH 12/38] Improve algae tracking - Filter algae pose results to better smooth out bad values - Still unsure why lollipops were failing. More testing is needed. --- ThriftyTest/simgui.json | 8 +- .../drivetrain/CommandSwerveDrivetrain.java | 83 ++++++++----------- .../subsystems/drivetrain/DriveConstants.java | 1 - .../robot/vision/tracking/AlgaeTracker.java | 31 ++----- .../vision/tracking/CameraIOTrackingSim.java | 4 +- .../vision/tracking/SimplePoseFilter.java | 70 ++++++++++------ 6 files changed, 88 insertions(+), 109 deletions(-) diff --git a/ThriftyTest/simgui.json b/ThriftyTest/simgui.json index 1e1246dd..a60095f1 100644 --- a/ThriftyTest/simgui.json +++ b/ThriftyTest/simgui.json @@ -64,6 +64,7 @@ "Algae": { "image": "../resources/algae.jpg", "length": 0.800000011920929, + "selectable": false, "width": 0.800000011920929 }, "FF": { @@ -92,6 +93,7 @@ 255.0 ], "length": 0.9399999976158142, + "selectable": false, "style": "Box/Image", "width": 0.9399999976158142 }, @@ -212,9 +214,6 @@ }, "transitory": { "SmartDashboard": { - "Algae": { - "open": true - }, "Algae Tracking": { "open": true }, @@ -223,9 +222,6 @@ "open": true } }, - "Vision": { - "open": true - }, "open": true }, "photonvision": { diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index eb88ea5f..4d33f01a 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -20,15 +20,14 @@ import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric; import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentricFacingAngle; import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; -import com.ctre.phoenix6.swerve.SwerveRequest.RobotCentric; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.util.DriveFeedforwards; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.swerve.SwerveSetpoint; import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; -import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; @@ -59,6 +58,7 @@ import frc.robot.utils.LoopTimer; import frc.robot.utils.OnboardLogger; import frc.robot.vision.localization.TimestampedPoseEstimate; +import frc.robot.vision.tracking.SimplePoseFilter; import frc.robot.vision.tracking.AlgaeTracker.ObjectTrackingStatus; /** @@ -86,9 +86,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) .withDriveRequestType(DriveRequestType.Velocity); - private RobotCentric m_trackingRequest = new RobotCentric() - .withDriveRequestType(DriveRequestType.Velocity); - private FieldCentricFacingAngle m_veloRequest = new FieldCentricFacingAngle() .withHeadingPID(DriveConstants.HeadingPID.kP, 0, 0) .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance) @@ -104,7 +101,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private boolean m_hasAppliedOperatorPerspective = false; /* last known object tracking input */ - private Optional m_objectStatus = Optional.empty(); + private Optional m_algae = Optional.empty(); + private final SimplePoseFilter m_algaeSmoother = new SimplePoseFilter(); + private Pose3d m_lastAlgae = Pose3d.kZero; private Pose2d m_estimatedPose = new Pose2d(); @@ -128,6 +127,7 @@ public CommandSwerveDrivetrain( m_ologger.registerPose("Estimated Pose", this::getPose); m_ologger.registerBoolean("Received Vision Update", () -> m_hasReceivedVisionUpdate); m_ologger.registerBoolean("Valid Object Estimate", seesAlgae()); + m_ologger.registerPose3d("Last Algae", () -> m_lastAlgae); RobotObserver.setVelocitySupplier(this::getVelocity); RobotObserver.setNoElevatorZoneSupplier(dangerZone()); @@ -259,12 +259,16 @@ public void periodic() { m_hasAppliedOperatorPerspective = true; }); } + // Update algae state + m_algae = m_algaeSmoother.calculate(); + if (m_algae.isPresent()) { + m_lastAlgae = m_algae.get(); + RobotObserver.getField().getObject("Algae").setPose(m_lastAlgae.toPose2d()); + } else { + RobotObserver.getField().getObject("Algae").setPoses(); + } m_ologger.log(); m_hasReceivedVisionUpdate = false; - // Expire old algae tracking data - if (m_objectStatus.isPresent() && m_objectStatus.get().isExpired()) { - m_objectStatus = Optional.empty(); - } m_timer.log(); } @@ -349,7 +353,7 @@ public void addPoseEstimate(TimestampedPoseEstimate estimate) { } public void addObjectTrackingData(ObjectTrackingStatus status) { - m_objectStatus = Optional.of(status); + m_algaeSmoother.add(status); } public Command sysIdQuasistaticTranslation(SysIdRoutine.Direction direction) { @@ -404,8 +408,8 @@ public Trigger inReefZone() { } public Trigger seesAlgae() { - return new Trigger(() -> m_objectStatus.isPresent() - && m_objectStatus.get().isWithin(DriveConstants.kObjectDistanceLimit)); + // TODO: ensure distance is below threshold + return new Trigger(() -> m_algae.isPresent()); } /** @@ -501,42 +505,25 @@ public Command seedLocal(Pose2d pose) { } public Command followObject() { - return run(new Runnable() { - private final PIDController thetaController = new PIDController(8, 0, 0); - - @Override - public void run() { - if (m_objectStatus.isEmpty()) { - stop(); - return; - } - ObjectTrackingStatus status = m_objectStatus.get(); - if (status.pose().isEmpty()) { - // Drive towards algae, based on camera location. NOT OPTIMAL. - - // We invert the yaw because the input is actually the angle is from the robot to the - // target, so turning in that direction is good. (basically our input is reverse - // coordinate system). - double rotationalRate = thetaController.calculate(-status.yaw().getRadians(), 0); - double speed = DriveConstants.kObjectTrackSpeed.in(MetersPerSecond); - double vx = speed * status.yaw().getCos(); - double vy = speed * status.yaw().getSin(); - setControl(m_trackingRequest - .withRotationalRate(rotationalRate) - .withVelocityX(vx) - .withVelocityY(vy)); - return; - } - // If we have a pose estimate, for algae, use Autopilot to go there. - APTarget target = new APTarget(status.pose().get().toPose2d() - .transformBy(new Transform2d(Translation2d.kZero, status.yaw())) - .transformBy(DriveConstants.kAlgaeOffset)); - Transform2d output = DriveConstants.kTightAutopilot.calculate( - m_estimatedPose, - getVelocityComponents(), - target); - setVelocity(output); + return run(() -> { + if (m_algae.isEmpty()) { + stop(); + return; } + // If we have a pose estimate, for algae, use Autopilot to go there. + // APTarget target = new APTarget(algae.transformBy(DriveConstants.kAlgaeOffset)); + Rotation2d angle = m_lastAlgae.toPose2d().getTranslation() + .minus(m_estimatedPose.getTranslation()) + .getAngle(); + APTarget target = + new APTarget(m_lastAlgae.toPose2d() + .transformBy(new Transform2d(Translation2d.kZero, angle)) + .transformBy(DriveConstants.kAlgaeOffset)); + Transform2d output = DriveConstants.kTightAutopilot.calculate( + m_estimatedPose, + getVelocityComponents(), + target); + setVelocity(output); }).onlyWhile(seesAlgae()); } } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java index 3d36f3a5..85e92ef9 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java @@ -2,7 +2,6 @@ import static edu.wpi.first.units.Units.Centimeters; import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java index 20342d5e..5abd6129 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java @@ -17,40 +17,29 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; -import frc.robot.RobotObserver; -import frc.robot.utils.OnboardLogger; import frc.robot.vision.CameraIO; import frc.robot.vision.CameraIO.CameraIOInputs; import frc.robot.vision.CameraIOHardware; import frc.robot.vision.CameraIOInputsLogger; public class AlgaeTracker implements Runnable { - private final OnboardLogger m_ologger = new OnboardLogger("Tracking"); - public static final boolean enabled = TrackingConstants.kEnabled; public record ObjectTrackingStatus( Rotation2d yaw, double time, - Optional pose, - Optional distance) { + Optional pose) { public boolean isExpired() { return Timer.getTimestamp() - time() >= TrackingConstants.kExpirationTime.in(Seconds); } - - public boolean isWithin(Distance cutoff) { - if (distance.isEmpty()) { - return true; - } - return distance().get() <= cutoff.in(Meters); + public boolean isOkay() { + return !isExpired(); } } @@ -65,8 +54,6 @@ public boolean isWithin(Distance cutoff) { private final Supplier m_robotPose; - private Pose3d m_lastSeenAlgae = Pose3d.kZero; - public AlgaeTracker(Supplier robotPose, Consumer action) { if (Robot.isSimulation()) { m_io = new CameraIOTrackingSim( @@ -79,8 +66,6 @@ public AlgaeTracker(Supplier robotPose, Consumer a m_action = action; m_robotPose = robotPose; m_inputsLogger = new CameraIOInputsLogger(m_inputs, TrackingConstants.kCameraName); - m_ologger.registerPose3d("Last Seen", () -> m_lastSeenAlgae); - m_ologger.registerPose("seen last", () -> m_lastSeenAlgae.toPose2d()); } private void addResult(PhotonPipelineResult result) { @@ -96,7 +81,6 @@ private void addResult(PhotonPipelineResult result) { m_action.accept(new ObjectTrackingStatus( yaw, Timer.getTimestamp(), - Optional.empty(), Optional.empty())); return; } @@ -106,20 +90,17 @@ private void addResult(PhotonPipelineResult result) { Transform3d cameraOffset = new Transform3d( yaw.getCos(), yaw.getSin(), - getAlgaeHeight(target).in(Meters), + getAlgaeHeight(target).minus(TrackingConstants.kRobotToCamera.getMeasureZ()).in(Meters), Rotation3d.kZero).times(dist); // Determine robot-relative position of algae Pose2d robot = m_robotPose.get(); Pose3d camera = new Pose3d(robot).transformBy(TrackingConstants.kRobotToCamera); Pose3d algae = camera.transformBy(cameraOffset); Translation2d relative = algae.toPose2d().relativeTo(robot).getTranslation(); - m_lastSeenAlgae = algae; - double distance = relative.getNorm(); m_action.accept(new ObjectTrackingStatus( relative.getAngle(), Timer.getTimestamp(), - Optional.of(algae), - Optional.of(distance))); + Optional.of(algae))); } public void run() { @@ -129,8 +110,6 @@ public void run() { List results = m_inputs.unreadResults; results.forEach(this::addResult); - m_ologger.log(); - RobotObserver.getField().getObject("Algae").setPose(m_lastSeenAlgae.toPose2d()); } private Distance getAlgaeHeight(PhotonTrackedTarget target) { diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java index 1f08e1f8..953159e4 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/CameraIOTrackingSim.java @@ -4,8 +4,6 @@ import static edu.wpi.first.units.Units.Seconds; import java.util.function.Supplier; -import javax.sound.midi.Track; - import org.photonvision.PhotonCamera; import org.photonvision.estimation.TargetModel; import org.photonvision.simulation.PhotonCameraSim; @@ -26,7 +24,7 @@ public class CameraIOTrackingSim implements CameraIO { private final PhotonCamera m_camera; private final Supplier m_robotPose; - + private final static Transform3d offset = new Transform3d(0, 0, TrackingConstants.kGroundAlgaeHeight.in(Meters), Rotation3d.kZero); diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/SimplePoseFilter.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/SimplePoseFilter.java index 2247277a..1b30f411 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/SimplePoseFilter.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/SimplePoseFilter.java @@ -1,39 +1,59 @@ package frc.robot.vision.tracking; -import java.util.ArrayList; -import java.util.HashSet; -import java.util.List; +import java.util.ArrayDeque; import java.util.Optional; -import java.util.Set; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; +import java.util.Queue; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.wpilibj.Timer; import frc.robot.vision.tracking.AlgaeTracker.ObjectTrackingStatus; public class SimplePoseFilter { - private record PoseData(ObjectTrackingStatus status, double trust) {}; + private record PoseData(Translation3d pos, double trust) { + }; + + private final Queue m_data; + private static final int SIZE_LIMIT = 100; - private final Set m_data; + public SimplePoseFilter() { + m_data = new ArrayDeque<>(SIZE_LIMIT); + } - public SimplePoseFilter() { - m_data = new HashSet<>(); + public void add(ObjectTrackingStatus status) { + // Add status to list + m_data.add(status); + while (m_data.size() > SIZE_LIMIT) { + m_data.remove(); } + } - public Optional calculate(ObjectTrackingStatus status) { - // Add new value with calculated trust - m_data.add(new PoseData(status, calculateTrust(status))); - // Filter old values from m_data - filterData(); - // If no data, return Optional.empty() - return weighAll(); + public Optional calculate() { + // Ensure that list is below size limit. + Optional result = m_data.stream() + .filter(ObjectTrackingStatus::isOkay) + .filter(s -> s.pose().isPresent()) + .map(s -> { + double trust = calculateTrust(s); + return new PoseData( + s.pose().get().getTranslation().times(trust), + trust); + }) + .reduce((a, b) -> new PoseData(a.pos().plus(b.pos()), a.trust() + b.trust())); + // If we have nothing, say so. + if (result.isEmpty()) { + return Optional.empty(); } + PoseData data = result.get(); + Pose3d combined = new Pose3d(data.pos().div(data.trust()), Rotation3d.kZero); + return Optional.of(combined); + } - private double calculateTrust(ObjectTrackingStatus status) { - double timeDiff = Timer.getTimestamp() - status.time(); - if (timeDiff == 0) { - return 1; - } - return Math.min(1, 1/timeDiff); + private double calculateTrust(ObjectTrackingStatus status) { + double dt = Timer.getTimestamp() - status.time(); + if (dt == 0) { + return 1; } -} \ No newline at end of file + return 1 / dt; + } +} From a4179f9da304d304a89d4377c8a6c37d2c588a1a Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Wed, 16 Jul 2025 18:56:22 -0400 Subject: [PATCH 13/38] make some changes --- ThriftyTest/autons/center.txt | 5 +- .../main/deploy/pathplanner/autos/Center.auto | 35 ++++- .../deploy/pathplanner/autos/LollipopK.auto | 134 +++++++++++++++++ .../main/deploy/pathplanner/autos/StealK.auto | 135 +++++++++++++++++- .../src/main/java/frc/robot/Constants.java | 11 +- .../frc/robot/binding/DashboardBindings.java | 2 + .../robot/binding/NamedCommandBindings.java | 27 +++- .../states/LowerReefAlgaeIntake.java | 3 +- .../states/UpperReefAlgaeIntake.java | 3 +- .../robot/vision/tracking/AlgaeTracker.java | 2 +- 10 files changed, 343 insertions(+), 14 deletions(-) create mode 100644 ThriftyTest/src/main/deploy/pathplanner/autos/LollipopK.auto diff --git a/ThriftyTest/autons/center.txt b/ThriftyTest/autons/center.txt index 56c8911b..f4f538a7 100644 --- a/ThriftyTest/autons/center.txt +++ b/ThriftyTest/autons/center.txt @@ -6,4 +6,7 @@ Net Upper Algae & Align IJ Align Barge Left Net -Align LIntake +Prep Lollipop Outside +Lollipop ? Lollipop Outside +Beeline Barge Left +Net diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto index 273f4bd7..4a1786ad 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto @@ -29,7 +29,7 @@ { "type": "named", "data": { - "name": "Lower Algae" + "name": "Upper Algae" } } ] @@ -81,7 +81,38 @@ { "type": "named", "data": { - "name": "Align LIntake" + "name": "Prepare Lollipop Outside" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Lollipop" + } + }, + { + "type": "named", + "data": { + "name": "Lollipop Outside" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Beeline Barge Left" + } + }, + { + "type": "named", + "data": { + "name": "Net" } } ] diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/LollipopK.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/LollipopK.auto new file mode 100644 index 00000000..64ad9d16 --- /dev/null +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/LollipopK.auto @@ -0,0 +1,134 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Align G" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Prepare Lollipop Inside" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Lollipop" + } + }, + { + "type": "named", + "data": { + "name": "Lollipop Inside" + } + } + ] + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Align Barge Center" + } + }, + { + "type": "named", + "data": { + "name": "Net" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Prepare Lollipop Outside" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Lollipop" + } + }, + { + "type": "named", + "data": { + "name": "Lollipop Outside" + } + } + ] + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Align Barge Center" + } + }, + { + "type": "named", + "data": { + "name": "Net" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/StealK.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/StealK.auto index b41479e5..2c29f013 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/StealK.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/StealK.auto @@ -1 +1,134 @@ -{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align G"}},{"type":"named","data":{"name":"L4"}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Prepare Steal Inside"}},{"type":"race","data":{"commands":[{"type":"named","data":{"name":"Track"}},{"type":"named","data":{"name":"Lollipop"}}]}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align Barge Center"}},{"type":"named","data":{"name":"Net"}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Prepare Steal Outside"}},{"type":"race","data":{"commands":[{"type":"named","data":{"name":"Track"}},{"type":"named","data":{"name":"Lollipop"}}]}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align Barge Center"}},{"type":"named","data":{"name":"Net"}}]}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Align G" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Prepare Steal Inside" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Lollipop" + } + }, + { + "type": "named", + "data": { + "name": "Steal Inside" + } + } + ] + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Align Barge Center" + } + }, + { + "type": "named", + "data": { + "name": "Net" + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Prepare Steal Outside" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Lollipop" + } + }, + { + "type": "named", + "data": { + "name": "Steal Outside" + } + } + ] + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Align Barge Center" + } + }, + { + "type": "named", + "data": { + "name": "Net" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/ThriftyTest/src/main/java/frc/robot/Constants.java b/ThriftyTest/src/main/java/frc/robot/Constants.java index 5d96e14d..6f0912c0 100644 --- a/ThriftyTest/src/main/java/frc/robot/Constants.java +++ b/ThriftyTest/src/main/java/frc/robot/Constants.java @@ -49,8 +49,15 @@ public static class FieldConstants { public static final Rotation2d kStartHeading = Rotation2d.kPi; // facing our alliance - public static final Pose2d kInsideSteal = new Pose2d(7.642, 1.226, Rotation2d.kZero); - public static final Pose2d kOutsideSteal = new Pose2d(7.642, 2.4, Rotation2d.kZero); + public static final Pose2d kInsideStealPrep = new Pose2d(7.642, 1.226, Rotation2d.kZero); + public static final Pose2d kOutsideStealPrep = new Pose2d(7.642, 2.4, Rotation2d.kZero); + public static final Pose2d kInsideSteal = new Pose2d(8, 1.226, Rotation2d.kZero); + public static final Pose2d kOutsideSteal = new Pose2d(8, 2.4, Rotation2d.kZero); + + public static final Pose2d kOutsideLollipopPrep = new Pose2d(7.642, 6.7, Rotation2d.kZero); + public static final Pose2d kInsideLollipopPrep = new Pose2d(7.642, 5.5, Rotation2d.kZero); + public static final Pose2d kOutsideLollipop = new Pose2d(8, 6.7, Rotation2d.kZero); + public static final Pose2d kInsideLollipop = new Pose2d(8, 5.5, Rotation2d.kZero); public static final Pose2d kInsideShoot = new Pose2d(6, 1.2, Rotation2d.kPi); public static final Pose2d kOutsideShoot = new Pose2d(6, 2.4, Rotation2d.kPi); diff --git a/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java index bed9c7aa..ff8ef73b 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java @@ -6,6 +6,7 @@ import frc.robot.superstructure.EnterableState; import frc.robot.superstructure.Superstructure; import frc.robot.superstructure.Superstructure.Subsystems; +import frc.robot.superstructure.states.ManualAlgaeEject; import frc.robot.superstructure.states.SeedAngle; import frc.robot.superstructure.states.SeedPose; import frc.robot.superstructure.states.Stow; @@ -24,6 +25,7 @@ public Command build(Subsystems subsystems) { return subsystems.elevator().go(ElevatorState.L4); } })); + SmartDashboard.putData("Test/FIRE!!!", superstructure.enter(new ManualAlgaeEject())); SmartDashboard.putData("Test/Follow Algae", superstructure.enter(new TrackAlgae())); } diff --git a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java index de822499..da3eb018 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -107,14 +107,35 @@ public void bind(Superstructure superstructure) { .onlyIf(superstructure.holdingAlgae())); // Steal! - APTarget insideSteal = new APTarget(FieldConstants.kInsideSteal) + APTarget insideStealPrep = new APTarget(FieldConstants.kInsideStealPrep) .withRotationRadius(Meters.of(1)); - APTarget outsideSteal = new APTarget(FieldConstants.kOutsideSteal) + APTarget outsideStealPrep = new APTarget(FieldConstants.kOutsideStealPrep) .withRotationRadius(Meters.of(1)); + APTarget outsideSteal = new APTarget(FieldConstants.kOutsideSteal); + APTarget insideSteal = new APTarget(FieldConstants.kInsideSteal); + APTarget insideLollipopPrep = new APTarget(FieldConstants.kInsideLollipop) + .withRotationRadius(Meters.of(1)); + APTarget outsideLollipopPrep = new APTarget(FieldConstants.kOutsideLollipopPrep) + .withRotationRadius(Meters.of(1)); + APTarget insideLollipop = new APTarget(FieldConstants.kInsideLollipop); + APTarget outsideLollipop = new APTarget(FieldConstants.kOutsideLollipop); NamedCommands.registerCommand("Prepare Steal Inside", - superstructure.enter(new Align(insideSteal).allianceRelative())); + superstructure.enter(new Align(insideStealPrep).allianceRelative())); NamedCommands.registerCommand("Prepare Steal Outside", + superstructure.enter(new Align(outsideStealPrep).allianceRelative())); + NamedCommands.registerCommand("Steal Inside", + superstructure.enter(new Align(insideSteal).allianceRelative())); + NamedCommands.registerCommand("Steal Outside", superstructure.enter(new Align(outsideSteal).allianceRelative())); + + NamedCommands.registerCommand("Prepare Lollipop Inside", + superstructure.enter(new Align(insideLollipopPrep).allianceRelative())); + NamedCommands.registerCommand("Prepare Lollipop Outside", + superstructure.enter(new Align(outsideLollipopPrep).allianceRelative())); + NamedCommands.registerCommand("Lollipop Inside", + superstructure.enter(new Align(insideLollipop).allianceRelative())); + NamedCommands.registerCommand("Lollipop Outside", + superstructure.enter(new Align(outsideLollipop).allianceRelative())); // Shoot! APTarget outsideShoot = new APTarget(FieldConstants.kOutsideShoot) diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java index 5d4e9e41..86e3f219 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java @@ -18,8 +18,7 @@ public Command build(Subsystems subsystems) { subsystems.elevator().go(ElevatorState.LowerReef), Commands.parallel( subsystems.pivot().go(PivotState.ReefIntake), - subsystems.algae().intake()), - subsystems.pivot().go(PivotState.ReefExtract)) + subsystems.algae().intake())) .finallyDo(subsystems.elevator()::release) .finallyDo(subsystems.algae()::release) diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java index c3e9edae..cac7c2d5 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java @@ -17,8 +17,7 @@ public Command build(Subsystems subsystems) { return Commands.sequence( subsystems.elevator().go(ElevatorState.UpperReef), subsystems.pivot().go(PivotState.ReefIntake), - subsystems.algae().intake(), - subsystems.pivot().go(PivotState.ReefExtract)) + subsystems.algae().intake()) .finallyDo(subsystems.pivot()::release) .finallyDo(subsystems.elevator()::release) diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java index 5abd6129..910abf3c 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java @@ -135,6 +135,6 @@ private Optional estimateDistance(PhotonTrackedTarget target) { private Optional estimateDistance(double pitch, Distance targetHeight) { double diff = TrackingConstants.kRobotToCamera.getZ() - targetHeight.in(Meters); - return Optional.of(Math.abs(diff / Math.sin(Units.degreesToRadians(pitch)))); + return Optional.of(Math.abs(0.5 * diff / Math.sin(Units.degreesToRadians(pitch)))); } } From 7efcc389c396adaf4dbd85064eecfc096344640a Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Wed, 16 Jul 2025 20:59:57 -0400 Subject: [PATCH 14/38] i dont even know --- ThriftyTest/simgui.json | 9 ++++----- .../main/java/frc/robot/binding/BindingConstants.java | 4 ++-- .../frc/robot/subsystems/elevator/ElevatorState.java | 2 +- .../superstructure/states/LowerReefAlgaeIntake.java | 6 +++--- .../superstructure/states/UpperReefAlgaeIntake.java | 5 +++-- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/ThriftyTest/simgui.json b/ThriftyTest/simgui.json index a60095f1..3c8c55b5 100644 --- a/ThriftyTest/simgui.json +++ b/ThriftyTest/simgui.json @@ -35,6 +35,7 @@ "/SmartDashboard/Test/Drive To Center": "Command", "/SmartDashboard/Test/Elevator Up": "Command", "/SmartDashboard/Test/Enter test state": "Command", + "/SmartDashboard/Test/FIRE!!!": "Command", "/SmartDashboard/Test/Follow Algae": "Command", "/SmartDashboard/Test/L4": "Command", "/SmartDashboard/Test/LowReefAlgaeIntake": "Command", @@ -200,11 +201,6 @@ "window": { "visible": true } - }, - "/SmartDashboard/Test/Follow Algae": { - "window": { - "visible": true - } } } }, @@ -214,6 +210,9 @@ }, "transitory": { "SmartDashboard": { + "Algae": { + "open": true + }, "Algae Tracking": { "open": true }, diff --git a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java index c445acf6..a3cabc99 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java @@ -31,7 +31,7 @@ public static class Operator { public static final int kSecondaryL1 = 14; - public static final int kEjectCoral = Button.kL2.value; + public static final int kEjectCoral = Button.kL1.value; public static final int kLeftAlign = Button.kSquare.value; public static final int kRightAlign = Button.kCircle.value; @@ -49,7 +49,7 @@ public static class Operator { public static final int kStow = Button.kPS.value; - public static final int kCoralIntake = Button.kL1.value; + public static final int kCoralIntake = Button.kR1.value; public static final int kCalibrateElevator = 15; diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java index db640942..129efc8e 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -28,7 +28,7 @@ public enum ElevatorState { /** Height to intake algae from lower reef */ LowerReef(2.0), /** Height to intake algae from upper reef */ - UpperReef(4.5 - 1.5 * ElevatorConstants.kInch); + UpperReef(4.5 - 3 * ElevatorConstants.kInch); private final double m_position; diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java index 86e3f219..d8143624 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/LowerReefAlgaeIntake.java @@ -16,9 +16,9 @@ public LowerReefAlgaeIntake() {} public Command build(Subsystems subsystems) { return Commands.sequence( subsystems.elevator().go(ElevatorState.LowerReef), - Commands.parallel( - subsystems.pivot().go(PivotState.ReefIntake), - subsystems.algae().intake())) + Commands.deadline( + subsystems.algae().intake(), + subsystems.pivot().go(PivotState.ReefIntake))) .finallyDo(subsystems.elevator()::release) .finallyDo(subsystems.algae()::release) diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java index cac7c2d5..8c3c196e 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/UpperReefAlgaeIntake.java @@ -16,8 +16,9 @@ public UpperReefAlgaeIntake() {} public Command build(Subsystems subsystems) { return Commands.sequence( subsystems.elevator().go(ElevatorState.UpperReef), - subsystems.pivot().go(PivotState.ReefIntake), - subsystems.algae().intake()) + Commands.deadline( + subsystems.algae().intake(), + subsystems.pivot().go(PivotState.ReefIntake))) .finallyDo(subsystems.pivot()::release) .finallyDo(subsystems.elevator()::release) From e6054b58aab3e34cf46a9c9dad8ca1ece7037d18 Mon Sep 17 00:00:00 2001 From: HeidiG2007 <78167256+HeidiG2007@users.noreply.github.com> Date: Mon, 21 Jul 2025 20:02:26 -0400 Subject: [PATCH 15/38] Was halving the size of the algae twice (Once in the original measurement of the algae height constants, once when estimating distance). Resulted in halved X and Y coordinates of algae in simulation. Removed halving in estimateDistance, and now simulation looks nicer. Need to test with real bot. --- .../src/main/java/frc/robot/vision/tracking/AlgaeTracker.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java index 910abf3c..5abd6129 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/AlgaeTracker.java @@ -135,6 +135,6 @@ private Optional estimateDistance(PhotonTrackedTarget target) { private Optional estimateDistance(double pitch, Distance targetHeight) { double diff = TrackingConstants.kRobotToCamera.getZ() - targetHeight.in(Meters); - return Optional.of(Math.abs(0.5 * diff / Math.sin(Units.degreesToRadians(pitch)))); + return Optional.of(Math.abs(diff / Math.sin(Units.degreesToRadians(pitch)))); } } From 646069dffb40c4b2510bd358ba0d3f7323685b0c Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Tue, 29 Jul 2025 19:52:19 -0400 Subject: [PATCH 16/38] Bump Autopilot version --- .../drivetrain/CommandSwerveDrivetrain.java | 17 +++++++++-------- ThriftyTest/vendordeps/Autopilot.json | 4 ++-- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 4d33f01a..90f735cb 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -52,6 +52,7 @@ import frc.robot.RobotObserver; import com.therekrab.autopilot.APTarget; import com.therekrab.autopilot.Autopilot; +import com.therekrab.autopilot.Autopilot.APResult; import frc.robot.driveassist.ForceField; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; import frc.robot.utils.FieldUtils; @@ -462,16 +463,16 @@ private LinearVelocity getMaxSpeed() { return DriveConstants.kMaxLinearSpeed; } - private void setVelocity(Transform2d goal) { - double norm = goal.getTranslation().getNorm(); + private void setVelocity(APResult goal) { + double norm = Math.hypot(goal.vx().in(MetersPerSecond), goal.vy().in(MetersPerSecond)); double max = getMaxSpeed().in(MetersPerSecond); if (norm > max) { - goal = goal.times(max / norm); + goal = new APResult(goal.vx().times(max / norm), goal.vy().times(max / norm), goal.targetAngle()); } setControl(m_veloRequest - .withVelocityX(goal.getX()) - .withVelocityY(goal.getY()) - .withTargetDirection(goal.getRotation()) + .withVelocityX(goal.vx()) + .withVelocityY(goal.vy()) + .withTargetDirection(goal.targetAngle()) .withMaxAbsRotationalRate(getMaxRotationalRate())); } @@ -486,7 +487,7 @@ public Command align(Autopilot autopilot, APTarget target) { }), run(() -> { Translation2d velocities = getVelocityComponents(); - Transform2d output = autopilot.calculate(m_estimatedPose, velocities, target); + APResult output = autopilot.calculate(m_estimatedPose, velocities, target); setVelocity(output); })) .until(() -> { @@ -519,7 +520,7 @@ public Command followObject() { new APTarget(m_lastAlgae.toPose2d() .transformBy(new Transform2d(Translation2d.kZero, angle)) .transformBy(DriveConstants.kAlgaeOffset)); - Transform2d output = DriveConstants.kTightAutopilot.calculate( + APResult output = DriveConstants.kTightAutopilot.calculate( m_estimatedPose, getVelocityComponents(), target); diff --git a/ThriftyTest/vendordeps/Autopilot.json b/ThriftyTest/vendordeps/Autopilot.json index 115430ef..f3d0f921 100644 --- a/ThriftyTest/vendordeps/Autopilot.json +++ b/ThriftyTest/vendordeps/Autopilot.json @@ -2,7 +2,7 @@ "name": "Autopilot", "uuid": "4e922d47-9954-4db5-929f-c708cc62e152", "fileName": "Autopilot.json", - "version": "1.2.1", + "version": "1.3.0", "frcYear": "2025", "jsonUrl": "https://therekrab.github.io/autopilot/vendordep.json", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.github.therekrab", "artifactId": "autopilot", - "version": "1.2.1" + "version": "1.3.0" } ], "cppDependencies": [], From a256e9a58fb833708add03149847bafd5ffb7848 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Thu, 31 Jul 2025 13:25:14 -0400 Subject: [PATCH 17/38] Replace track algae with net score. --- .../java/frc/robot/binding/OperatorBindings.java | 14 +++++--------- .../java/frc/robot/superstructure/states/Net.java | 1 + 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java index f4ae1936..539d4278 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java @@ -26,7 +26,6 @@ import frc.robot.superstructure.states.ProcessorPrep; import frc.robot.superstructure.states.DeferredAlign; import frc.robot.superstructure.states.Stow; -import frc.robot.superstructure.states.TrackAlgae; public class OperatorBindings implements Binder { private final CommandPS5Controller m_controller = @@ -44,10 +43,7 @@ public class OperatorBindings implements Binder { private final Trigger m_coralIntake = m_controller.button(Operator.kCoralIntake); private final Trigger m_ejectCoral = m_controller.button(Operator.kEjectCoral); - private final Trigger m_untrackedAlgae = m_controller.button(Operator.kAlgae); - private final Trigger m_trackAlgae = m_controller.button(Operator.kTrackAlgae); - - private final Trigger m_algae = m_untrackedAlgae.or(m_trackAlgae); + private final Trigger m_algae = m_controller.button(Operator.kAlgae); private final Trigger m_algaeGround = m_controller.pov(Operator.kGroundAlgaeIntake); private final Trigger m_algaeHighGround = m_controller.pov(Operator.kHighGroundAlgaeIntake); @@ -55,7 +51,8 @@ public class OperatorBindings implements Binder { private final Trigger m_algaeHighReef = m_controller.button(Operator.kUpperAlgae); private final Trigger m_processor = m_controller.pov(Operator.kProcessor); - private final Trigger m_net = m_controller.pov(Operator.kNet); + private final Trigger m_netPrep = m_controller.pov(Operator.kNetPrep); + private final Trigger m_netScore = m_controller.button(Operator.kScoreNet); private final Trigger m_raiseClimb = m_controller.button(Operator.kRaiseClimb); private final Trigger m_climb = m_controller.button(Operator.kClimb); @@ -70,7 +67,6 @@ public class OperatorBindings implements Binder { public void bind(Superstructure superstructure) { /* algae tracking */ - m_trackAlgae.whileTrue(superstructure.enter(new TrackAlgae())); /* algae intake */ m_algae.and(m_algaeGround).whileTrue(superstructure.enter(new GroundAlgaeIntake())); @@ -81,8 +77,8 @@ public void bind(Superstructure superstructure) { /* algae score */ m_algae.and(m_processor).whileTrue(superstructure.enter(new ProcessorPrep())); m_algae.and(m_processor).onFalse(superstructure.enter(new Processor())); - m_algae.and(m_net).whileTrue(superstructure.enter(new NetPrep())); - m_algae.and(m_net).onFalse(superstructure.enter(new Net())); + m_algae.and(m_netPrep).whileTrue(superstructure.enter(new NetPrep())); + m_netScore.onTrue(superstructure.enter(new Net())); /* coral intake & score */ m_coralIntake.whileTrue(superstructure.enter(new CoralIntake())); diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java index b0130924..9f0154cc 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java @@ -23,6 +23,7 @@ public Command build(Subsystems subsystems) { .finallyDo(subsystems.elevator()::release) .finallyDo(subsystems.pivot()::release) .finallyDo(subsystems.algae()::release) + .onlyIf(subsystems.elevator().ready(ElevatorState.Net) .onlyIf(subsystems.algae().holdingAlgae()); } } From a7694942a52540672b4f638262eb9e7c86c26475 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Thu, 31 Jul 2025 13:27:35 -0400 Subject: [PATCH 18/38] Fix naming issue --- .../src/main/java/frc/robot/binding/BindingConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java index a3cabc99..5aeeaa95 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java @@ -41,7 +41,7 @@ public static class Operator { public static final int kGroundAlgaeIntake = 180; public static final int kProcessor = 90; public static final int kHighGroundAlgaeIntake = 270; - public static final int kNet = 0; + public static final int kNetPrep = 0; public static final int kAlgae = Button.kR2.value; public static final int kRaiseClimb = Button.kCreate.value; @@ -55,7 +55,7 @@ public static class Operator { public static final int kRightFunnel = 11; public static final int kLeftFunnel = 12; - public static final int kTrackAlgae = Button.kL2.value; + public static final int kScoreNet = Button.kL2.value; } } From d96e949a0cd4cdcdfdee992c471141d2c3acc53c Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Thu, 31 Jul 2025 13:28:44 -0400 Subject: [PATCH 19/38] Stow on release of algae+net --- .../src/main/java/frc/robot/binding/OperatorBindings.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java index 539d4278..d97e66a5 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java @@ -77,7 +77,9 @@ public void bind(Superstructure superstructure) { /* algae score */ m_algae.and(m_processor).whileTrue(superstructure.enter(new ProcessorPrep())); m_algae.and(m_processor).onFalse(superstructure.enter(new Processor())); - m_algae.and(m_netPrep).whileTrue(superstructure.enter(new NetPrep())); + m_algae.and(m_netPrep) + .whileTrue(superstructure.enter(new NetPrep())) + .onFalse(superstructure.enter(new Stow())); m_netScore.onTrue(superstructure.enter(new Net())); /* coral intake & score */ From eb35830634164f6b6fc264f07f286b8ee523e42b Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Thu, 31 Jul 2025 13:31:14 -0400 Subject: [PATCH 20/38] Disable algae tracking - In order for this to become a viable option, we need to account for the fisheye warp of the lens. cool idea tho --- .../main/java/frc/robot/vision/tracking/TrackingConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java b/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java index 3b2985c4..fe6a8c89 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/tracking/TrackingConstants.java @@ -10,7 +10,7 @@ import edu.wpi.first.units.measure.Time; class TrackingConstants { - public static final boolean kEnabled = true; + public static final boolean kEnabled = false; protected static final String kCameraName = "algaeman"; /* it is very important that the robot's position be ON THE GROUND (z = 0) */ From 3c181dbcbca55a64db19053a69d9f8e673339a91 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Fri, 1 Aug 2025 18:01:33 -0400 Subject: [PATCH 21/38] Fix paren issue --- .../src/main/java/frc/robot/superstructure/states/Net.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java index 9f0154cc..dcf01f43 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java @@ -23,7 +23,7 @@ public Command build(Subsystems subsystems) { .finallyDo(subsystems.elevator()::release) .finallyDo(subsystems.pivot()::release) .finallyDo(subsystems.algae()::release) - .onlyIf(subsystems.elevator().ready(ElevatorState.Net) + .onlyIf(subsystems.elevator().ready(ElevatorState.Net)) .onlyIf(subsystems.algae().holdingAlgae()); } } From 35b3de693b0be6f9a93416bfc305a1626d2b0388 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Fri, 1 Aug 2025 18:01:49 -0400 Subject: [PATCH 22/38] Add Net Prep command - This helps us, if we want, to parallelize driving and prepping for a net score. --- .../src/main/java/frc/robot/binding/NamedCommandBindings.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java index da3eb018..5a6c4010 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -19,6 +19,7 @@ import frc.robot.superstructure.states.LowerReefAlgaeIntake; import frc.robot.superstructure.states.ManualAlgaeEject; import frc.robot.superstructure.states.Net; +import frc.robot.superstructure.states.NetPrep; import frc.robot.superstructure.states.TrackAlgae; import frc.robot.superstructure.states.UpperReefAlgaeIntake; @@ -40,6 +41,7 @@ public void bind(Superstructure superstructure) { NamedCommands.registerCommand("Lower Algae", superstructure.enter(new LowerReefAlgaeIntake())); NamedCommands.registerCommand("Upper Algae", superstructure.enter(new UpperReefAlgaeIntake())); NamedCommands.registerCommand("Net", superstructure.enter(new Net())); + NamedCommands.registerCommand("Net Prep", superstructure.enter(new NetPrep())); NamedCommands.registerCommand("Algae Stow", superstructure.enter(new AlgaeStow())); NamedCommands.registerCommand("Lollipop", superstructure.enter(new HighGroundAlgaeIntake())); NamedCommands.registerCommand("Eject Algae", superstructure.enter(new ManualAlgaeEject())); From 3b86d2ed1fdbd9e0712b0c080c1493da901c7a02 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Fri, 1 Aug 2025 18:02:23 -0400 Subject: [PATCH 23/38] Removed Center Barge auto (which didn't work because of the swap to Autopilot) --- .../deploy/pathplanner/autos/CenterBarge.auto | 111 ------------------ 1 file changed, 111 deletions(-) delete mode 100644 ThriftyTest/src/main/deploy/pathplanner/autos/CenterBarge.auto diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/CenterBarge.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/CenterBarge.auto deleted file mode 100644 index 7cd04d27..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/CenterBarge.auto +++ /dev/null @@ -1,111 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align H" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align GH" - } - }, - { - "type": "named", - "data": { - "name": "Lower Algae" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "HBarge" - } - }, - { - "type": "named", - "data": { - "name": "Beeline Barge Center" - } - }, - { - "type": "named", - "data": { - "name": "Net" - } - }, - { - "type": "path", - "data": { - "pathName": "BargeIJ" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Upper Algae" - } - }, - { - "type": "named", - "data": { - "name": "Align IJ" - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "IJBarge2" - } - }, - { - "type": "named", - "data": { - "name": "Beeline Barge Center" - } - }, - { - "type": "named", - "data": { - "name": "Net" - } - }, - { - "type": "path", - "data": { - "pathName": "Barge2Backup" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file From 20111bb5baf3725149a8ffe9a4d38e6841b3c04e Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Fri, 1 Aug 2025 18:17:53 -0400 Subject: [PATCH 24/38] initial improvements to auto naming --- ThriftyTest/autogen.sh | 14 +++++++++++++ ThriftyTest/autons/center.txt | 2 +- ThriftyTest/autons/center2.txt | 12 ----------- ThriftyTest/autons/k_steal.txt | 5 ----- ThriftyTest/autons/{left4.txt => left.txt} | 0 ThriftyTest/autons/{right4.txt => right.txt} | 0 ThriftyTest/autons/steal.txt | 5 +++++ .../{k_steal2.txt => steal_and_shoot.txt} | 0 .../main/deploy/pathplanner/autos/Center.auto | 20 ++++++++++++++++++- .../autos/{LollipopK.auto => Lollipop.auto} | 4 ++-- .../autos/{StealK.auto => Steal.auto} | 4 ++-- ...Steal2K.auto => Steal2K (needs work).auto} | 0 .../robot/binding/NamedCommandBindings.java | 2 +- 13 files changed, 44 insertions(+), 24 deletions(-) create mode 100755 ThriftyTest/autogen.sh delete mode 100644 ThriftyTest/autons/center2.txt delete mode 100644 ThriftyTest/autons/k_steal.txt rename ThriftyTest/autons/{left4.txt => left.txt} (100%) rename ThriftyTest/autons/{right4.txt => right.txt} (100%) create mode 100644 ThriftyTest/autons/steal.txt rename ThriftyTest/autons/{k_steal2.txt => steal_and_shoot.txt} (100%) rename ThriftyTest/src/main/deploy/pathplanner/autos/{LollipopK.auto => Lollipop.auto} (96%) rename ThriftyTest/src/main/deploy/pathplanner/autos/{StealK.auto => Steal.auto} (96%) rename ThriftyTest/src/main/deploy/pathplanner/autos/{Steal2K.auto => Steal2K (needs work).auto} (100%) diff --git a/ThriftyTest/autogen.sh b/ThriftyTest/autogen.sh new file mode 100755 index 00000000..bfe038b3 --- /dev/null +++ b/ThriftyTest/autogen.sh @@ -0,0 +1,14 @@ +#!/bin/bash +for fullpath in autons/*; do + if [[ -d "$fullpath" ]]; then + echo "[ INFO ] Skipping directory $fullpath" + continue + fi + filename=`basename $fullpath` + if [[ $filename = "auto-builder.py" ]]; then + continue + fi + name="${filename%.*}" # bash magic from stack overflow + auto_gen $fullpath > src/main/deploy/pathplanner/autos/$name.auto + echo "[ CREATE ] $name.auto" +done diff --git a/ThriftyTest/autons/center.txt b/ThriftyTest/autons/center.txt index f4f538a7..3353f6e7 100644 --- a/ThriftyTest/autons/center.txt +++ b/ThriftyTest/autons/center.txt @@ -7,6 +7,6 @@ Upper Algae & Align IJ Align Barge Left Net Prep Lollipop Outside -Lollipop ? Lollipop Outside +Lollipop Intake ? Lollipop Outside Beeline Barge Left Net diff --git a/ThriftyTest/autons/center2.txt b/ThriftyTest/autons/center2.txt deleted file mode 100644 index b8dc1060..00000000 --- a/ThriftyTest/autons/center2.txt +++ /dev/null @@ -1,12 +0,0 @@ -Align H -L4 -Align Inside Lollipop & High Ground Algae -Beeline Barge Center -Net -Align Outside Lollipop & High Ground Algae -Beeline Barge Left -Net -Align IJ & High Ground Algae -Align Barge Left -Net -Beeline LIntake diff --git a/ThriftyTest/autons/k_steal.txt b/ThriftyTest/autons/k_steal.txt deleted file mode 100644 index e856cb24..00000000 --- a/ThriftyTest/autons/k_steal.txt +++ /dev/null @@ -1,5 +0,0 @@ -Align G + L4 -Prepare Steal Inside + (Lollipop * Track) -Align Barge Outside + Net -Prepare Steal Outside + (Lollipop * Track) -Align Barge Center + Net diff --git a/ThriftyTest/autons/left4.txt b/ThriftyTest/autons/left.txt similarity index 100% rename from ThriftyTest/autons/left4.txt rename to ThriftyTest/autons/left.txt diff --git a/ThriftyTest/autons/right4.txt b/ThriftyTest/autons/right.txt similarity index 100% rename from ThriftyTest/autons/right4.txt rename to ThriftyTest/autons/right.txt diff --git a/ThriftyTest/autons/steal.txt b/ThriftyTest/autons/steal.txt new file mode 100644 index 00000000..54526572 --- /dev/null +++ b/ThriftyTest/autons/steal.txt @@ -0,0 +1,5 @@ +Align G + L4 +Prepare Steal Inside + (Lollipop Intake * Track) +Align Barge Outside + Net +Prepare Steal Outside + (Lollipop Intake * Track) +Align Barge Center + Net diff --git a/ThriftyTest/autons/k_steal2.txt b/ThriftyTest/autons/steal_and_shoot.txt similarity index 100% rename from ThriftyTest/autons/k_steal2.txt rename to ThriftyTest/autons/steal_and_shoot.txt diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto index 4a1786ad..042e2341 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto @@ -41,6 +41,12 @@ "name": "Align Barge Center" } }, + { + "type": "named", + "data": { + "name": "Net Prep" + } + }, { "type": "named", "data": { @@ -72,6 +78,12 @@ "name": "Align Barge Left" } }, + { + "type": "named", + "data": { + "name": "Net Prep" + } + }, { "type": "named", "data": { @@ -91,7 +103,7 @@ { "type": "named", "data": { - "name": "Lollipop" + "name": "Lollipop Intake" } }, { @@ -109,6 +121,12 @@ "name": "Beeline Barge Left" } }, + { + "type": "named", + "data": { + "name": "Net Prep" + } + }, { "type": "named", "data": { diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/LollipopK.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Lollipop.auto similarity index 96% rename from ThriftyTest/src/main/deploy/pathplanner/autos/LollipopK.auto rename to ThriftyTest/src/main/deploy/pathplanner/autos/Lollipop.auto index 64ad9d16..4261936a 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/LollipopK.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Lollipop.auto @@ -40,7 +40,7 @@ { "type": "named", "data": { - "name": "Lollipop" + "name": "Lollipop Intake" } }, { @@ -91,7 +91,7 @@ { "type": "named", "data": { - "name": "Lollipop" + "name": "Lollipop Intake" } }, { diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/StealK.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Steal.auto similarity index 96% rename from ThriftyTest/src/main/deploy/pathplanner/autos/StealK.auto rename to ThriftyTest/src/main/deploy/pathplanner/autos/Steal.auto index 2c29f013..157d4da5 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/StealK.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Steal.auto @@ -40,7 +40,7 @@ { "type": "named", "data": { - "name": "Lollipop" + "name": "Lollipop Intake" } }, { @@ -91,7 +91,7 @@ { "type": "named", "data": { - "name": "Lollipop" + "name": "Lollipop Intake" } }, { diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Steal2K.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Steal2K (needs work).auto similarity index 100% rename from ThriftyTest/src/main/deploy/pathplanner/autos/Steal2K.auto rename to ThriftyTest/src/main/deploy/pathplanner/autos/Steal2K (needs work).auto diff --git a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java index 5a6c4010..3a362560 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -43,7 +43,7 @@ public void bind(Superstructure superstructure) { NamedCommands.registerCommand("Net", superstructure.enter(new Net())); NamedCommands.registerCommand("Net Prep", superstructure.enter(new NetPrep())); NamedCommands.registerCommand("Algae Stow", superstructure.enter(new AlgaeStow())); - NamedCommands.registerCommand("Lollipop", superstructure.enter(new HighGroundAlgaeIntake())); + NamedCommands.registerCommand("Lollipop Intake", superstructure.enter(new HighGroundAlgaeIntake())); NamedCommands.registerCommand("Eject Algae", superstructure.enter(new ManualAlgaeEject())); /* steal hehe */ From c7b9a58d4d46d0c2cbdb70e9333491e60c2a2f50 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Fri, 1 Aug 2025 19:01:05 -0400 Subject: [PATCH 25/38] Update center.txt to prep net then score net --- ThriftyTest/autons/center.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ThriftyTest/autons/center.txt b/ThriftyTest/autons/center.txt index 3353f6e7..5bf573cd 100644 --- a/ThriftyTest/autons/center.txt +++ b/ThriftyTest/autons/center.txt @@ -2,11 +2,11 @@ Align H L4 Align GH & Upper Algae Align Barge Center -Net +Net Prep + Net Upper Algae & Align IJ Align Barge Left -Net +Net Prep + Net Prep Lollipop Outside Lollipop Intake ? Lollipop Outside Beeline Barge Left -Net +Net Prep + Net From 2a7573126f15156dc78dcefb0353aeb4f863f2f4 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Fri, 1 Aug 2025 21:11:33 -0400 Subject: [PATCH 26/38] Finish auton fixes Now they don't use tracking, which is good seeing as it's unreliably bad for lollipops right now. Still want to implement some form of timeout for lollipop intake tho --- ThriftyTest/autons/lollipop.txt | 8 + ThriftyTest/autons/steal-and-shoot.txt | 6 + ThriftyTest/autons/steal.txt | 11 +- ThriftyTest/autons/steal_and_shoot.txt | 5 - .../deploy/pathplanner/autos/4PieceLeft.auto | 202 ------------------ .../deploy/pathplanner/autos/4PieceRight.auto | 202 ------------------ .../main/deploy/pathplanner/autos/Center.auto | 143 +------------ .../deploy/pathplanner/autos/Lollipop.auto | 135 +----------- .../main/deploy/pathplanner/autos/Steal.auto | 135 +----------- .../autos/Steal2K (needs work).auto | 1 - .../main/deploy/pathplanner/autos/left.auto | 1 + .../main/deploy/pathplanner/autos/right.auto | 1 + .../pathplanner/autos/steal-and-shoot.auto | 1 + .../robot/binding/NamedCommandBindings.java | 8 +- 14 files changed, 31 insertions(+), 828 deletions(-) create mode 100644 ThriftyTest/autons/lollipop.txt create mode 100644 ThriftyTest/autons/steal-and-shoot.txt delete mode 100644 ThriftyTest/autons/steal_and_shoot.txt delete mode 100644 ThriftyTest/src/main/deploy/pathplanner/autos/4PieceLeft.auto delete mode 100644 ThriftyTest/src/main/deploy/pathplanner/autos/4PieceRight.auto delete mode 100644 ThriftyTest/src/main/deploy/pathplanner/autos/Steal2K (needs work).auto create mode 100644 ThriftyTest/src/main/deploy/pathplanner/autos/left.auto create mode 100644 ThriftyTest/src/main/deploy/pathplanner/autos/right.auto create mode 100644 ThriftyTest/src/main/deploy/pathplanner/autos/steal-and-shoot.auto diff --git a/ThriftyTest/autons/lollipop.txt b/ThriftyTest/autons/lollipop.txt new file mode 100644 index 00000000..ea71e7dd --- /dev/null +++ b/ThriftyTest/autons/lollipop.txt @@ -0,0 +1,8 @@ +Align G + L4 +Prep Lollipop Inside + (Lollipop Intake ? Lollipop Inside) +Beeline Barge Center +Net Prep + Net +Prep Lollipop Outside + (Lollipop Intake ? Lollipop Outside) +Beeline Barge Left +Net Prep + Net +Beeline LIntake diff --git a/ThriftyTest/autons/steal-and-shoot.txt b/ThriftyTest/autons/steal-and-shoot.txt new file mode 100644 index 00000000..7092b0bb --- /dev/null +++ b/ThriftyTest/autons/steal-and-shoot.txt @@ -0,0 +1,6 @@ +Align G + L4 +Prep Steal Inside + (Lollipop Intake ? Steal Inside) +Shoot Outside + Eject Algae +Prep Steal Outside + (Lollipop Intake ? Steal Outside) +Shoot Inside + Eject Algae +Beeline RIntake diff --git a/ThriftyTest/autons/steal.txt b/ThriftyTest/autons/steal.txt index 54526572..34b81f60 100644 --- a/ThriftyTest/autons/steal.txt +++ b/ThriftyTest/autons/steal.txt @@ -1,5 +1,8 @@ Align G + L4 -Prepare Steal Inside + (Lollipop Intake * Track) -Align Barge Outside + Net -Prepare Steal Outside + (Lollipop Intake * Track) -Align Barge Center + Net +Prep Steal Inside + (Lollipop Intake ? Steal Inside) +Beeline Barge Center +Net Prep + Net +Prep Steal Outside + (Lollipop Intake ? Steal Outside) +Beeline Barge Left +Net Prep + Net +Beeline LIntake diff --git a/ThriftyTest/autons/steal_and_shoot.txt b/ThriftyTest/autons/steal_and_shoot.txt deleted file mode 100644 index 4660017e..00000000 --- a/ThriftyTest/autons/steal_and_shoot.txt +++ /dev/null @@ -1,5 +0,0 @@ -Align G + L4 -Prepare Steal Inside + (Lollipop * Track) -Shoot Outside + Eject Algae -Prepare Steal Outside + (Lollipop * Track) -Shoot Inside + Eject Algae diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceLeft.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceLeft.auto deleted file mode 100644 index bf2049f9..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceLeft.auto +++ /dev/null @@ -1,202 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align J" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Recalibrate" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Coral Wait" - } - }, - { - "type": "named", - "data": { - "name": "Align LIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align K" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Recalibrate" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Coral Wait" - } - }, - { - "type": "named", - "data": { - "name": "Beeline LIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align L" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Recalibrate" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Coral Wait" - } - }, - { - "type": "named", - "data": { - "name": "Beeline LIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align A" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "named", - "data": { - "name": "Recalibrate" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceRight.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceRight.auto deleted file mode 100644 index 62534586..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/4PieceRight.auto +++ /dev/null @@ -1,202 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align E" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Recalibrate" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Coral Wait" - } - }, - { - "type": "named", - "data": { - "name": "Align RIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align D" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Recalibrate" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Coral Wait" - } - }, - { - "type": "named", - "data": { - "name": "Beeline RIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align C" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Recalibrate" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Coral Wait" - } - }, - { - "type": "named", - "data": { - "name": "Beeline RIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align B" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Intake" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "named", - "data": { - "name": "Recalibrate" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto index 042e2341..9a33e49c 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto @@ -1,142 +1 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align H" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align GH" - } - }, - { - "type": "named", - "data": { - "name": "Upper Algae" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align Barge Center" - } - }, - { - "type": "named", - "data": { - "name": "Net Prep" - } - }, - { - "type": "named", - "data": { - "name": "Net" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Upper Algae" - } - }, - { - "type": "named", - "data": { - "name": "Align IJ" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Align Barge Left" - } - }, - { - "type": "named", - "data": { - "name": "Net Prep" - } - }, - { - "type": "named", - "data": { - "name": "Net" - } - }, - { - "type": "named", - "data": { - "name": "Prepare Lollipop Outside" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Lollipop Intake" - } - }, - { - "type": "named", - "data": { - "name": "Lollipop Outside" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Beeline Barge Left" - } - }, - { - "type": "named", - "data": { - "name": "Net Prep" - } - }, - { - "type": "named", - "data": { - "name": "Net" - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align H"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Align GH"}},{"type":"named","data":{"name":"Upper Algae"}}]}},{"type":"named","data":{"name":"Align Barge Center"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Upper Algae"}},{"type":"named","data":{"name":"Align IJ"}}]}},{"type":"named","data":{"name":"Align Barge Left"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}},{"type":"named","data":{"name":"Prep Lollipop Outside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Lollipop Outside"}}]}},{"type":"named","data":{"name":"Beeline Barge Left"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Lollipop.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Lollipop.auto index 4261936a..bfd53739 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Lollipop.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Lollipop.auto @@ -1,134 +1 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align G" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Prepare Lollipop Inside" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Lollipop Intake" - } - }, - { - "type": "named", - "data": { - "name": "Lollipop Inside" - } - } - ] - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align Barge Center" - } - }, - { - "type": "named", - "data": { - "name": "Net" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Prepare Lollipop Outside" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Lollipop Intake" - } - }, - { - "type": "named", - "data": { - "name": "Lollipop Outside" - } - } - ] - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align Barge Center" - } - }, - { - "type": "named", - "data": { - "name": "Net" - } - } - ] - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align G"}},{"type":"named","data":{"name":"L4"}},{"type":"named","data":{"name":"Prep Lollipop Inside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Lollipop Inside"}}]}},{"type":"named","data":{"name":"Beeline Barge Center"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}},{"type":"named","data":{"name":"Prep Lollipop Outside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Lollipop Outside"}}]}},{"type":"named","data":{"name":"Beeline Barge Left"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}},{"type":"named","data":{"name":"Beeline LIntake"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Steal.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Steal.auto index 157d4da5..61fcaf2b 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Steal.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Steal.auto @@ -1,134 +1 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align G" - } - }, - { - "type": "named", - "data": { - "name": "L4" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Prepare Steal Inside" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Lollipop Intake" - } - }, - { - "type": "named", - "data": { - "name": "Steal Inside" - } - } - ] - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align Barge Center" - } - }, - { - "type": "named", - "data": { - "name": "Net" - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Prepare Steal Outside" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Lollipop Intake" - } - }, - { - "type": "named", - "data": { - "name": "Steal Outside" - } - } - ] - } - } - ] - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Align Barge Center" - } - }, - { - "type": "named", - "data": { - "name": "Net" - } - } - ] - } - } - ] - } - }, - "resetOdom": false, - "folder": null, - "choreoAuto": false -} \ No newline at end of file +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align G"}},{"type":"named","data":{"name":"L4"}},{"type":"named","data":{"name":"Prep Steal Inside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Steal Inside"}}]}},{"type":"named","data":{"name":"Beeline Barge Center"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}},{"type":"named","data":{"name":"Prep Steal Outside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Steal Outside"}}]}},{"type":"named","data":{"name":"Beeline Barge Left"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}},{"type":"named","data":{"name":"Beeline LIntake"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Steal2K (needs work).auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Steal2K (needs work).auto deleted file mode 100644 index 16b84912..00000000 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Steal2K (needs work).auto +++ /dev/null @@ -1 +0,0 @@ -{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align G"}},{"type":"named","data":{"name":"L4"}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Prepare Steal Inside"}},{"type":"race","data":{"commands":[{"type":"named","data":{"name":"Track"}},{"type":"named","data":{"name":"Lollipop"}}]}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Shoot Outside"}},{"type":"named","data":{"name":"Eject Algae"}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Prepare Steal Outside"}},{"type":"race","data":{"commands":[{"type":"named","data":{"name":"Track"}},{"type":"named","data":{"name":"Lollipop"}}]}}]}},{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Shoot Inside"}},{"type":"named","data":{"name":"Eject Algae"}}]}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/left.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/left.auto new file mode 100644 index 00000000..d8f477a3 --- /dev/null +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/left.auto @@ -0,0 +1 @@ +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align J"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Recalibrate"}},{"type":"sequential","data":{"commands":[{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Coral Wait"}},{"type":"named","data":{"name":"Align LIntake"}}]}},{"type":"named","data":{"name":"Align K"}}]}}]}},{"type":"named","data":{"name":"Intake"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Recalibrate"}},{"type":"sequential","data":{"commands":[{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Coral Wait"}},{"type":"named","data":{"name":"Beeline LIntake"}}]}},{"type":"named","data":{"name":"Align L"}}]}}]}},{"type":"named","data":{"name":"Intake"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Recalibrate"}},{"type":"sequential","data":{"commands":[{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Coral Wait"}},{"type":"named","data":{"name":"Beeline LIntake"}}]}},{"type":"named","data":{"name":"Align A"}}]}}]}},{"type":"named","data":{"name":"Intake"}},{"type":"named","data":{"name":"L4"}},{"type":"named","data":{"name":"Recalibrate"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/right.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/right.auto new file mode 100644 index 00000000..17ec859f --- /dev/null +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/right.auto @@ -0,0 +1 @@ +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align E"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Recalibrate"}},{"type":"sequential","data":{"commands":[{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Coral Wait"}},{"type":"named","data":{"name":"Align RIntake"}}]}},{"type":"named","data":{"name":"Align D"}}]}}]}},{"type":"named","data":{"name":"Intake"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Recalibrate"}},{"type":"sequential","data":{"commands":[{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Coral Wait"}},{"type":"named","data":{"name":"Beeline RIntake"}}]}},{"type":"named","data":{"name":"Align C"}}]}}]}},{"type":"named","data":{"name":"Intake"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Recalibrate"}},{"type":"sequential","data":{"commands":[{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Coral Wait"}},{"type":"named","data":{"name":"Beeline RIntake"}}]}},{"type":"named","data":{"name":"Align B"}}]}}]}},{"type":"named","data":{"name":"Intake"}},{"type":"named","data":{"name":"L4"}},{"type":"named","data":{"name":"Recalibrate"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/steal-and-shoot.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/steal-and-shoot.auto new file mode 100644 index 00000000..424baac8 --- /dev/null +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/steal-and-shoot.auto @@ -0,0 +1 @@ +{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align G"}},{"type":"named","data":{"name":"L4"}},{"type":"named","data":{"name":"Prep Steal Inside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Steal Inside"}}]}},{"type":"named","data":{"name":"Shoot Outside"}},{"type":"named","data":{"name":"Eject Algae"}},{"type":"named","data":{"name":"Prep Steal Outside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Steal Outside"}}]}},{"type":"named","data":{"name":"Shoot Inside"}},{"type":"named","data":{"name":"Eject Algae"}},{"type":"named","data":{"name":"Beeline RIntake"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} diff --git a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java index 3a362560..857d9f8e 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -121,18 +121,18 @@ public void bind(Superstructure superstructure) { .withRotationRadius(Meters.of(1)); APTarget insideLollipop = new APTarget(FieldConstants.kInsideLollipop); APTarget outsideLollipop = new APTarget(FieldConstants.kOutsideLollipop); - NamedCommands.registerCommand("Prepare Steal Inside", + NamedCommands.registerCommand("Prep Steal Inside", superstructure.enter(new Align(insideStealPrep).allianceRelative())); - NamedCommands.registerCommand("Prepare Steal Outside", + NamedCommands.registerCommand("Prep Steal Outside", superstructure.enter(new Align(outsideStealPrep).allianceRelative())); NamedCommands.registerCommand("Steal Inside", superstructure.enter(new Align(insideSteal).allianceRelative())); NamedCommands.registerCommand("Steal Outside", superstructure.enter(new Align(outsideSteal).allianceRelative())); - NamedCommands.registerCommand("Prepare Lollipop Inside", + NamedCommands.registerCommand("Prep Lollipop Inside", superstructure.enter(new Align(insideLollipopPrep).allianceRelative())); - NamedCommands.registerCommand("Prepare Lollipop Outside", + NamedCommands.registerCommand("Prep Lollipop Outside", superstructure.enter(new Align(outsideLollipopPrep).allianceRelative())); NamedCommands.registerCommand("Lollipop Inside", superstructure.enter(new Align(insideLollipop).allianceRelative())); From 3a466bae0eda137f41f24a9ce6c910fb5cee4388 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Fri, 1 Aug 2025 21:51:42 -0400 Subject: [PATCH 27/38] Enable endless alignment for teleop This means that as long as the button is pressed down, the alignment command will continue to try to correct for error. We'll have to see if this is a good solution, but I guess we'll find out. --- .../drivetrain/CommandSwerveDrivetrain.java | 21 +++++++++++-------- .../subsystems/drivetrain/DriveConstants.java | 2 ++ .../superstructure/states/DeferredAlign.java | 8 ++++--- 3 files changed, 19 insertions(+), 12 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 90f735cb..00b301a7 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -467,7 +467,8 @@ private void setVelocity(APResult goal) { double norm = Math.hypot(goal.vx().in(MetersPerSecond), goal.vy().in(MetersPerSecond)); double max = getMaxSpeed().in(MetersPerSecond); if (norm > max) { - goal = new APResult(goal.vx().times(max / norm), goal.vy().times(max / norm), goal.targetAngle()); + goal = new APResult(goal.vx().times(max / norm), goal.vy().times(max / norm), + goal.targetAngle()); } setControl(m_veloRequest .withVelocityX(goal.vx()) @@ -476,10 +477,7 @@ private void setVelocity(APResult goal) { .withMaxAbsRotationalRate(getMaxRotationalRate())); } - /** - * Drives to a certain point on the field - */ - public Command align(Autopilot autopilot, APTarget target) { + public Command alignEndless(Autopilot autopilot, APTarget target) { return Commands.sequence( runOnce(() -> { RobotObserver.getField().getObject("reference").setPose(target.getReference()); @@ -489,17 +487,22 @@ public Command align(Autopilot autopilot, APTarget target) { Translation2d velocities = getVelocityComponents(); APResult output = autopilot.calculate(m_estimatedPose, velocities, target); setVelocity(output); + + setAligned(autopilot.atTarget(m_estimatedPose, target)); })) - .until(() -> { - return autopilot.atTarget(m_estimatedPose, target); - }) .finallyDo(this::stop) - .finallyDo(interrupted -> setAligned(!interrupted)) .finallyDo(() -> { RobotObserver.getField().getObject("reference").setPoses(); }); } + /** + * Drives to a certain point on the field + */ + public Command align(Autopilot autopilot, APTarget target) { + return alignEndless(autopilot, target).until(() -> autopilot.atTarget(m_estimatedPose, target)); + } + public Command seedLocal(Pose2d pose) { return Commands.runOnce(() -> resetPose(FieldUtils.getLocalPose(pose))) .ignoringDisable(true); diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java index 85e92ef9..fc6a8742 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java @@ -24,6 +24,8 @@ import frc.robot.generated.TunerConstants; public class DriveConstants { + public static final boolean kEnableEndlessAlignment = true; + protected static final PIDConstants kTranslationPID = new PIDConstants(2, 0.0, 0.0); protected static final PIDConstants kRotationPID = new PIDConstants(1.5, 0.0, 0.0); diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java index b3adfd06..4fbb96f9 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java @@ -54,9 +54,11 @@ public Command build(Subsystems subsystems) { Pose2d closest = subsystems.drivetrain().getPose().nearest(locations); APTarget target = new APTarget(closest) .withEntryAngle(closest.getRotation()); - return Commands.sequence( - subsystems.drivetrain().align(DriveConstants.kTightAutopilot, target), - Commands.idle(subsystems.drivetrain())); + if (DriveConstants.kEnableEndlessAlignment) { + return subsystems.drivetrain().alignEndless(DriveConstants.kTightAutopilot, target); + } else { + return subsystems.drivetrain().align(DriveConstants.kTightAutopilot, target); + } }, Set.of(subsystems.drivetrain())); } From 4dd1776cf5e52838287ef997da25a58f7ac163c2 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Sun, 3 Aug 2025 14:49:44 -0400 Subject: [PATCH 28/38] Remove unused coral intake button binding - This makes is more obvious these buttons aren't useful --- .../src/main/java/frc/robot/binding/BindingConstants.java | 2 -- .../src/main/java/frc/robot/binding/OperatorBindings.java | 5 ----- 2 files changed, 7 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java index 5aeeaa95..5e21f2ae 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java @@ -49,8 +49,6 @@ public static class Operator { public static final int kStow = Button.kPS.value; - public static final int kCoralIntake = Button.kR1.value; - public static final int kCalibrateElevator = 15; public static final int kRightFunnel = 11; diff --git a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java index d97e66a5..58844a7e 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java @@ -7,10 +7,8 @@ import frc.robot.superstructure.Superstructure; import frc.robot.superstructure.states.Climb; import frc.robot.superstructure.states.RaiseClimb; -import frc.robot.superstructure.states.CompleteCoralIntake; import frc.robot.superstructure.states.CompleteCoralScore; import frc.robot.superstructure.states.CoralEject; -import frc.robot.superstructure.states.CoralIntake; import frc.robot.superstructure.states.CoralScore; import frc.robot.superstructure.states.CoralScorePrep; import frc.robot.superstructure.states.ElevatorZero; @@ -40,7 +38,6 @@ public class OperatorBindings implements Binder { private final Trigger m_left = m_controller.button(Operator.kLeftAlign); private final Trigger m_right = m_controller.button(Operator.kRightAlign); - private final Trigger m_coralIntake = m_controller.button(Operator.kCoralIntake); private final Trigger m_ejectCoral = m_controller.button(Operator.kEjectCoral); private final Trigger m_algae = m_controller.button(Operator.kAlgae); @@ -83,8 +80,6 @@ public void bind(Superstructure superstructure) { m_netScore.onTrue(superstructure.enter(new Net())); /* coral intake & score */ - m_coralIntake.whileTrue(superstructure.enter(new CoralIntake())); - m_coralIntake.onFalse(superstructure.enter(new CompleteCoralIntake())); m_ejectCoral.whileTrue(superstructure.enter(new CoralEject())); bindCoral(m_l1.and(m_secondaryL1.negate()), CoralLevel.L1, superstructure); bindCoral(m_l1.and(m_secondaryL1), CoralLevel.SecondaryL1, superstructure); From d22f92b00471fc4a51e454f1571f3e81319698cf Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Mon, 4 Aug 2025 18:22:08 -0400 Subject: [PATCH 29/38] Add small wait to CoralWait to avoid mistakes --- .../java/frc/robot/superstructure/states/CoralWait.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralWait.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralWait.java index aae780b3..98a145ac 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralWait.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CoralWait.java @@ -8,11 +8,14 @@ public class CoralWait implements EnterableState { /** * A state that puts the robot into a "waiting" state until a coral can be acquired. - * The state is NOT a coral itake state, however, and does not actually change any mechanisms + * The state is NOT a coral itake state, however, and does not actually change any mechanisms. + * This also will wait 0.1s first before anything else happens to avoid any mistakes (coral leaving scoring mechanism). */ public CoralWait() {} public Command build(Subsystems subsystems) { - return Commands.waitUntil(subsystems.coral().present()); + return Commands.sequence( + Commands.waitSeconds(0.1), + Commands.waitUntil(subsystems.coral().present())); } } From aa2775c9176b5a3cccec91c43848347ce8e6bfc5 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Wed, 6 Aug 2025 09:15:08 -0400 Subject: [PATCH 30/38] Make it easier to enable processor alignment --- .../frc/robot/binding/BindingConstants.java | 1 + .../java/frc/robot/binding/DriveBindings.java | 19 +++++++++++++++++-- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java index 5e21f2ae..edc441d0 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java @@ -21,6 +21,7 @@ public static class Driver { public static final int kLeftAlign = 4; public static final double deadband = 0.01; + public static final boolean kEnableSmartButton = false; } public static class Operator { diff --git a/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java index 7a9d92be..8d0edb05 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java @@ -1,10 +1,15 @@ package frc.robot.binding; import java.util.function.DoubleSupplier; +import com.therekrab.autopilot.APTarget; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.FieldConstants; import frc.robot.binding.BindingConstants.Driver; import frc.robot.superstructure.Superstructure; +import frc.robot.superstructure.states.Align; import frc.robot.superstructure.states.DeferredAlign; import frc.robot.superstructure.states.DeferredAlign.AlignLocation; import frc.robot.superstructure.states.HeadingReset; @@ -31,8 +36,18 @@ public void bind(Superstructure superstructure) { superstructure.setDrive(superstructure.enterWithoutProxy(new TeleopDrive(m_x, m_y, m_rot))); m_resetHeading.onTrue(superstructure.enter(new HeadingReset())); - m_smartAlign.and(superstructure.holdingAlgae().negate()).whileTrue(superstructure.enter( - new DeferredAlign(AlignLocation.Center))); + + Command smartWithAlgae; + if (Driver.kEnableSmartButton) { + smartWithAlgae = superstructure.enter(new Align(new APTarget(FieldConstants.k_processor))); + } else { + smartWithAlgae = Commands.none(); + } + m_smartAlign.whileTrue(Commands.either( + smartWithAlgae, + superstructure.enter(new DeferredAlign(AlignLocation.Center)), + superstructure.holdingAlgae())); + m_leftAlign.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Left))); m_rightAlign.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Right))); } From 70ed3c13ed4a0325435fb61758fa1e810391273c Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Wed, 6 Aug 2025 18:11:42 -0400 Subject: [PATCH 31/38] Enable processor align; disable endless align --- .../java/frc/robot/binding/BindingConstants.java | 2 +- .../drivetrain/CommandSwerveDrivetrain.java | 12 ++++-------- .../robot/subsystems/drivetrain/DriveConstants.java | 2 -- .../robot/superstructure/states/DeferredAlign.java | 6 +----- .../vision/localization/LocalizationConstants.java | 9 +++++---- .../localization/SingleInputPoseEstimator.java | 4 ++-- 6 files changed, 13 insertions(+), 22 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java index edc441d0..fa16c34e 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java @@ -21,7 +21,7 @@ public static class Driver { public static final int kLeftAlign = 4; public static final double deadband = 0.01; - public static final boolean kEnableSmartButton = false; + public static final boolean kEnableSmartButton = true; } public static class Operator { diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 00b301a7..1c613919 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -477,7 +477,7 @@ private void setVelocity(APResult goal) { .withMaxAbsRotationalRate(getMaxRotationalRate())); } - public Command alignEndless(Autopilot autopilot, APTarget target) { + public Command align(Autopilot autopilot, APTarget target) { return Commands.sequence( runOnce(() -> { RobotObserver.getField().getObject("reference").setPose(target.getReference()); @@ -490,19 +490,15 @@ public Command alignEndless(Autopilot autopilot, APTarget target) { setAligned(autopilot.atTarget(m_estimatedPose, target)); })) + .until(() -> { + return autopilot.atTarget(m_estimatedPose, target); + }) .finallyDo(this::stop) .finallyDo(() -> { RobotObserver.getField().getObject("reference").setPoses(); }); } - /** - * Drives to a certain point on the field - */ - public Command align(Autopilot autopilot, APTarget target) { - return alignEndless(autopilot, target).until(() -> autopilot.atTarget(m_estimatedPose, target)); - } - public Command seedLocal(Pose2d pose) { return Commands.runOnce(() -> resetPose(FieldUtils.getLocalPose(pose))) .ignoringDisable(true); diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java index fc6a8742..85e92ef9 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/DriveConstants.java @@ -24,8 +24,6 @@ import frc.robot.generated.TunerConstants; public class DriveConstants { - public static final boolean kEnableEndlessAlignment = true; - protected static final PIDConstants kTranslationPID = new PIDConstants(2, 0.0, 0.0); protected static final PIDConstants kRotationPID = new PIDConstants(1.5, 0.0, 0.0); diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java index 4fbb96f9..8e483c75 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/DeferredAlign.java @@ -54,11 +54,7 @@ public Command build(Subsystems subsystems) { Pose2d closest = subsystems.drivetrain().getPose().nearest(locations); APTarget target = new APTarget(closest) .withEntryAngle(closest.getRotation()); - if (DriveConstants.kEnableEndlessAlignment) { - return subsystems.drivetrain().alignEndless(DriveConstants.kTightAutopilot, target); - } else { - return subsystems.drivetrain().align(DriveConstants.kTightAutopilot, target); - } + return subsystems.drivetrain().align(DriveConstants.kTightAutopilot, target); }, Set.of(subsystems.drivetrain())); } diff --git a/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java b/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java index f38a2dda..4e83ca5d 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java @@ -98,11 +98,12 @@ public class LocalizationConstants { protected static final double kAvgErr = 0.08; protected static final double kErrStdDevs = 0.02; - public static final boolean kEnableReefFilter = true; + public static final boolean kEnableTagFilter = true; - protected static final Set kReefIds = Set.of( - 6, 7, 8, 9, 10, 11, // red tags - 17, 18, 19, 20, 21, 22 // blue tags + protected static final Set kApprovedTagIds = Set.of( + 6, 7, 8, 9, 10, 11, // red reef tags + 17, 18, 19, 20, 21, 22, // blue reef tags + 3, 16 // processor tags are okay as well. ); } diff --git a/ThriftyTest/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java b/ThriftyTest/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java index ca8f08d3..d96b71f3 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/localization/SingleInputPoseEstimator.java @@ -163,8 +163,8 @@ private boolean precheckValidity(PhotonPipelineResult result) { } // Ensure we only accept reef-focused estimates return result.hasTargets() - && (!LocalizationConstants.kEnableReefFilter - || LocalizationConstants.kReefIds.contains(result.getBestTarget().getFiducialId())); + && (!LocalizationConstants.kEnableTagFilter + || LocalizationConstants.kApprovedTagIds.contains(result.getBestTarget().getFiducialId())); } private Optional process(PhotonPipelineResult result, Pose3d pose) { From f32f8fd7d9bdd99bbca72326a650b20f5611e612 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Wed, 6 Aug 2025 20:28:37 -0400 Subject: [PATCH 32/38] fixes --- .../robot/binding/NamedCommandBindings.java | 4 +-- .../frc/robot/binding/OperatorBindings.java | 8 +++--- .../superstructure/states/CheckedNet.java | 20 +++++++++++++++ .../robot/superstructure/states/StowTemp.java | 25 +++++++++++++++++++ .../states/{Net.java => UncheckedNet.java} | 5 ++-- 5 files changed, 54 insertions(+), 8 deletions(-) create mode 100644 ThriftyTest/src/main/java/frc/robot/superstructure/states/CheckedNet.java create mode 100644 ThriftyTest/src/main/java/frc/robot/superstructure/states/StowTemp.java rename ThriftyTest/src/main/java/frc/robot/superstructure/states/{Net.java => UncheckedNet.java} (87%) diff --git a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java index 857d9f8e..9525d71b 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -18,7 +18,7 @@ import frc.robot.superstructure.states.IntakeComplete; import frc.robot.superstructure.states.LowerReefAlgaeIntake; import frc.robot.superstructure.states.ManualAlgaeEject; -import frc.robot.superstructure.states.Net; +import frc.robot.superstructure.states.UncheckedNet; import frc.robot.superstructure.states.NetPrep; import frc.robot.superstructure.states.TrackAlgae; import frc.robot.superstructure.states.UpperReefAlgaeIntake; @@ -40,7 +40,7 @@ public void bind(Superstructure superstructure) { /* algae */ NamedCommands.registerCommand("Lower Algae", superstructure.enter(new LowerReefAlgaeIntake())); NamedCommands.registerCommand("Upper Algae", superstructure.enter(new UpperReefAlgaeIntake())); - NamedCommands.registerCommand("Net", superstructure.enter(new Net())); + NamedCommands.registerCommand("Net", superstructure.enter(new UncheckedNet())); NamedCommands.registerCommand("Net Prep", superstructure.enter(new NetPrep())); NamedCommands.registerCommand("Algae Stow", superstructure.enter(new AlgaeStow())); NamedCommands.registerCommand("Lollipop Intake", superstructure.enter(new HighGroundAlgaeIntake())); diff --git a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java index 58844a7e..865c535d 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java @@ -18,12 +18,14 @@ import frc.robot.superstructure.states.UpperReefAlgaeIntake; import frc.robot.superstructure.states.DeferredAlign.AlignLocation; import frc.robot.superstructure.states.LowerReefAlgaeIntake; -import frc.robot.superstructure.states.Net; +import frc.robot.superstructure.states.UncheckedNet; import frc.robot.superstructure.states.NetPrep; import frc.robot.superstructure.states.Processor; import frc.robot.superstructure.states.ProcessorPrep; +import frc.robot.superstructure.states.CheckedNet; import frc.robot.superstructure.states.DeferredAlign; import frc.robot.superstructure.states.Stow; +import frc.robot.superstructure.states.StowTemp; public class OperatorBindings implements Binder { private final CommandPS5Controller m_controller = @@ -76,8 +78,8 @@ public void bind(Superstructure superstructure) { m_algae.and(m_processor).onFalse(superstructure.enter(new Processor())); m_algae.and(m_netPrep) .whileTrue(superstructure.enter(new NetPrep())) - .onFalse(superstructure.enter(new Stow())); - m_netScore.onTrue(superstructure.enter(new Net())); + .onFalse(superstructure.enter(new StowTemp())); + m_netScore.onTrue(superstructure.enter(new CheckedNet())); /* coral intake & score */ m_ejectCoral.whileTrue(superstructure.enter(new CoralEject())); diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/CheckedNet.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CheckedNet.java new file mode 100644 index 00000000..95ef4d3b --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/CheckedNet.java @@ -0,0 +1,20 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class CheckedNet implements EnterableState { + /** + * A state that scores an algae in the net + */ + public CheckedNet() {} + + public Command build(Subsystems subsystems) { + return new UncheckedNet().build(subsystems) + .onlyIf(subsystems.elevator().ready(ElevatorState.Net)); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/StowTemp.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/StowTemp.java new file mode 100644 index 00000000..502c2cc4 --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/StowTemp.java @@ -0,0 +1,25 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.subsystems.pivot.PivotState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class StowTemp implements EnterableState { + /** + * Stows all the stowables on the robot + */ + public StowTemp() {} + + public Command build(Subsystems subsystems) { + return Commands.parallel( + subsystems.elevator().go(ElevatorState.Stow), + subsystems.pivot().go(PivotState.Stow)) + + .finallyDo(subsystems.elevator()::release) // This allows the elevator to stow itself automatically. + .finallyDo(subsystems.pivot()::release); + } +} + diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/UncheckedNet.java similarity index 87% rename from ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java rename to ThriftyTest/src/main/java/frc/robot/superstructure/states/UncheckedNet.java index dcf01f43..bd4e985a 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/UncheckedNet.java @@ -7,11 +7,11 @@ import frc.robot.superstructure.EnterableState; import frc.robot.superstructure.Superstructure.Subsystems; -public class Net implements EnterableState { +public class UncheckedNet implements EnterableState { /** * A state that scores an algae in the net */ - public Net() {} + public UncheckedNet() {} public Command build(Subsystems subsystems) { return Commands.sequence( @@ -23,7 +23,6 @@ public Command build(Subsystems subsystems) { .finallyDo(subsystems.elevator()::release) .finallyDo(subsystems.pivot()::release) .finallyDo(subsystems.algae()::release) - .onlyIf(subsystems.elevator().ready(ElevatorState.Net)) .onlyIf(subsystems.algae().holdingAlgae()); } } From c3bda698b77eecbb15d12dcd312ed764149c246f Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Thu, 7 Aug 2025 15:58:01 -0400 Subject: [PATCH 33/38] Replace cam1 with much sharper cam1 --- .../frc/robot/vision/localization/LocalizationConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java b/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java index 4e83ca5d..8d49df50 100644 --- a/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/vision/localization/LocalizationConstants.java @@ -47,7 +47,7 @@ public class LocalizationConstants { Map.entry("test", new Transform3d(0, 0, 0, new Rotation3d()))); private static Map kRealCameras = Map.ofEntries( - Map.entry("cam1", new Transform3d( // left tight + Map.entry("cam5", new Transform3d( // left tight new Translation3d(0.256, 0.289, kCameraHeight), new Rotation3d(0, kTightPitch, -kTightYaw + Units.degreesToRadians(0.9)))), Map.entry("cam2", new Transform3d( // left wide From 2ae357fbd54cb69ef91398a313317ddf594164a4 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Thu, 7 Aug 2025 17:59:39 -0400 Subject: [PATCH 34/38] minor improvements, change auto locations --- ThriftyTest/autons/center.txt | 6 +- .../main/deploy/pathplanner/autos/Center.auto | 126 +++++++++++++++++- .../src/main/java/frc/robot/Constants.java | 4 +- .../src/main/java/frc/robot/Robot.java | 1 + .../robot/binding/NamedCommandBindings.java | 2 + .../frc/robot/subsystems/climber/Climber.java | 16 +-- 6 files changed, 140 insertions(+), 15 deletions(-) diff --git a/ThriftyTest/autons/center.txt b/ThriftyTest/autons/center.txt index 5bf573cd..a907cf8f 100644 --- a/ThriftyTest/autons/center.txt +++ b/ThriftyTest/autons/center.txt @@ -6,7 +6,5 @@ Net Prep + Net Upper Algae & Align IJ Align Barge Left Net Prep + Net -Prep Lollipop Outside -Lollipop Intake ? Lollipop Outside -Beeline Barge Left -Net Prep + Net +Stow +Align LIntake \ No newline at end of file diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto index 9a33e49c..541ea4dc 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto @@ -1 +1,125 @@ -{"version":"2025.0","command":{"type":"sequential","data":{"commands":[{"type":"named","data":{"name":"Align H"}},{"type":"named","data":{"name":"L4"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Align GH"}},{"type":"named","data":{"name":"Upper Algae"}}]}},{"type":"named","data":{"name":"Align Barge Center"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}},{"type":"parallel","data":{"commands":[{"type":"named","data":{"name":"Upper Algae"}},{"type":"named","data":{"name":"Align IJ"}}]}},{"type":"named","data":{"name":"Align Barge Left"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}},{"type":"named","data":{"name":"Prep Lollipop Outside"}},{"type":"deadline","data":{"commands":[{"type":"named","data":{"name":"Lollipop Intake"}},{"type":"named","data":{"name":"Lollipop Outside"}}]}},{"type":"named","data":{"name":"Beeline Barge Left"}},{"type":"named","data":{"name":"Net Prep"}},{"type":"named","data":{"name":"Net"}}]}},"resetOdom":false,"folder":null,"choreoAuto":false} +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Align H" + } + }, + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Align GH" + } + }, + { + "type": "named", + "data": { + "name": "Upper Algae" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Align Barge Center" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Net Prep" + } + }, + { + "type": "named", + "data": { + "name": "Net" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Upper Algae" + } + }, + { + "type": "named", + "data": { + "name": "Align IJ" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Align Barge Left" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Net Prep" + } + }, + { + "type": "named", + "data": { + "name": "Net" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Stow" + } + }, + { + "type": "named", + "data": { + "name": "Align LIntake" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/ThriftyTest/src/main/java/frc/robot/Constants.java b/ThriftyTest/src/main/java/frc/robot/Constants.java index 6f0912c0..98e5d14e 100644 --- a/ThriftyTest/src/main/java/frc/robot/Constants.java +++ b/ThriftyTest/src/main/java/frc/robot/Constants.java @@ -40,8 +40,8 @@ public static class FieldConstants { public static final Pose2d kIJ = new Pose2d(5.155, 5.194, Rotation2d.fromDegrees(-120)); public static final Pose2d kBargeFromCenter = - new Pose2d(7.459, 4.717, Rotation2d.fromDegrees(21.0)); - public static final Pose2d kBargeFromLeft = new Pose2d(7.5, 6.6, Rotation2d.fromDegrees(21)); + new Pose2d(7.25, 4.717, Rotation2d.fromDegrees(21.0)); + public static final Pose2d kBargeFromLeft = new Pose2d(7.25, 6.6, Rotation2d.fromDegrees(21)); public static final Pose2d kStartCenter = new Pose2d(7.076, 3.991, Rotation2d.kPi); public static final Pose2d kStartRight = new Pose2d(7.076, 1.2, Rotation2d.kPi); diff --git a/ThriftyTest/src/main/java/frc/robot/Robot.java b/ThriftyTest/src/main/java/frc/robot/Robot.java index 506b267d..997603ad 100644 --- a/ThriftyTest/src/main/java/frc/robot/Robot.java +++ b/ThriftyTest/src/main/java/frc/robot/Robot.java @@ -35,6 +35,7 @@ public Robot() { m_loopTimer = new LoopTimer("Robot"); m_ologger = new OnboardLogger("Robot"); m_ologger.registerDouble("Battery Voltage", RobotController::getBatteryVoltage); + m_ologger.registerBoolean("FMS Connected", DriverStation::isFMSAttached); } @Override diff --git a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java index 9525d71b..5b70f024 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -20,6 +20,7 @@ import frc.robot.superstructure.states.ManualAlgaeEject; import frc.robot.superstructure.states.UncheckedNet; import frc.robot.superstructure.states.NetPrep; +import frc.robot.superstructure.states.StowTemp; import frc.robot.superstructure.states.TrackAlgae; import frc.robot.superstructure.states.UpperReefAlgaeIntake; @@ -45,6 +46,7 @@ public void bind(Superstructure superstructure) { NamedCommands.registerCommand("Algae Stow", superstructure.enter(new AlgaeStow())); NamedCommands.registerCommand("Lollipop Intake", superstructure.enter(new HighGroundAlgaeIntake())); NamedCommands.registerCommand("Eject Algae", superstructure.enter(new ManualAlgaeEject())); + NamedCommands.registerCommand("Stow", superstructure.enter(new StowTemp())); /* steal hehe */ NamedCommands.registerCommand("Track", superstructure.enter(new TrackAlgae())); diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/climber/Climber.java b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/Climber.java index ad4fa0f6..9526e280 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -90,14 +90,14 @@ public Trigger lowered() { } protected void passive() { - if (DriverStation.isFMSAttached() && DriverStation.isTeleop() - && DriverStation.getMatchTime() < 40) { - if (raised().getAsBoolean()) { - setUp(); - } else { - stop(); - } - } + // if (DriverStation.isFMSAttached() && DriverStation.isTeleop() + // && DriverStation.getMatchTime() < 40) { + // if (!raised().getAsBoolean()) { + // setUp(); + // } else { + // stop(); + // } + // } } /** From 2c9f2a1b5eb6504fede8946a85d159511ba13cd3 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Fri, 8 Aug 2025 18:37:04 -0400 Subject: [PATCH 35/38] Update Autopilot version to 1.4.0 --- .../subsystems/drivetrain/CommandSwerveDrivetrain.java | 6 +++--- ThriftyTest/vendordeps/Autopilot.json | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 1c613919..47718c7b 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -484,8 +484,8 @@ public Command align(Autopilot autopilot, APTarget target) { setAligned(false); }), run(() -> { - Translation2d velocities = getVelocityComponents(); - APResult output = autopilot.calculate(m_estimatedPose, velocities, target); + ChassisSpeeds robotRelatiSpeeds = getRobotRelativeSpeeds(); + APResult output = autopilot.calculate(m_estimatedPose, robotRelatiSpeeds, target); setVelocity(output); setAligned(autopilot.atTarget(m_estimatedPose, target)); @@ -521,7 +521,7 @@ public Command followObject() { .transformBy(DriveConstants.kAlgaeOffset)); APResult output = DriveConstants.kTightAutopilot.calculate( m_estimatedPose, - getVelocityComponents(), + getRobotRelativeSpeeds(), target); setVelocity(output); }).onlyWhile(seesAlgae()); diff --git a/ThriftyTest/vendordeps/Autopilot.json b/ThriftyTest/vendordeps/Autopilot.json index f3d0f921..89514027 100644 --- a/ThriftyTest/vendordeps/Autopilot.json +++ b/ThriftyTest/vendordeps/Autopilot.json @@ -2,7 +2,7 @@ "name": "Autopilot", "uuid": "4e922d47-9954-4db5-929f-c708cc62e152", "fileName": "Autopilot.json", - "version": "1.3.0", + "version": "1.4.0", "frcYear": "2025", "jsonUrl": "https://therekrab.github.io/autopilot/vendordep.json", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.github.therekrab", "artifactId": "autopilot", - "version": "1.3.0" + "version": "1.4.0" } ], "cppDependencies": [], From 9e2725bfba44d219cca1dcdf25d6307618e2c2c0 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Sat, 9 Aug 2025 07:39:10 -0400 Subject: [PATCH 36/38] working code! --- ThriftyTest/autons/center.txt | 4 ++-- .../main/deploy/pathplanner/autos/Center.auto | 12 ++++++------ .../src/main/java/frc/robot/Constants.java | 4 ++-- .../frc/robot/binding/BindingConstants.java | 2 +- .../frc/robot/binding/OperatorBindings.java | 19 ++++++++++--------- .../robot/subsystems/elevator/Elevator.java | 7 +++++++ .../superstructure/states/ClimbStowed.java | 3 ++- 7 files changed, 30 insertions(+), 21 deletions(-) diff --git a/ThriftyTest/autons/center.txt b/ThriftyTest/autons/center.txt index a907cf8f..610dbaeb 100644 --- a/ThriftyTest/autons/center.txt +++ b/ThriftyTest/autons/center.txt @@ -1,9 +1,9 @@ Align H L4 -Align GH & Upper Algae +Upper Algae ? Align GH Align Barge Center Net Prep + Net -Upper Algae & Align IJ +Upper Algae ? Align IJ Align Barge Left Net Prep + Net Stow diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto index 541ea4dc..aadd0ce8 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto @@ -17,19 +17,19 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ { "type": "named", "data": { - "name": "Align GH" + "name": "Upper Algae" } }, { "type": "named", "data": { - "name": "Upper Algae" + "name": "Align GH" } } ] @@ -48,20 +48,20 @@ { "type": "named", "data": { - "name": "Net Prep" + "name": "Net" } }, { "type": "named", "data": { - "name": "Net" + "name": "Net Prep" } } ] } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ { diff --git a/ThriftyTest/src/main/java/frc/robot/Constants.java b/ThriftyTest/src/main/java/frc/robot/Constants.java index 98e5d14e..807e453d 100644 --- a/ThriftyTest/src/main/java/frc/robot/Constants.java +++ b/ThriftyTest/src/main/java/frc/robot/Constants.java @@ -40,8 +40,8 @@ public static class FieldConstants { public static final Pose2d kIJ = new Pose2d(5.155, 5.194, Rotation2d.fromDegrees(-120)); public static final Pose2d kBargeFromCenter = - new Pose2d(7.25, 4.717, Rotation2d.fromDegrees(21.0)); - public static final Pose2d kBargeFromLeft = new Pose2d(7.25, 6.6, Rotation2d.fromDegrees(21)); + new Pose2d(7.4, 4.717, Rotation2d.fromDegrees(21.0)); + public static final Pose2d kBargeFromLeft = new Pose2d(7.4, 6.6, Rotation2d.fromDegrees(21)); public static final Pose2d kStartCenter = new Pose2d(7.076, 3.991, Rotation2d.kPi); public static final Pose2d kStartRight = new Pose2d(7.076, 1.2, Rotation2d.kPi); diff --git a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java index fa16c34e..5334bef3 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java @@ -30,7 +30,7 @@ public static class Operator { public static final int kL3 = 90; public static final int kL4 = 0; - public static final int kSecondaryL1 = 14; + public static final int kStowClimb = 14; public static final int kEjectCoral = Button.kL1.value; diff --git a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java index 865c535d..3d4f3781 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java @@ -6,6 +6,7 @@ import frc.robot.binding.BindingConstants.Operator; import frc.robot.superstructure.Superstructure; import frc.robot.superstructure.states.Climb; +import frc.robot.superstructure.states.ClimbStowed; import frc.robot.superstructure.states.RaiseClimb; import frc.robot.superstructure.states.CompleteCoralScore; import frc.robot.superstructure.states.CoralEject; @@ -32,29 +33,29 @@ public class OperatorBindings implements Binder { new CommandPS5Controller(BindingConstants.kOperatorPort); private final Trigger m_l1 = m_controller.pov(Operator.kL1); - private final Trigger m_secondaryL1 = m_controller.pov(Operator.kSecondaryL1); private final Trigger m_l2 = m_controller.pov(Operator.kL2); private final Trigger m_l3 = m_controller.pov(Operator.kL3); private final Trigger m_l4 = m_controller.pov(Operator.kL4); - + private final Trigger m_left = m_controller.button(Operator.kLeftAlign); private final Trigger m_right = m_controller.button(Operator.kRightAlign); - + private final Trigger m_ejectCoral = m_controller.button(Operator.kEjectCoral); - + private final Trigger m_algae = m_controller.button(Operator.kAlgae); - + private final Trigger m_algaeGround = m_controller.pov(Operator.kGroundAlgaeIntake); private final Trigger m_algaeHighGround = m_controller.pov(Operator.kHighGroundAlgaeIntake); private final Trigger m_algaeLowReef = m_controller.button(Operator.kLowerAlgae); private final Trigger m_algaeHighReef = m_controller.button(Operator.kUpperAlgae); - + private final Trigger m_processor = m_controller.pov(Operator.kProcessor); private final Trigger m_netPrep = m_controller.pov(Operator.kNetPrep); private final Trigger m_netScore = m_controller.button(Operator.kScoreNet); - + private final Trigger m_raiseClimb = m_controller.button(Operator.kRaiseClimb); private final Trigger m_climb = m_controller.button(Operator.kClimb); + private final Trigger m_stowClimb = m_controller.button(Operator.kStowClimb); private final Trigger m_stow = m_controller.button(Operator.kStow); @@ -83,8 +84,7 @@ public void bind(Superstructure superstructure) { /* coral intake & score */ m_ejectCoral.whileTrue(superstructure.enter(new CoralEject())); - bindCoral(m_l1.and(m_secondaryL1.negate()), CoralLevel.L1, superstructure); - bindCoral(m_l1.and(m_secondaryL1), CoralLevel.SecondaryL1, superstructure); + bindCoral(m_l1, CoralLevel.L1, superstructure); bindCoral(m_l2, CoralLevel.L2, superstructure); bindCoral(m_l3, CoralLevel.L3, superstructure); bindCoral(m_l4, CoralLevel.L4, superstructure); @@ -96,6 +96,7 @@ public void bind(Superstructure superstructure) { /* climb */ m_climb.whileTrue(superstructure.enter(new Climb())); m_raiseClimb.whileTrue(superstructure.enter(new RaiseClimb())); + m_stowClimb.whileTrue(superstructure.enter(new ClimbStowed())); /* misc */ m_zeroElevator.whileTrue(superstructure.enter(new ElevatorZero())); diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 9d983261..31d019c5 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -54,6 +54,7 @@ public Elevator() { m_inputsLogger = new ElevatorIOInputsLogger(m_inputs); m_ologger = new OnboardLogger("Elevator"); m_ologger.registerString("State", () -> m_reference.toString()); + SmartDashboard.putData("Elevator/Force Net", unsafeNet()); SmartDashboard.putData("Elevator/Lazy Zero", runOnce(m_io::calibrateZero).ignoringDisable(true).withName("Lazy Zero")); m_timer = new LoopTimer("Elevator"); @@ -155,6 +156,12 @@ public Command autoZero() { .withName("Autozero"); } + private Command unsafeNet() { + return Commands.sequence( + runOnce(() -> m_io.setPosition(ElevatorState.Net.position())), + Commands.idle()); + } + public void setPrefireRequirement(Trigger trigger) { m_prefireReq = trigger; } diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/ClimbStowed.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ClimbStowed.java index 94299bf5..77d2634f 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/ClimbStowed.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/ClimbStowed.java @@ -1,9 +1,10 @@ package frc.robot.superstructure.states; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.EnterableState; import frc.robot.superstructure.Superstructure.Subsystems; -public class ClimbStowed { +public class ClimbStowed implements EnterableState { /** * A state that represents the climber at its "stow" position */ From 63e11139f89640682bda31697cf2e623a2d5ca41 Mon Sep 17 00:00:00 2001 From: Nolan Barker Date: Sat, 9 Aug 2025 08:50:27 -0400 Subject: [PATCH 37/38] Attempt to fix coral scoring issues in match --- .../frc/robot/binding/BindingConstants.java | 1 + .../frc/robot/binding/OperatorBindings.java | 12 +++++--- .../robot/subsystems/elevator/Elevator.java | 4 +++ .../states/AutoSelectCoralScore.java | 29 +++++++++++++++++++ 4 files changed, 42 insertions(+), 4 deletions(-) create mode 100644 ThriftyTest/src/main/java/frc/robot/superstructure/states/AutoSelectCoralScore.java diff --git a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java index fa16c34e..4e522ecd 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/BindingConstants.java @@ -55,6 +55,7 @@ public static class Operator { public static final int kRightFunnel = 11; public static final int kLeftFunnel = 12; public static final int kScoreNet = Button.kL2.value; + public static final int kCoralScore = Button.kR1.value; } } diff --git a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java index 865c535d..fd0c9ea8 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java @@ -22,6 +22,7 @@ import frc.robot.superstructure.states.NetPrep; import frc.robot.superstructure.states.Processor; import frc.robot.superstructure.states.ProcessorPrep; +import frc.robot.superstructure.states.AutoSelectCoralScore; import frc.robot.superstructure.states.CheckedNet; import frc.robot.superstructure.states.DeferredAlign; import frc.robot.superstructure.states.Stow; @@ -62,6 +63,8 @@ public class OperatorBindings implements Binder { private final Trigger m_funnelRight = m_controller.button(Operator.kRightFunnel); private final Trigger m_funnel = m_funnelLeft.and(m_funnelRight); + private final Trigger m_coralScore = m_controller.button(Operator.kCoralScore); + private final Trigger m_zeroElevator = m_controller.button(Operator.kCalibrateElevator); public void bind(Superstructure superstructure) { @@ -88,6 +91,7 @@ public void bind(Superstructure superstructure) { bindCoral(m_l2, CoralLevel.L2, superstructure); bindCoral(m_l3, CoralLevel.L3, superstructure); bindCoral(m_l4, CoralLevel.L4, superstructure); + m_coralScore.onTrue(superstructure.enter(new AutoSelectCoralScore())); /* align */ m_left.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Left))); @@ -104,10 +108,10 @@ public void bind(Superstructure superstructure) { } private void bindCoral(Trigger trigger, CoralLevel level, Superstructure superstructure) { - trigger.and(m_algae.negate()).whileTrue(superstructure.enter(new CoralScorePrep(level))); - trigger.and(m_algae.negate()).and(superstructure.aligned()) - .onTrue(superstructure.enter(new CoralScore(level))); - trigger.and(m_algae.negate()).onFalse(superstructure.enter(new CompleteCoralScore(level))); + trigger.and(m_algae.negate()).onTrue(superstructure.enter(new CoralScorePrep(level))); + // trigger.and(m_algae.negate()).and(superstructure.aligned()) + // .onTrue(superstructure.enter(new CoralScore(level))); + // trigger.and(m_algae.negate()).onFalse(superstructure.enter(new CompleteCoralScore(level))); } } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 9d983261..24df757a 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -162,4 +162,8 @@ public void setPrefireRequirement(Trigger trigger) { public void setStayRequirement(Trigger trigger) { m_stay = trigger; } + + public ElevatorState getElevatorState() { + return m_reference; + } } diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/AutoSelectCoralScore.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/AutoSelectCoralScore.java new file mode 100644 index 00000000..1666eabf --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/AutoSelectCoralScore.java @@ -0,0 +1,29 @@ +package frc.robot.superstructure.states; + +import java.util.Map; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.CoralLevel; +import frc.robot.subsystems.elevator.ElevatorState; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class AutoSelectCoralScore implements EnterableState { + + + public Command build(Subsystems subsystems) { + final Map scoreCommands = Map.ofEntries( + Map.entry(ElevatorState.L1, + new CoralScore(CoralLevel.L1).build(subsystems) + .onlyIf(subsystems.elevator().ready(ElevatorState.L1))), + Map.entry(ElevatorState.L2, + new CoralScore(CoralLevel.L2).build(subsystems) + .onlyIf(subsystems.elevator().ready(ElevatorState.L2))), + Map.entry(ElevatorState.L3, + new CoralScore(CoralLevel.L3).build(subsystems) + .onlyIf(subsystems.elevator().ready(ElevatorState.L3))), + Map.entry(ElevatorState.L4, new CoralScore(CoralLevel.L4).build(subsystems) + .onlyIf(subsystems.elevator().ready(ElevatorState.L4)))); + return Commands.select(scoreCommands, subsystems.elevator()::getElevatorState); + } +} From 406d65cc5f1e0e07cfd79a58cafbc857c0c12ca7 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Sat, 9 Aug 2025 09:23:26 -0400 Subject: [PATCH 38/38] Ian likes it better this way --- .../src/main/java/frc/robot/binding/OperatorBindings.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java index 91e2c86b..93154e84 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/OperatorBindings.java @@ -109,7 +109,9 @@ public void bind(Superstructure superstructure) { } private void bindCoral(Trigger trigger, CoralLevel level, Superstructure superstructure) { - trigger.and(m_algae.negate()).onTrue(superstructure.enter(new CoralScorePrep(level))); + trigger.and(m_algae.negate()) + .whileTrue(superstructure.enter(new CoralScorePrep(level))) + .onFalse(superstructure.enter(new StowTemp())); // trigger.and(m_algae.negate()).and(superstructure.aligned()) // .onTrue(superstructure.enter(new CoralScore(level))); // trigger.and(m_algae.negate()).onFalse(superstructure.enter(new CompleteCoralScore(level)));