From 943e9612b6291b595f9f4adc4b4da798eb62b543 Mon Sep 17 00:00:00 2001 From: kully Date: Sun, 7 Apr 2024 17:10:52 +0300 Subject: [PATCH 1/2] use april tag for pose est so auto will work better but wont fuck the teleop drive --- .../java/frc/robot/subsystems/swerve/SwerveSubsystem.java | 6 +++--- src/main/java/frc/util/vision/Vision.java | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 60457c1..087525f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -197,9 +197,9 @@ public void periodic() { var estPose = est.estimatedPose.toPose2d(); // Change our trust in the measurement based on the tags we can see var estStdDevs = vision.getEstimationStdDevsRight(estPose); - + poseEstimation.addVisionMeasurement( - new Pose2d(estPose.getTranslation(), getYaw()), est.timestampSeconds, estStdDevs); + estPose, est.timestampSeconds, estStdDevs); }); vision.getLeftEstimatedGlobalPose().ifPresent( @@ -209,7 +209,7 @@ public void periodic() { var estStdDevs = vision.getEstimationStdDevsLeft(estPose); poseEstimation.addVisionMeasurement( - new Pose2d(estPose.getTranslation(), getYaw()), est.timestampSeconds, estStdDevs); + estPose, est.timestampSeconds, estStdDevs); }); field2d.setRobotPose(poseEstimation.getEstimatedPosition()); diff --git a/src/main/java/frc/util/vision/Vision.java b/src/main/java/frc/util/vision/Vision.java index 3573580..288b880 100644 --- a/src/main/java/frc/util/vision/Vision.java +++ b/src/main/java/frc/util/vision/Vision.java @@ -139,7 +139,7 @@ public Matrix getEstimationStdDevsRight(Pose2d estimatedPose) { if (numTags > 1) estStdDevs = kRightMultiTagStdDevs; // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 7)//TODO: test on field + if (numTags == 1 && avgDist > 7) estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); From f617d25f0358346b425c3f93ef6d2ce37a757428 Mon Sep 17 00:00:00 2001 From: Eilon Date: Wed, 17 Apr 2024 11:57:12 +0300 Subject: [PATCH 2/2] align to tag --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/RobotContainer.java | 13 +++ .../robot/a_robotCommandGroups/AlignTag.java | 26 ++++++ .../ShooterArmCommands/ShooterArmTag.java | 32 ++++++++ .../AutoAlignToTagWhileTrue.java | 82 +++++++++++++++++++ .../SwerveCommands/TurnToNote.java | 46 +++++++++++ .../SwerveCommands/TurnToTag.java | 36 ++++++++ .../subsystems/swerve/SwerveConstants.java | 2 + src/main/java/frc/util/vision/Vision.java | 27 ++++++ 9 files changed, 265 insertions(+) create mode 100644 src/main/java/frc/robot/a_robotCommandGroups/AlignTag.java create mode 100644 src/main/java/frc/robot/basicCommands/ShooterArmCommands/ShooterArmTag.java create mode 100644 src/main/java/frc/robot/basicCommands/SwerveCommands/AutoAlignToTagWhileTrue.java create mode 100644 src/main/java/frc/robot/basicCommands/SwerveCommands/TurnToNote.java create mode 100644 src/main/java/frc/robot/basicCommands/SwerveCommands/TurnToTag.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f32c726..a0d6f0e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,6 +24,7 @@ public final class Constants { public static class Vision { public static final String kRightCameraName = "right_Camera"; public static final String kLeftCameraName = "left_Camera"; + public static final String kTestCameraName = "noteCamera"; // Cam mounted facing forward, half a meter forward of center, half a meter up // from center. public static final Transform3d kRightRobotToCam = new Transform3d(new Translation3d(-0.16, -0.075, 0.45), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 03a8323..a34c9e0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.a_robotCommandGroups.AlignTag; import frc.robot.a_robotCommandGroups.ReadyShootSpeaker; import frc.robot.a_robotCommandGroups.ShootBase; import frc.robot.a_robotCommandGroups.ShootSpeaker; @@ -20,6 +21,7 @@ import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.takeFeed.TakeFeedSubsystem; import frc.util.PathPlanner.PathPlannerHelper; +import frc.robot.basicCommands.SwerveCommands.TurnToNote; /** * This class is where the bulk of the robot should be declared. Since @@ -34,6 +36,7 @@ public class RobotContainer { /* Controllers */ private final CommandXboxController driver = new CommandXboxController(0); private final CommandXboxController operator = new CommandXboxController(1); + private final CommandXboxController stalls = new CommandXboxController(2); // Driver Triggers Trigger driverYTrigger = driver.y(); @@ -55,6 +58,11 @@ public class RobotContainer { Trigger operatorBTrigger = operator.b(); Trigger operatorStart = operator.start(); + // stalls Triggers + Trigger stallsRightBumperTrigger = stalls.rightBumper(); + Trigger stallsYTrigger = stalls.y(); + + PathPlannerHelper pathPlannerHelper = PathPlannerHelper.getInstace(); /* Subsystems */ @@ -106,5 +114,10 @@ private void configureButtonBindings() { operatorYTrigger.onTrue(new ReadyShootSpeaker()); operatorBTrigger.onTrue(new InstantCommand(() -> ShooterSubsystem.getInstance().coast(), ShooterSubsystem.getInstance())); TakeFeedSubsystem.getInstance().setDefaultCommand(new TakeFeedJoystickSetSpeed(()-> - operator.getHID().getLeftY())); + + //stalls Buttons + stallsRightBumperTrigger.whileTrue(new AlignTag()); + stallsYTrigger.onTrue(new TurnToNote()); + } } diff --git a/src/main/java/frc/robot/a_robotCommandGroups/AlignTag.java b/src/main/java/frc/robot/a_robotCommandGroups/AlignTag.java new file mode 100644 index 0000000..f640a28 --- /dev/null +++ b/src/main/java/frc/robot/a_robotCommandGroups/AlignTag.java @@ -0,0 +1,26 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.a_robotCommandGroups; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.basicCommands.ShooterArmCommands.ShooterArmSpeakerAngle; +import frc.robot.basicCommands.ShooterArmCommands.ShooterArmTag; +import frc.robot.basicCommands.SwerveCommands.AutoAlignToTagWhileTrue; +import frc.robot.basicCommands.SwerveCommands.TurnToTag; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class AlignTag extends ParallelCommandGroup { + /** Creates a new AlignTag. */ + public AlignTag() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new TurnToTag(), + new ShooterArmTag() + ); + } +} diff --git a/src/main/java/frc/robot/basicCommands/ShooterArmCommands/ShooterArmTag.java b/src/main/java/frc/robot/basicCommands/ShooterArmCommands/ShooterArmTag.java new file mode 100644 index 0000000..a676514 --- /dev/null +++ b/src/main/java/frc/robot/basicCommands/ShooterArmCommands/ShooterArmTag.java @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.basicCommands.ShooterArmCommands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.shooterArm.ShooterArmSubsystem; +import frc.robot.subsystems.swerve.SwerveSubsystem; + +public class ShooterArmTag extends Command { + + private final ShooterArmSubsystem shooterArm = ShooterArmSubsystem.getInstance(); + double angle; + + public ShooterArmTag() { + addRequirements(shooterArm); + } + + @Override + public void initialize() { + SmartDashboard.putNumber("angle", shooterArm.speakerInterpolate()); + shooterArm.moveArmTo(shooterArm.speakerInterpolate());} + + + @Override + public void execute() { + SmartDashboard.putNumber("angle", shooterArm.speakerInterpolate()); + shooterArm.moveArmTo(shooterArm.speakerInterpolate()); + } +} diff --git a/src/main/java/frc/robot/basicCommands/SwerveCommands/AutoAlignToTagWhileTrue.java b/src/main/java/frc/robot/basicCommands/SwerveCommands/AutoAlignToTagWhileTrue.java new file mode 100644 index 0000000..f9a8111 --- /dev/null +++ b/src/main/java/frc/robot/basicCommands/SwerveCommands/AutoAlignToTagWhileTrue.java @@ -0,0 +1,82 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.basicCommands.SwerveCommands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.FieldConstants.Speaker; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.subsystems.takeFeed.TakeFeedSubsystem; +import frc.util.AllianceFlipUtil; +import frc.util.vision.Vision; + +public class AutoAlignToTagWhileTrue extends Command { + /** Creates a new TurnToDegree. */ + SwerveSubsystem swerve = SwerveSubsystem.getInstance(); + + private Vision vision = Vision.getInstance(); + double angleFromTarget = vision.GetAngleFromTarget().getDegrees(); + + Rotation2d heading = new Rotation2d(); + + public AutoAlignToTagWhileTrue() { + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + + Pose2d robot = swerve.getPose(); + + double deltaX = Math.abs(robot.getX() - AllianceFlipUtil.apply(Speaker.centerSpeakerOpening.getX())); + double deltaY = Speaker.centerSpeakerOpening.getY() - robot.getY(); + double tempHeading = Units.radiansToDegrees(Math.atan2(deltaY, deltaX)); + if(DriverStation.getAlliance().get() == Alliance.Red) + heading = new Rotation2d(Units.degreesToRadians(tempHeading + 180)); + else + heading = new Rotation2d(Units.degreesToRadians(360-tempHeading)); + + SmartDashboard.putNumber("heading", heading.getDegrees()); + swerve.setHeading(heading); + swerve.drive(new Translation2d(),0,true,false); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + Pose2d robot = swerve.getPose(); + + double deltaX = Math.abs(robot.getX() - AllianceFlipUtil.apply(Speaker.centerSpeakerOpening.getX())); + double deltaY = Speaker.centerSpeakerOpening.getY() - robot.getY(); + double tempHeading = Units.radiansToDegrees(Math.atan2(deltaY, deltaX)); + if(DriverStation.getAlliance().get() == Alliance.Red) + heading = new Rotation2d(Units.degreesToRadians(tempHeading + 180)); + else + heading = new Rotation2d(Units.degreesToRadians(360-tempHeading)); + + SmartDashboard.putNumber("heading", heading.getDegrees()); + swerve.setHeading(heading); + swerve.drive(new Translation2d(),0,true,true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + swerve.disableHeading(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return swerve.headingPid.atSetpoint(); + } +} diff --git a/src/main/java/frc/robot/basicCommands/SwerveCommands/TurnToNote.java b/src/main/java/frc/robot/basicCommands/SwerveCommands/TurnToNote.java new file mode 100644 index 0000000..903efda --- /dev/null +++ b/src/main/java/frc/robot/basicCommands/SwerveCommands/TurnToNote.java @@ -0,0 +1,46 @@ +package frc.robot.basicCommands.SwerveCommands; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.subsystems.swerve.SwerveConstants.swerveConstants; +import frc.util.vision.Vision; + +/* + * Turn to note is one of our best codes so far. It is written by one of the + * best programmer we have! mister Eilon Nave Huppert. + * this code is programmed to detectd notes with the help of our Vision class and turn to position + * where our great XXXfeederXXX intake face them. + */ + +public class TurnToNote extends Command { + SwerveSubsystem swerve = SwerveSubsystem.getInstance(); + Vision vision = Vision.getInstance(); + public PIDController notepid = swerveConstants.notePid; + + + public TurnToNote() { + addRequirements(swerve); + notepid.setTolerance(3); + } + + @Override + public void initialize() { + } + + @Override + public void execute() { + double angelFromNote = vision.GetAngelFromNote(); + swerve.drive(new Translation2d(),notepid.calculate(angelFromNote,0),true,false); + } + + @Override + public void end(boolean interrupted) { + swerve.drive(new Translation2d(),0,true,false); + } + + @Override + public boolean isFinished() { + return notepid.atSetpoint(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/basicCommands/SwerveCommands/TurnToTag.java b/src/main/java/frc/robot/basicCommands/SwerveCommands/TurnToTag.java new file mode 100644 index 0000000..ebac712 --- /dev/null +++ b/src/main/java/frc/robot/basicCommands/SwerveCommands/TurnToTag.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.basicCommands.SwerveCommands; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.subsystems.swerve.SwerveConstants.swerveConstants; +import frc.util.vision.Vision; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class TurnToTag extends Command { + SwerveSubsystem swerve = SwerveSubsystem.getInstance(); + Vision vision = Vision.getInstance(); + public PIDController notepid = swerveConstants.notePid; + public TurnToTag() { + addRequirements(swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + @Override + public void execute() { + double angelFromTag = vision.GetAngelFromTag(); + swerve.drive(new Translation2d(),notepid.calculate(angelFromTag,0),true,false); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index b33647e..c9d791f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java @@ -26,6 +26,8 @@ public class SwerveConstants { public static final class swerveConstants { public static final double minimumErrorAligning = 0; public static final PIDController aligningPID = new PIDController(0.14, 0, 0.0); + public static final PIDController notePid = new PIDController(0.05, 0, 0); + public static final int pigeonID = 11; public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW- diff --git a/src/main/java/frc/util/vision/Vision.java b/src/main/java/frc/util/vision/Vision.java index 288b880..1415091 100644 --- a/src/main/java/frc/util/vision/Vision.java +++ b/src/main/java/frc/util/vision/Vision.java @@ -41,8 +41,10 @@ import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.PhotonUtils; +import edu.wpi.first.units.Angle; public class Vision { + private final PhotonCamera noteCamera; private final PhotonCamera rightCamera; private final PhotonPoseEstimator rightPhotonEstimator; public double rightLastEstTimestamp = 0; @@ -62,6 +64,7 @@ public static Vision getInstance() { } private Vision(){ + noteCamera = new PhotonCamera(kTestCameraName); rightCamera = new PhotonCamera(kRightCameraName); rightPhotonEstimator = new PhotonPoseEstimator( @@ -189,4 +192,28 @@ public Rotation2d GetAngleFromTarget() { public Rotation2d GetAngleFromTarget(Pose2d targetPose) { return PhotonUtils.getYawToPose(SwerveSubsystem.getInstance().getPose(), targetPose); } + public double GetAngelFromNote(){ + var result = noteCamera.getLatestResult(); + double angle = 0; + + if (result.hasTargets()) { // Check if the camera see the note. + angle = result.getBestTarget().getYaw(); // Provides the best angel. + } + return angle; + } + public double GetAngelFromTag(){ + var result = leftCamera.getLatestResult(); + double angle = 0; + + if (result.hasTargets()) { // Check if the camera see the note. + angle = result.getBestTarget().getYaw(); // Provides the best angel. + } + return angle; + } + + // Check if the camera see the note. + public boolean seeTarget(){ + var result = noteCamera.getLatestResult(); + return result.hasTargets(); + } }