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