diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 501b4f5..0108060 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -36,10 +36,12 @@ 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.Pose2d; 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; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -153,7 +155,6 @@ public final class Constants { public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13); public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 14); - public static final CanDevice e = new CanDevice("NONEXISTANT_CAN", 50); } public static final class PIDConstants { @@ -185,13 +186,14 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); - public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune - - public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value - public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value + public static final Gains XY_GAINS = new Gains(3,0,0); + public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); + + + public static final double XY_TOLERANCE = 0.05; + public static final double ROT_TOLERANCE = 1; + + public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d()); } @@ -289,10 +291,11 @@ public final class Constants { public static final String CAMERA_NAME = "Camera_Module_v1"; 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(); - + + // Test april tag field layout public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( Arrays.asList(new AprilTag[] { new AprilTag(1, new Pose3d( diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5fc11db..b2b4e9f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.GenericHID; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.robot.Constants.OIConstants; +import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -167,7 +168,7 @@ public class RobotContainer { // ? /* Operator Buttons */ new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision)); + .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos)); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); @@ -224,15 +225,16 @@ public class RobotContainer { public Command getAutonomousCommand() { //return autoPlayback; //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); - try{ - // Load the path you want to follow using its name in the GUI - return new PathPlannerAuto("Example Auto"); + //try{ + // // Load the path you want to follow using its name in the GUI + / return new PathPlannerAuto("Example Auto"); } catch (Exception e) { DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace()); return Commands.none(); } // zach told me to do the below comment //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); + return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); } diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java index edfb97f..e7d82ed 100644 --- a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -5,6 +5,7 @@ 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; +import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Vision; import frc4388.utility.Gains; @@ -17,14 +18,11 @@ public class GotoPositionCommand extends Command { // 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(xygains, 0); - private PID yPID = new PID(xygains, 0); - private PID rotPID = new PID(rotgains, 0); + private PID xPID = new PID(AutoConstants.XY_GAINS, 0); + private PID yPID = new PID(AutoConstants.XY_GAINS, 0); + private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); + private Pose2d targetpos; SwerveDrive swerveDrive; Vision vision; @@ -34,9 +32,10 @@ public class GotoPositionCommand extends Command { * @param SwerveDrive m_robotSwerveDrive */ - public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) { + public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) { this.swerveDrive = swerveDrive; this.vision = vision; + this.targetpos = targetpos; addRequirements(swerveDrive); } @@ -49,8 +48,6 @@ public class GotoPositionCommand extends Command { double xerr; double yerr; double roterr; - double xytolerance = 0.01; - double rottolerance = 1; @Override public void execute() { @@ -58,8 +55,8 @@ public class GotoPositionCommand extends Command { 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); + // SmartDashboard.putNumber("PID X Error", xerr); + // SmartDashboard.putNumber("PID Y Error", yerr); double xoutput = xPID.update(xerr); double youtput = yPID.update(yerr); @@ -76,16 +73,16 @@ public class GotoPositionCommand extends Command { 0 ); - SmartDashboard.putNumber("PID X Output", xoutput); - SmartDashboard.putNumber("PID Y Output", youtput); + // SmartDashboard.putNumber("PID X Output", xoutput); // SmartDashboard.putNumber("PID Y Output", youtput); + // // SmartDashboard.putNumber("PID Y Output", youtput); swerveDrive.driveWithInput(leftStick, rightStick, true); } @Override public final boolean isFinished() { - return (Math.abs(xerr) < xytolerance && Math.abs(yerr) < xytolerance && Math.abs(roterr) < rottolerance); + return (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE); // this statement is a boolean in and of itself }