diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1b8278b..e33d8bb 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -233,21 +233,21 @@ public class RobotContainer { //TEST - > Defense: X position on wheels and swerve drive pid on position new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .whileTrue(new RunCommand(() -> { - m_robotSwerveDrive.defenseXPosition(); - }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> { - m_robotSwerveDrive.stopDefenseXPosition(); - - // .onTrue(new InstantCommand(() -> { - // currentPose = m_robotSwerveDrive.getCurrentPose(); - // })) - // .whileTrue(new RunCommand(() -> { - // m_stayInPosition.goToTargetPose(currentPose); + // .whileTrue(new RunCommand(() -> { + // m_robotSwerveDrive.defenseXPosition(); // }, m_robotSwerveDrive)) - // .onFalse(new InstantCommand(() -> { - // m_robotSwerveDrive.softStop(); - // })); + // .onFalse(new InstantCommand(() -> { + // m_robotSwerveDrive.stopDefenseXPosition(); + + .onTrue(new InstantCommand(() -> { + currentPose = m_robotSwerveDrive.getCurrentPose(); + })) + .whileTrue(new RunCommand(() -> { + m_stayInPosition.goToTargetPose(currentPose); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> { + m_robotSwerveDrive.softStop(); + })); diff --git a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java index 8d9be18..4b66de2 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java +++ b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java @@ -5,52 +5,46 @@ package frc4388.robot.commands.Swerve; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import frc4388.robot.commands.PID; import frc4388.robot.subsystems.swerve.SwerveDrive; public class StayInPosition extends PID { + SwerveDrive drive; - SwerveDrive drive; + public StayInPosition(SwerveDrive drive) { + super(0.3, 0.0, 0.0, 0.0, 1); + this.drive = drive; + addRequirements(drive); + } - /** Creates a new StayInPosition. */ - public StayInPosition(SwerveDrive drive) { - super(0.3, 0.0, 0.0, 0.0, 1); - - this.drive = drive; + public void goToTargetPose(Pose2d targetPose) { + Pose2d currentPose = drive.getCurrentPose(); - addRequirements(drive); - } + double translationX = targetPose.getX() - currentPose.getX(); + double translationY = targetPose.getY() - currentPose.getY(); - public void goToTargetPose(Pose2d targetPose){ - Pose2d currentPose = drive.getCurrentPose(); - double translationX = targetPose.getX() - currentPose.getX(); - double translationY = targetPose.getY() - currentPose.getY(); - Rotation2d deltaRotation = targetPose.getRotation().minus(currentPose.getRotation()); - Translation2d driveTranslation = new Translation2d(translationX, translationY); + double magnitude = Math.sqrt(translationX * translationX + translationY * translationY); + if (magnitude > 1.0) { + translationX /= magnitude; + translationY /= magnitude; + } - drive.driveFieldAngle(driveTranslation, deltaRotation); - } + Translation2d driveTranslation; + if (magnitude < 0.05) { + driveTranslation = new Translation2d(); + } else { + driveTranslation = new Translation2d(translationX, translationY); + } - @Override - public double getError() { - return 0; - // return targetAngle - drive.getGyroAngle(); - } + drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation()); + } - @Override - public void runWithOutput(double output) { - // drive.driveWithInput(new Pose2d(new Translation2d(0.0, 0.0), new Rotation2d(output / Math.abs(getError())))); - } + @Override + public double getError() { + return 0; + } - // @Override - // public boolean isFinished() { - // Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation(); - // double ballAngleDeg = m_lidar.getLatestBallAngleDegrees(); - - // // TODO: Tune - // return Math.abs(curRot.getDegrees() +ballAngleDeg) < 5; - // } - -} + @Override + public void runWithOutput(double output) {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index d8311c9..44b0323 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 158; - public static final String GIT_SHA = "07ec609b019fd8db439e14beb74c50fdbf75a091"; - public static final String GIT_DATE = "2026-03-14 15:41:27 MDT"; + public static final int GIT_REVISION = 159; + public static final String GIT_SHA = "ebab028818fc0362daa4d7eb1010131819edd1cb"; + public static final String GIT_DATE = "2026-03-14 18:29:15 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-14 17:27:47 MDT"; - public static final long BUILD_UNIX_TIME = 1773530867415L; + public static final String BUILD_DATE = "2026-03-14 18:46:53 MDT"; + public static final long BUILD_UNIX_TIME = 1773535613285L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 69d865f..e90be1b 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -295,7 +295,27 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // SmartDashboard.putBoolean("drift correction", true); } - + public void driveFieldAngleSIP(Translation2d leftStick, Rotation2d heading) { + + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); + leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); + + rotTarget = heading.getDegrees(); + + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(heading); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kP, + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kI, + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kD + ); + io.setControl(ctrl); + } public void driveIntake(Translation2d leftStick, boolean invertRotation){ // if (invert){ @@ -371,14 +391,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public void defenseXPosition(){ - io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_FRONT_STEER, Rotation2d.fromDegrees(45.0)); - io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER, Rotation2d.fromDegrees(-45.0)); - io.setModuleSteerAngle(SwerveDriveConstants.IDs.LEFT_BACK_STEER, Rotation2d.fromDegrees(-45.0)); - io.setModuleSteerAngle(SwerveDriveConstants.IDs.RIGHT_BACK_STEER, Rotation2d.fromDegrees(45.0)); + io.setControl(new SwerveRequest.SwerveDriveBrake()); } public void stopDefenseXPosition(){ - io.restoreSteerOffsets(); + stopModules(); } public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) { diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index a0c47d0..0034c0d 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -77,8 +77,7 @@ public final class SwerveDriveConstants { public static final class ModuleSpecificConstants { //2026 //Front Left - public static final Angle FRONT_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); - public static Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; @@ -86,8 +85,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - public static final Angle FRONT_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.465332+0.3+0.003174-0.0103); - public static Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; @@ -95,8 +93,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - public static final Angle BACK_LEFT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.023438+0.5+0.0168-0.00562); - public static Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; @@ -104,8 +101,7 @@ public final class SwerveDriveConstants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - public static final Angle BACK_RIGHT_ENCODER_OFFSET_CONSTANT = Rotations.of(0.029541+0.05-0.002197-0.00366); - public static Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java index 8d81397..25db223 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -27,10 +27,6 @@ public interface SwerveIO { public default void tareEverything() {} - public default void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) {} - - public default void restoreSteerOffsets(){} - public default void resetPose(Pose2d pose) {} public default void addVisionMeasurement(List poses) {} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java index ae1c594..9574699 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java @@ -56,15 +56,6 @@ public class SwerveReal implements SwerveIO { swerveDriveTrain.addVisionMeasurement(pose, Utils.fpgaToCurrentTime(Vision.getTime())); } } - - @Override - public void setModuleSteerAngle(CanDevice steerDevice, Rotation2d angle) { - } - - @Override - public void restoreSteerOffsets() { - - } @Override public void setLimits(double limitInAmps) {