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] 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()); - - - }