From d81c4b381d06804bfcaff0c5682f4edc009bc9c7 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 9 Jan 2025 12:57:04 -0700 Subject: [PATCH 1/6] Add code from photonvision --- simgui-ds.json | 2 +- src/main/java/frc4388/robot/Constants.java | 19 ++ .../java/frc4388/robot/RobotContainer.java | 5 +- src/main/java/frc4388/robot/RobotMap.java | 6 + .../robot/commands/GotoPositionCommand.java | 43 ++++ .../frc4388/robot/subsystems/SwerveDrive.java | 15 +- .../java/frc4388/robot/subsystems/Vision.java | 242 ++++++++++++++++++ src/main/java/frc4388/utility/CanDevice.java | 10 +- vendordeps/PathplannerLib-2025.1.1.json | 38 +++ vendordeps/photonlib.json | 71 +++++ 10 files changed, 436 insertions(+), 15 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/GotoPositionCommand.java create mode 100644 src/main/java/frc4388/robot/subsystems/Vision.java create mode 100644 vendordeps/PathplannerLib-2025.1.1.json create mode 100644 vendordeps/photonlib.json diff --git a/simgui-ds.json b/simgui-ds.json index 49cf3aa..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,7 +91,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000" + "guid": "Keyboard0" } ] } diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3bfe1d4..61cbf07 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -27,6 +27,15 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; @@ -267,6 +276,16 @@ public final class Constants { } public static final class VisionConstants { + public static final String CAMERA_NAME = "photonvision"; + + public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(), new Rotation3d()); + + public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + + // The standard deviations of our vision estimated poses, which affect correction rate + // (Fake values. Experiment and determine estimation noise on an actual robot.) + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); } public static final class DriveConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 63f9eec..3be1700 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -33,6 +33,7 @@ import frc4388.robot.commands.Swerve.neoJoystickRecorder; // Subsystems // import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.SwerveDrive; // Utilites @@ -53,8 +54,10 @@ public class RobotContainer { /* Subsystems */ // private final LED m_robotLED = new LED(); + public final Vision m_vision = new Vision(m_robotMap.camera); + + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4990efa..3541a58 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -8,6 +8,9 @@ package frc4388.robot; import com.ctre.phoenix6.hardware.TalonFX; + +import org.photonvision.PhotonCamera; + import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrain; @@ -18,6 +21,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; // import edu.wpi.first.wpilibj.motorcontrol.Spark; // import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.Constants.VisionConstants; // import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; @@ -28,6 +32,8 @@ import frc4388.utility.RobotGyro; public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); + + public PhotonCamera camera = new PhotonCamera(VisionConstants.CAMERA_NAME); public RobotMap() { configureDriveMotorControllers(); diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java new file mode 100644 index 0000000..070fc21 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -0,0 +1,43 @@ +package frc4388.robot.commands; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.UtilityStructs.AutoRecordingFrame; +import frc4388.utility.controller.VirtualController; + +public class GotoPositionCommand extends Command { + private boolean isFinished = false; + + SwerveDrive swerveDrive; + + /** + * Command to drive robot to position. + * @param SwerveDrive m_robotSwerveDrive + */ + public GotoPositionCommand(SwerveDrive swerveDrive) { + this.swerveDrive = swerveDrive; + } + + @Override + public void initialize() { + isFinished = false; + } + + @Override + public void execute() { + + + } + + @Override + public void end(boolean interrupted) { + + } + + @Override + public boolean isFinished() { + return isFinished; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index c685f23..09b3bb5 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -33,6 +33,8 @@ import frc4388.utility.Status.ReportLevel; public class SwerveDrive extends Subsystem { private SwerveDrivetrain swerveDriveTrain; + + private Vision vision; private int gear_index = SwerveDriveConstants.STARTING_GEAR; private boolean stopped = false; @@ -45,16 +47,14 @@ public class SwerveDrive extends Subsystem { public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - private Field2d field = new Field2d(); - /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { + public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { super(); - SmartDashboard.putData(field); - this.swerveDriveTrain = swerveDriveTrain; + this.vision = vision; } + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); // // rightStick.getAngle() @@ -172,10 +172,9 @@ public class SwerveDrive extends Subsystem { SmartDashboard.putNumber("Gyro", getGyroAngle()); SmartDashboard.putNumber("RotTartget", rotTarget); - Optional e = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()); + vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(Vision.getTime())); - if(e.isPresent()) - field.setRobotPose(e.get()); + // if(e.isPresent()) } private void reset_index() { diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java new file mode 100644 index 0000000..899dba0 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -0,0 +1,242 @@ +package frc4388.robot.subsystems; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +import java.util.List; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonTrackedTarget; + +import com.ctre.phoenix6.Utils; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.Constants.VisionConstants; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; + +public class Vision extends Subsystem { + + private PhotonCamera camera; + + private boolean isTag = false; + private Pose2d lastVisionPose = new Pose2d(); + private Pose2d lastPhysOdomPose = new Pose2d(); + + private Matrix curStdDevs; + private final PhotonPoseEstimator photonEstimator; + + private Field2d field = new Field2d(); + + public Vision(PhotonCamera camera){ + this.camera = camera; + SmartDashboard.putData(field); + + photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS); + photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } + + @Override + public void periodic() { + var result = camera.getLatestResult(); + isTag = result.hasTargets(); + + if(!isTag){ + field.setRobotPose(lastPhysOdomPose); + return; + } + + var EstimatedRobotPose = getEstimatedGlobalPose(); + + // In case the pose estimator fails to estimate the pose, fallback to physical odometry. + if(EstimatedRobotPose.isEmpty()){ + field.setRobotPose(lastPhysOdomPose); + isTag = false; + return; + } + + field.setRobotPose(lastVisionPose); + } + + + + + /** + * The latest estimated robot pose on the field from vision data. This may be empty. This should + * only be called once per loop. + * + *

