From 23575dd33ab48abd53f0ed116c9cf6f957e5a24b Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 12 Mar 2024 16:10:20 -0600 Subject: [PATCH] Made some autos (need to correct PID) (Im tweakin) --- src/main/java/frc4388/robot/Constants.java | 8 ++++---- src/main/java/frc4388/robot/RobotContainer.java | 16 ++++++++-------- .../frc4388/robot/subsystems/SwerveDrive.java | 2 +- .../frc4388/robot/subsystems/SwerveModule.java | 13 ++++++++++--- 4 files changed, 23 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b653677..44f08d6 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -40,9 +40,9 @@ public final class Constants { public static final double TURBO_SPEED = 4.0; public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = 216.869140625; - 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 FRONT_LEFT_ROT_OFFSET = 222.7145469; + public static final double FRONT_RIGHT_ROT_OFFSET = 255.0464068;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; + public static final double BACK_LEFT_ROT_OFFSET = -277.4284884; public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625; } @@ -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.0, 0.0, 0, 0.0); + public static final Gains SWERVE_GAINS = new Gains(1.2, 0.0, 0.2, 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 30d5cac..fa06eec 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -87,9 +87,9 @@ public class RobotContainer { private ParallelCommandGroup intakeToShoot = new ParallelCommandGroup( new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.idle()), - new InstantCommand(() -> m_driverXbox.setRumble(null, 1.0)).andThen(new WaitCommand(1)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(null, 0.0))) - //new InstantCommand(() -> m_robotShooter.spin()) + new InstantCommand(() -> m_robotShooter.idle()) + // new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))), + // new InstantCommand(() -> m_robotShooter.spin()) ); // private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( @@ -376,11 +376,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 e2f3da9..d6ae2e4 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -81,7 +81,7 @@ public class SwerveDrive extends SubsystemBase { // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX(), gyro.getRotation2d());//.times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 1ddab51..cfdd7ab 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -18,12 +18,14 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; +import frc4388.utility.configurable.ConfigurableDouble; public class SwerveModule extends SubsystemBase { private WPI_TalonFX driveMotor; private WPI_TalonFX angleMotor; private CANCoder encoder; - + //private ConfigurableDouble offsetGetter; + private static int swerveId = 0; public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS; /** Creates a new SwerveModule. */ @@ -31,7 +33,8 @@ public class SwerveModule extends SubsystemBase { this.driveMotor = driveMotor; this.angleMotor = angleMotor; this.encoder = encoder; - + // this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset); + swerveId++; TalonFXConfiguration angleConfig = new TalonFXConfiguration(); angleConfig.slot0.kP = swerveGains.kP; angleConfig.slot0.kI = swerveGains.kI; @@ -48,7 +51,11 @@ public class SwerveModule extends SubsystemBase { driveMotor.setSelectedSensorPosition(0); driveMotor.config_kP(0, 0.2); } - + + //@Override + // public void periodic() { + // encoder.configMagnetOffset(offsetGetter.get()); + // } /** * Get the drive motor of the SwerveModule * @return the drive motor of the SwerveModule