From e1b0864c283ae3d859129519d31d08da4e1c1dae Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Fri, 8 Mar 2024 18:01:58 -0700 Subject: [PATCH] drift correction and recorded one note taxi auto (im geekin) --- src/main/java/frc4388/robot/Constants.java | 8 ++++---- src/main/java/frc4388/robot/RobotContainer.java | 16 ++++++++-------- .../frc4388/robot/subsystems/SwerveDrive.java | 11 +++++++++-- 3 files changed, 21 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a845954..7d24243 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -41,9 +41,9 @@ public final class Constants { public static final class DefaultSwerveRotOffsets { public static final double FRONT_LEFT_ROT_OFFSET = 216.869140625; - public static final double FRONT_RIGHT_ROT_OFFSET = 226.119140625;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; - public static final double BACK_LEFT_ROT_OFFSET = -277.5587969; - public static final double BACK_RIGHT_ROT_OFFSET = 52.646 + 90; + public static final double FRONT_RIGHT_ROT_OFFSET = 229.4826875;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; + public static final double BACK_LEFT_ROT_OFFSET = -277.646484375; + public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625; } public static final class IDs { @@ -67,7 +67,7 @@ public final class Constants { public static final class PIDConstants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.4, 0.0, 0, 1.0); + public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.0, 0.0, 0, 0.0); } public static final class AutoConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 39f0506..4647936 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -275,7 +275,7 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); configureVirtualButtonBindings(); - new DeferredBlock(() -> m_robotSwerveDrive.resetGyroFlip()); + new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); DriverStation.silenceJoystickConnectionWarning(true); @@ -334,12 +334,12 @@ public class RobotContainer { new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, - "blue_center_1Note.auto")) + "Nuetral_Center_1note_taxi.auto")) .onFalse(new InstantCommand()); new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, - "blue_center_1Note.auto", + "Nuetral_Center_1note_taxi.auto", new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false)) .onFalse(new InstantCommand()); @@ -352,11 +352,11 @@ public class RobotContainer { // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - // .whileTrue(new InstantCommand(() -> - // m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), - // new Translation2d(0, 0), - // true), m_robotSwerveDrive)); + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .whileTrue(new InstantCommand(() -> + m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), + new Translation2d(0, 0), + true), m_robotSwerveDrive)); //? /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 20b5964..3d11490 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -125,13 +125,20 @@ public class SwerveDrive extends SubsystemBase { public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) { - Translation2d rightStick = new Translation2d(rightX, rightY); + Translation2d rightStick = new Translation2d(-rightX, rightY); if(fieldRelative) { double rot = 0; if(rightStick.getNorm() > 0.5) { orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); - rot = orientRotTarget.minus(gyro.getRotation2d()).getRadians(); + + Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5)); + double min = tmp.getDegrees(); + min = Math.max(Math.abs(min), 2); + if(tmp.getDegrees() < 0) + min*=-1; + tmp = new Rotation2d(min * Math.PI / 180); + rot = tmp.getRadians(); // x x - y/x } Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);