Also includes updates for the standard deviations, which can (optionally) be retrieved with + * {@link getEstimationStdDevs} + * + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets + * used for estimation. + */ + public Optional getEstimatedGlobalPose() { + Optional visionEst = Optional.empty(); + for (var change : camera.getAllUnreadResults()) { + visionEst = photonEstimator.update(change); + updateEstimationStdDevs(visionEst, change.getTargets()); + + // if (Robot.isSimulation()) { + // visionEst.ifPresentOrElse( + // est -> + // getSimDebugField() + // .getObject("VisionEstimation") + // .setPose(est.estimatedPose.toPose2d()), + // () -> { + // getSimDebugField().getObject("VisionEstimation").setPoses(); + // }); + // } + } + return visionEst; + } + + + + + + + + /** + * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard + * deviations based on number of tags, estimation strategy, and distance from the tags. + * + * @param estimatedPose The estimated pose to guess standard deviations for. + * @param targets All targets in this camera frame + */ + private void updateEstimationStdDevs( + Optional estimatedPose, List targets) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + + } else { + // Pose present. Start running Heuristic + var estStdDevs = VisionConstants.kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an average-distance metric + for (var tgt : targets) { + var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + curStdDevs = estStdDevs; + } + } + } + + /** + * Returns the latest standard deviations of the estimated pose from {@link + * #getEstimatedGlobalPose()}, for use with {@link + * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should + * only be used when there are targets visible. + */ + public Matrix getEstimationStdDevs() { + return curStdDevs; + } + + + + + + + + + + public void setLastOdomPose(Optional pose){ + if(pose.isPresent()) + lastPhysOdomPose = pose.get(); + } + + public Pose2d getPose2d() { + if(isTag) + return lastVisionPose; + else + return lastPhysOdomPose; + } + + public static double getTime() { + return Utils.getCurrentTimeSeconds(); + } + + + + + + + + + + + @Override + public String getSubsystemName() { + return "Vision"; + } + + + ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") + .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .withSize(2, 2); + + GenericEntry sbTag = subsystemLayout + .add("Tag Detected", false) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); + +// GenericEntry sbShiftState = subsystemLayout +// .add("Shift State", 0) +// .withWidget(BuiltInWidgets.kNumberBar) +// .getEntry(); + + + @Override + public void queryStatus() { + sbTag.setBoolean(isTag); + field.setRobotPose(getPose2d()); + } + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + return status; + } + +} diff --git a/src/main/java/frc4388/utility/CanDevice.java b/src/main/java/frc4388/utility/CanDevice.java index ec05fdb..1805403 100644 --- a/src/main/java/frc4388/utility/CanDevice.java +++ b/src/main/java/frc4388/utility/CanDevice.java @@ -35,13 +35,13 @@ public class CanDevice { System.out.println(getName() + " - " + str); } - public Status queryStatus() { - Status s = new Status(); + // public Status queryStatus() { + // Status s = new Status(); - s.addReport(ReportLevel.INFO, "TODO"); + // s.addReport(ReportLevel.INFO, "TODO"); - return s; - } + // return s; + // } public Status diagnosticStatus() { Status s = new Status(); diff --git a/vendordeps/PathplannerLib-2025.1.1.json b/vendordeps/PathplannerLib-2025.1.1.json new file mode 100644 index 0000000..6322388 --- /dev/null +++ b/vendordeps/PathplannerLib-2025.1.1.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2025.1.1.json", + "name": "PathplannerLib", + "version": "2025.1.1", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2025", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2025.1.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2025.1.1", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..db43d6d --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.0.0-beta-8", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-8", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.0.0-beta-8", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-8", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.0.0-beta-8" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.0.0-beta-8" + } + ] +} \ No newline at end of file From 76b53c95a7283616a15009c186b973b5d170f716 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 9 Jan 2025 19:40:37 -0700 Subject: [PATCH 2/6] e --- src/main/java/frc4388/robot/Constants.java | 6 ++--- .../java/frc4388/robot/RobotContainer.java | 1 + .../frc4388/robot/subsystems/SwerveDrive.java | 9 +++++++- .../java/frc4388/robot/subsystems/Vision.java | 22 ++++++++++++++++--- 4 files changed, 31 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 61cbf07..bd8ee92 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -276,11 +276,11 @@ public final class Constants { } public static final class VisionConstants { - public static final String CAMERA_NAME = "photonvision"; + public static final String CAMERA_NAME = "Camera_Module_v1"; public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(), new Rotation3d()); - public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); // The standard deviations of our vision estimated poses, which affect correction rate // (Fake values. Experiment and determine estimation noise on an actual robot.) @@ -305,4 +305,4 @@ public final class Constants { public static final double LEFT_AXIS_DEADBAND = 0.1; } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3be1700..bd0889d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -57,6 +57,7 @@ public class RobotContainer { public final Vision m_vision = new Vision(m_robotMap.camera); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); + // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); /* Controllers */ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 09b3bb5..4894fb4 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -49,6 +49,7 @@ public class SwerveDrive extends Subsystem { /** Creates a new SwerveDrive. */ public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { + // public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { super(); this.swerveDriveTrain = swerveDriveTrain; @@ -172,7 +173,13 @@ public class SwerveDrive extends Subsystem { SmartDashboard.putNumber("Gyro", getGyroAngle()); SmartDashboard.putNumber("RotTartget", rotTarget); - vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(Vision.getTime())); + double time = Vision.getTime(); + + vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); + + if(vision.isTag()){ + swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); + } // if(e.isPresent()) } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 899dba0..6848989 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -56,9 +56,15 @@ public class Vision extends Subsystem { public void periodic() { var result = camera.getLatestResult(); isTag = result.hasTargets(); + + field.setRobotPose(lastPhysOdomPose); + + // sbCamConnected.setBoolean(camera); + + // System.out.println(isTag); if(!isTag){ - field.setRobotPose(lastPhysOdomPose); + sbTag.setBoolean(isTag); return; } @@ -66,12 +72,11 @@ public class Vision extends Subsystem { // In case the pose estimator fails to estimate the pose, fallback to physical odometry. if(EstimatedRobotPose.isEmpty()){ + sbTag.setBoolean(isTag); field.setRobotPose(lastPhysOdomPose); isTag = false; return; } - - field.setRobotPose(lastVisionPose); } @@ -196,6 +201,11 @@ public class Vision extends Subsystem { return Utils.getCurrentTimeSeconds(); } + public boolean isTag(){ + return isTag; + } + + @@ -219,6 +229,11 @@ public class Vision extends Subsystem { .add("Tag Detected", false) .withWidget(BuiltInWidgets.kBooleanBox) .getEntry(); + + GenericEntry sbCamConnected = subsystemLayout + .add("Camera Connnected", false) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); // GenericEntry sbShiftState = subsystemLayout // .add("Shift State", 0) @@ -229,6 +244,7 @@ public class Vision extends Subsystem { @Override public void queryStatus() { sbTag.setBoolean(isTag); + sbCamConnected.setBoolean(camera.isConnected()); field.setRobotPose(getPose2d()); } From 33629e9fdb461bdab9199994cc0c3ed75550bb55 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Fri, 10 Jan 2025 21:36:30 -0700 Subject: [PATCH 3/6] Make auto goto position work, work on vision locaalization --- src/main/java/frc4388/robot/Constants.java | 7 +- .../java/frc4388/robot/RobotContainer.java | 4 +- .../robot/commands/GotoPositionCommand.java | 136 +++++++++++++++--- .../frc4388/robot/subsystems/SwerveDrive.java | 2 +- .../java/frc4388/robot/subsystems/Vision.java | 62 +++++--- 5 files changed, 172 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index bd8ee92..34cceba 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -31,6 +31,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +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.geometry.Translation3d; @@ -275,10 +276,12 @@ public final class Constants { public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } - public static final class VisionConstants { + public static final class VisionConstants { public static final String CAMERA_NAME = "Camera_Module_v1"; - public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(), new Rotation3d()); + public static final Rotation2d ROTATE_BY = Rotation2d.fromDegrees(180); + + public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(.3048, .2413, .2794), new Rotation3d(0,0.52333,0)); public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bd0889d..d459c99 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; // Autos import frc4388.utility.controller.VirtualController; +import frc4388.robot.commands.GotoPositionCommand; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; @@ -213,7 +214,8 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return autoPlayback; + //return autoPlayback; + return new GotoPositionCommand(m_robotSwerveDrive, m_vision); } /** diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java index 070fc21..f481505 100644 --- a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -1,43 +1,147 @@ package frc4388.robot.commands; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Vision; +import frc4388.utility.Gains; import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; import frc4388.utility.UtilityStructs.AutoRecordingFrame; import frc4388.utility.controller.VirtualController; public class GotoPositionCommand extends Command { - private boolean isFinished = false; + + + private Translation2d translation2d= new Translation2d(0,0); + static Gains gains = new Gains(3,0,0); + static double tolerance = 0; + + private PID xPID = new PID(gains, 0); + private PID yPID = new PID(gains, 0); SwerveDrive swerveDrive; + Vision vision; /** * Command to drive robot to position. * @param SwerveDrive m_robotSwerveDrive */ - public GotoPositionCommand(SwerveDrive swerveDrive) { + + public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) { this.swerveDrive = swerveDrive; + this.vision = vision; + addRequirements(swerveDrive); } @Override public void initialize() { - isFinished = false; - } - - @Override - public void execute() { - - - } - - @Override - public void end(boolean interrupted) { - + xPID.initialize(); + yPID.initialize(); } @Override - public boolean isFinished() { - return isFinished; + public void execute() { + double xerr = translation2d.getX() - vision.getPose2d().getX(); + double yerr = translation2d.getY() - vision.getPose2d().getY(); + + SmartDashboard.putNumber("PID X Error", xerr); + SmartDashboard.putNumber("PID Y Error", yerr); + + double xoutput = xPID.update(xerr); + double youtput = yPID.update(yerr); + + Translation2d leftStick = new Translation2d( + Math.max(Math.min(youtput, 1), -1), + Math.max(Math.min(xoutput, 1), -1) + ); + + Translation2d rightStick = new Translation2d(); + + SmartDashboard.putNumber("PID X Output", xoutput); + SmartDashboard.putNumber("PID Y Output", youtput); + + swerveDrive.driveWithInput(leftStick, rightStick, true); + } + // @Override + // public void end(boolean interrupted) { + + // } + + + + // @Override + // public double getError() { + // return e; + // } + + + + // @Override + // public void runWithOutput(double output) { + // // TODO Auto-generated method stub + // Translation2d leftStick = new Translation2d(Math.max(Math.min(output, 1), -1),0); + // Translation2d rightStick = new Translation2d(); + // // System.out.println("Output = " + -output); + // SmartDashboard.putNumber("PID Output", output); + // swerveDrive.driveWithInput(leftStick, rightStick, true); + // } + + + + + + + + + + + + + + + + + + private class PID { + protected Gains gains; + private double output = 0; + private double tolerance = 0; + + /** Creates a new PelvicInflammatoryDisease. */ + public PID(double kp, double ki, double kd, double kf, double tolerance) { + gains = new Gains(kp, ki, kd, kf, 0); + this.tolerance = tolerance; + } + + public PID(Gains gains, double tolerance) { + this.gains = gains; + this.tolerance = tolerance; + } + + // Called when the command is initially scheduled. + public final void initialize() { + output = 0; + } + + private double prevError, cumError = 0; + + // Called every time the scheduler runs while the command is scheduled. + public double update(double error) { + cumError += error * .02; // 20 ms + double delta = error - prevError; + + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; + + return output; + } + + // // Returns true when the command should end. + // public final boolean isFinished() { + // return Math.abs(getError()) < tolerance; + // } } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 4894fb4..d33686e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -26,7 +26,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.robot.Constants.SwerveDriveConstants; - +import frc4388.robot.Constants.VisionConstants; import frc4388.utility.Status; import frc4388.utility.Subsystem; import frc4388.utility.Status.ReportLevel; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 6848989..2eebceb 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -3,6 +3,8 @@ package frc4388.robot.subsystems; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -16,6 +18,7 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.targeting.PhotonTrackedTarget; import com.ctre.phoenix6.Utils; @@ -43,6 +46,21 @@ public class Vision extends Subsystem { private final PhotonPoseEstimator photonEstimator; private Field2d field = new Field2d(); + + + ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") + .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .withSize(2, 2); + + GenericEntry sbTag = subsystemLayout + .add("Tag Detected", false) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); + + GenericEntry sbCamConnected = subsystemLayout + .add("Camera Connnected", false) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); public Vision(PhotonCamera camera){ this.camera = camera; @@ -57,7 +75,15 @@ public class Vision extends Subsystem { var result = camera.getLatestResult(); isTag = result.hasTargets(); - field.setRobotPose(lastPhysOdomPose); + // Optional multitag = result.getMultiTagResult(); + + // if (multitag.isEmpty()) { + // Transform3d fieldToCamera = result.getMultiTagResult().estimatedPose.best; + // }else if() + + + // sbTag.setBoolean(isTag); + // field.setRobotPose(getPose2d()); // sbCamConnected.setBoolean(camera); @@ -65,6 +91,7 @@ public class Vision extends Subsystem { if(!isTag){ sbTag.setBoolean(isTag); + field.setRobotPose(getPose2d()); return; } @@ -72,11 +99,23 @@ public class Vision extends Subsystem { // In case the pose estimator fails to estimate the pose, fallback to physical odometry. if(EstimatedRobotPose.isEmpty()){ - sbTag.setBoolean(isTag); - field.setRobotPose(lastPhysOdomPose); isTag = false; + sbTag.setBoolean(isTag); + field.setRobotPose(getPose2d()); return; } + + lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); + // lastVisionPose = new Pose2d( + // new Translation2d(lastVisionPose.getY(), lastVisionPose.getX()), + // lastVisionPose.getRotation() + // ); + + + field.setRobotPose(getPose2d()); + + + } @@ -220,21 +259,6 @@ public class Vision extends Subsystem { return "Vision"; } - - ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") - .getLayout(getSubsystemName(), BuiltInLayouts.kList) - .withSize(2, 2); - - GenericEntry sbTag = subsystemLayout - .add("Tag Detected", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - - GenericEntry sbCamConnected = subsystemLayout - .add("Camera Connnected", false) - .withWidget(BuiltInWidgets.kBooleanBox) - .getEntry(); - // GenericEntry sbShiftState = subsystemLayout // .add("Shift State", 0) // .withWidget(BuiltInWidgets.kNumberBar) @@ -245,7 +269,7 @@ public class Vision extends Subsystem { public void queryStatus() { sbTag.setBoolean(isTag); sbCamConnected.setBoolean(camera.isConnected()); - field.setRobotPose(getPose2d()); + // field.setRobotPose(getPose2d()); } @Override From 84faad46c47488f5cd64123d01a232dbc892253e Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sat, 11 Jan 2025 11:26:10 -0700 Subject: [PATCH 4/6] Working thing --- src/main/java/frc4388/robot/Constants.java | 4 +--- src/main/java/frc4388/robot/subsystems/Vision.java | 6 ++++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 34cceba..4e0c599 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -279,9 +279,7 @@ public final class Constants { public static final class VisionConstants { public static final String CAMERA_NAME = "Camera_Module_v1"; - public static final Rotation2d ROTATE_BY = Rotation2d.fromDegrees(180); - - public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(.3048, .2413, .2794), new Rotation3d(0,0.52333,0)); + public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 2eebceb..21f3500 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -106,12 +106,14 @@ public class Vision extends Subsystem { } lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); + // lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation())); // lastVisionPose = new Pose2d( - // new Translation2d(lastVisionPose.getY(), lastVisionPose.getX()), - // lastVisionPose.getRotation() + // lastVisionPose.getTranslation(), + // lastPhysOdomPose.getRotation() // ); + field.setRobotPose(getPose2d()); From 7fdb5dcd582624c4276960f4d7717375d0f73ff7 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sat, 11 Jan 2025 15:35:24 -0700 Subject: [PATCH 5/6] Make Vision Work --- src/main/java/frc4388/robot/Constants.java | 18 +++++++++++-- .../robot/commands/GotoPositionCommand.java | 27 ++++++++++++++----- .../frc4388/robot/subsystems/SwerveDrive.java | 17 ++++++++++++ .../java/frc4388/robot/subsystems/Vision.java | 5 +--- 4 files changed, 54 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4e0c599..6e67e3a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -10,6 +10,10 @@ package frc4388.robot; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Rotations; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -27,10 +31,12 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; +import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +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.Transform3d; @@ -279,9 +285,17 @@ public final class Constants { public static final class VisionConstants { public static final String CAMERA_NAME = "Camera_Module_v1"; - public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); + public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI)); - public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + // public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + + + public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( + Arrays.asList(new AprilTag[] { + new AprilTag(1, new Pose3d( + new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) + )), + }), 100, 100); // The standard deviations of our vision estimated poses, which affect correction rate // (Fake values. Experiment and determine estimation noise on an actual robot.) diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java index f481505..42a42c2 100644 --- a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -1,5 +1,7 @@ package frc4388.robot.commands; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -13,12 +15,16 @@ import frc4388.utility.controller.VirtualController; public class GotoPositionCommand extends Command { - private Translation2d translation2d= new Translation2d(0,0); - static Gains gains = new Gains(3,0,0); + // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998); + // private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999); + private Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d()); + static Gains xygains = new Gains(3,0,0); + static Gains rotgains = new Gains(0.1,0,0.0); static double tolerance = 0; - private PID xPID = new PID(gains, 0); - private PID yPID = new PID(gains, 0); + private PID xPID = new PID(xygains, 0); + private PID yPID = new PID(xygains, 0); + private PID rotPID = new PID(rotgains, 0); SwerveDrive swerveDrive; Vision vision; @@ -42,24 +48,31 @@ public class GotoPositionCommand extends Command { @Override public void execute() { - double xerr = translation2d.getX() - vision.getPose2d().getX(); - double yerr = translation2d.getY() - vision.getPose2d().getY(); + double xerr = targetpos.getX() - vision.getPose2d().getX(); + double yerr = targetpos.getY() - vision.getPose2d().getY(); + double roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(); SmartDashboard.putNumber("PID X Error", xerr); SmartDashboard.putNumber("PID Y Error", yerr); double xoutput = xPID.update(xerr); double youtput = yPID.update(yerr); + double rotoutput = rotPID.update(roterr); Translation2d leftStick = new Translation2d( Math.max(Math.min(youtput, 1), -1), Math.max(Math.min(xoutput, 1), -1) + // 0,0 ); - Translation2d rightStick = new Translation2d(); + Translation2d rightStick = new Translation2d( + Math.max(Math.min(rotoutput, 1), -1), + 0 + ); SmartDashboard.putNumber("PID X Output", xoutput); SmartDashboard.putNumber("PID Y Output", youtput); + // SmartDashboard.putNumber("PID Y Output", youtput); swerveDrive.driveWithInput(leftStick, rightStick, true); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index d33686e..bd96aff 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -155,6 +155,23 @@ public class SwerveDrive extends Subsystem { return false; } + public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { + // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + // stopModules(); // stop the swerve + + // if (leftStick.getNorm() < 0.05) //if no imput + // return; // don't bother doing swerve drive math and return early. + + leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX()*-speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withTargetDirection(rot) + ); + // double + } + public double getGyroAngle() { return swerveDriveTrain.getRotation3d().getAngle(); } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 21f3500..07ca483 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -108,16 +108,13 @@ public class Vision extends Subsystem { lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); // lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation())); // lastVisionPose = new Pose2d( - // lastVisionPose.getTranslation(), + // lastVisionPose.getTranslation(), // lastPhysOdomPose.getRotation() // ); field.setRobotPose(getPose2d()); - - - } From 882acc9b9344bc21dd2d47baa5f171591d42cd10 Mon Sep 17 00:00:00 2001 From: McGrathMaggie <78870mm@gmail.com> Date: Mon, 13 Jan 2025 19:43:35 -0700 Subject: [PATCH 6/6] Y auto activation autos activate when y is pressed, automatically stop when done --- .../java/frc4388/robot/RobotContainer.java | 7 +++++++ .../robot/commands/GotoPositionCommand.java | 19 ++++++++++++++++--- 2 files changed, 23 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d459c99..66eedc0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -164,6 +164,13 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); // ? /* Operator Buttons */ + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision)); + + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); + // creates an empty command & requires the swerve drive, subsystems can run only 1 command at a time // ? /* Programer Buttons (Controller 3)*/ diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java index 42a42c2..edfb97f 100644 --- a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -46,11 +46,17 @@ public class GotoPositionCommand extends Command { yPID.initialize(); } + double xerr; + double yerr; + double roterr; + double xytolerance = 0.01; + double rottolerance = 1; + @Override public void execute() { - double xerr = targetpos.getX() - vision.getPose2d().getX(); - double yerr = targetpos.getY() - vision.getPose2d().getY(); - double roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(); + xerr = targetpos.getX() - vision.getPose2d().getX(); + yerr = targetpos.getY() - vision.getPose2d().getY(); + roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(); SmartDashboard.putNumber("PID X Error", xerr); SmartDashboard.putNumber("PID Y Error", yerr); @@ -76,6 +82,13 @@ public class GotoPositionCommand extends Command { swerveDrive.driveWithInput(leftStick, rightStick, true); } + + @Override + public final boolean isFinished() { + return (Math.abs(xerr) < xytolerance && Math.abs(yerr) < xytolerance && Math.abs(roterr) < rottolerance); + // this statement is a boolean in and of itself + } + // @Override // public void end(boolean interrupted) {