From 7b6d84ad1e710aa107e11e9a6d8574ebc7924d16 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 2 Feb 2024 22:49:50 -0500 Subject: [PATCH 1/3] Add vision data validation --- .../stuypulse/robot/util/OV2311Camera.java | 28 ++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/stuypulse/robot/util/OV2311Camera.java b/src/main/java/com/stuypulse/robot/util/OV2311Camera.java index c468a10..1262ce7 100644 --- a/src/main/java/com/stuypulse/robot/util/OV2311Camera.java +++ b/src/main/java/com/stuypulse/robot/util/OV2311Camera.java @@ -112,6 +112,28 @@ private Pose3d getRobotPose() { new Transform3d(offset.getTranslation(), offset.getRotation()).inverse()); } + private boolean isValidData(VisionData data) { + for (long id : data.tids) { + boolean found = false; + for (Fiducial f : Field.FIDUCIALS) { + if (f.getID() == id) { + found = true; + break; + } + } + + if (!found) { + return false; + } + } + + if (Double.isNaN(data.robotPose.getX()) || data.robotPose.getX() < 0 || data.robotPose.getX() > Field.WIDTH) return false; + if (Double.isNaN(data.robotPose.getY()) || data.robotPose.getY() < 0 || data.robotPose.getY() > Field.HEIGHT) return false; + if (Double.isNaN(data.robotPose.getZ()) || data.robotPose.getZ() < -1 || data.robotPose.getZ() > 1) return false; + + return true; + } + public Optional getVisionData() { updateData(); @@ -123,7 +145,11 @@ public Optional getVisionData() { LogPose3d.logPose3d("Vision/Robot Pose", getRobotPose()); - return Optional.of(new VisionData(rawIdData, offset, getRobotPose(), timestamp)); + VisionData data = new VisionData(rawIdData, offset, getRobotPose(), timestamp); + + if (!isValidData(data)) return Optional.empty(); + + return Optional.of(data); } public void setLayout(Fiducial[] layout) { From 4298b51f520598f2154cc16c070d8cbf378a38d7 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 2 Feb 2024 22:50:03 -0500 Subject: [PATCH 2/3] Add default low ramp rate --- src/main/java/com/stuypulse/robot/constants/Motors.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 66b262a..bdb665a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -61,7 +61,7 @@ public CANSparkMaxConfig( } public CANSparkMaxConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps) { - this(inverted, idleMode, currentLimitAmps, 0.0); + this(inverted, idleMode, currentLimitAmps, 0.05); } public CANSparkMaxConfig(boolean inverted, IdleMode idleMode) { @@ -95,7 +95,7 @@ public CANSparkFlexConfig( } public CANSparkFlexConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps) { - this(inverted, idleMode, currentLimitAmps, 0.0); + this(inverted, idleMode, currentLimitAmps, 0.05); } public CANSparkFlexConfig(boolean inverted, IdleMode idleMode) { From 0848677f3b2eb7f44b493b3341ecdc38bf17fd24 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Fri, 2 Feb 2024 22:50:54 -0500 Subject: [PATCH 3/3] Add swerve to starting auto position command --- .../com/stuypulse/robot/RobotContainer.java | 4 + .../swerve/SwerveDriveToAutoStart.java | 92 +++++++++++++++++++ .../stuypulse/robot/constants/Settings.java | 6 +- 3 files changed, 99 insertions(+), 3 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index ff70618..97a3c3d 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -11,6 +11,7 @@ import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; import com.stuypulse.robot.commands.swerve.SwerveDriveDriveToScore; import com.stuypulse.robot.commands.swerve.SwerveDriveResetHeading; +import com.stuypulse.robot.commands.swerve.SwerveDriveToAutoStart; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToScore; import com.stuypulse.robot.commands.swerve.SwerveDriveWithAiming; @@ -101,6 +102,9 @@ private void configureButtonBindings() { driver.getLeftButton().whileTrue(new SwerveDriveToScore()); driver.getRightButton().whileTrue(new SwerveDriveDriveToScore(driver)); // driver.getBottomButton().whileTrue(AutoBuilder.pathfindToPose(new Pose2d(), new PathConstraints(3, 4, Units.degreesToRadians(540), Units.degreesToRadians(720)), 0, 0)); + + driver.getStartButton() + .whileTrue(new SwerveDriveToAutoStart(() -> autonChooser.getSelected().getName())); } /**************/ diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java new file mode 100644 index 0000000..9fb8940 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToAutoStart.java @@ -0,0 +1,92 @@ +/************************ PROJECT JON *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/ +/* This work is licensed under the terms of the MIT license. */ +/**************************************************************/ + +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; + +import static com.stuypulse.robot.constants.Settings.Alignment.*; + +import java.util.function.Supplier; + +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.HolonomicController; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + + +public class SwerveDriveToAutoStart extends Command { + private final SwerveDrive swerve; + private Pose2d targetPose; + + // Holonomic control + private final HolonomicController controller; + private final BStream aligned; + + private final FieldObject2d targetPose2d; + + private final Supplier name; + + public SwerveDriveToAutoStart(Supplier name) { + this.swerve = SwerveDrive.getInstance(); + this.name = name; + + controller = new HolonomicController( + new PIDController(Translation.P,Translation.I,Translation.D), + new PIDController(Translation.P, Translation.I, Translation.D), + new AnglePIDController(Rotation.P, Rotation.I, Rotation.D)); + + SmartDashboard.putData("Alignment/Controller", controller); + + aligned = BStream.create(this::isAligned).filtered(new BDebounceRC.Rising(DEBOUNCE_TIME)); + + targetPose2d = AbstractOdometry.getInstance().getField().getObject("Target Pose"); + addRequirements(swerve); + } + + private boolean isAligned() { + return controller.isDone(X_TOLERANCE.get(), Y_TOLERANCE.get(), ANGLE_TOLERANCE.get()); + } + + @Override + public void initialize() { + try { + targetPose = PathPlannerAuto.getStaringPoseFromAutoFile(name.get()); + } catch (Exception e) { + targetPose = Odometry.getInstance().getPose(); + } + } + + @Override + public void execute() { + Pose2d currentPose = AbstractOdometry.getInstance().getPose(); + + targetPose2d.setPose(targetPose); + + controller.update(targetPose, currentPose); + swerve.setChassisSpeeds(controller.getOutput()); + } + + @Override + public boolean isFinished(){ + return aligned.get(); + } + + public void end(boolean interupted) { + swerve.stop(); + targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN)); + } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 0bcf2e8..c69d84e 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -158,9 +158,9 @@ public interface Intake { public interface Alignment { SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.15); - SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); - SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); - SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5); + SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.05); + SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.05); + SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 3); SmartNumber TARGET_DISTANCE_IN = new SmartNumber("Alignment/Target Distance (in)", 110); SmartNumber TAKEOVER_DISTANCE_IN = new SmartNumber("Alignment/Takeover Distance (in)", 50);