From 6e7c58422dd1a644337a91d0889234762251171c Mon Sep 17 00:00:00 2001 From: Aarav Date: Wed, 15 Mar 2023 13:03:02 -0600 Subject: [PATCH] commit stuff --- src/main/java/frc4388/robot/Constants.java | 5 ++++- .../java/frc4388/robot/RobotContainer.java | 6 ++++-- .../java/frc4388/robot/commands/RunArmIn.java | 2 +- .../java/frc4388/robot/subsystems/Arm.java | 10 ++-------- .../frc4388/robot/subsystems/SwerveDrive.java | 20 ++++++++++++------- 5 files changed, 24 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2d8438b..f82c168 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -27,7 +27,10 @@ public final class Constants { public static final double MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 0.8; public static double ROTATION_SPEED = MAX_ROT_SPEED; - public static double ROT_CORRECTION_SPEED = MIN_ROT_SPEED; + public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; + + public static final double CORRECTION_MIN = 10; + public static final double CORRECTION_MAX = 50; public static final class IDs { public static final int LEFT_FRONT_WHEEL_ID = 2; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 583944c..77e5f1b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.AutoBalance; @@ -122,7 +123,7 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(), m_robotSwerveDrive)); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(m_robotArm.getArmRotation()-135), m_robotSwerveDrive)); // // .onFalse() new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) @@ -164,8 +165,9 @@ public class RobotContainer { new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new ParallelCommandGroup( new InstantCommand(() -> m_robotClaw.toggle()), + new RunArmIn(m_robotArm), new SequentialCommandGroup( - new RunArmIn(m_robotArm), + new WaitCommand(.25), new PivotCommand(m_robotArm, 135) ) )); diff --git a/src/main/java/frc4388/robot/commands/RunArmIn.java b/src/main/java/frc4388/robot/commands/RunArmIn.java index 914e173..e1fddd5 100644 --- a/src/main/java/frc4388/robot/commands/RunArmIn.java +++ b/src/main/java/frc4388/robot/commands/RunArmIn.java @@ -12,7 +12,7 @@ public class RunArmIn extends CommandBase { public RunArmIn(Arm arm) { m_arm = arm; - addRequirements(arm); + addRequirements(); } // Called when the command is initially scheduled. diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 1d017e0..2f4e844 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -55,9 +55,9 @@ public class Arm extends SubsystemBase { if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) { m_pivot.set(ControlMode.PercentOutput, 0); } else if (degrees > 90 && vel > 0) { - m_pivot.set(ControlMode.PercentOutput, .15 * vel); + m_pivot.set(ControlMode.PercentOutput, .1 * vel); } else { - m_pivot.set(ControlMode.PercentOutput, .3 * vel); + m_pivot.set(ControlMode.PercentOutput, .25 * vel); } } @@ -169,12 +169,6 @@ public class Arm extends SubsystemBase { boolean soft_limits = true; public void killSoftLimits() { resetTeleSoftLimit(); - var pivot_soft = m_pivot.getSelectedSensorPosition(); - var tele_soft = m_tele.getSelectedSensorPosition(); - - m_pivot.configForwardSoftLimitEnable(!soft_limits); - m_pivot.configReverseSoftLimitEnable(!soft_limits); - soft_limits = !soft_limits; } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 44ab39f..2edb77d 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.math.kinematics.SwerveModulePosition; import frc4388.robot.Constants.SwerveDriveConstants; @@ -37,7 +38,7 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default - public Rotation2d rotTarget = new Rotation2d(); + public double rotTarget = 0; public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ @@ -56,11 +57,13 @@ public class SwerveDrive extends SubsystemBase { if (fieldRelative) { double rot = 0; - if (rightStick.getNorm() > 0.01) { - rotTarget = gyro.getRotation2d(); + if (rightStick.getNorm() > 0.05) { + rotTarget = gyro.getAngle(); rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; + SmartDashboard.putBoolean("drift correction", false); } else { - rot = rotTarget.minus(gyro.getRotation2d()).getRadians() * SwerveDriveConstants.ROT_CORRECTION_SPEED; + SmartDashboard.putBoolean("drift correction", true); + rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; } // Use the left joystick to set speed. Apply a cubic curve and the set max speed. @@ -95,7 +98,7 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); - rotTarget = new Rotation2d(0); + rotTarget = 0; } public void stopModules() { @@ -113,11 +116,14 @@ public class SwerveDrive extends SubsystemBase { // This method will be called once per scheduler run } - public void toggleGear() { - if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) { + public void toggleGear(double angle) { + if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW + && Math.abs(angle) < 2) { this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MAX; } else { this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; } }