diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 29a76af..80ad347 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -23,6 +23,7 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { + 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; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 45ca574..eac33f3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,10 +14,7 @@ import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; 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; @@ -28,7 +25,6 @@ import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.commands.JoystickRecorder; import frc4388.robot.commands.PivotCommand; import frc4388.robot.commands.PlaybackChooser; -import frc4388.robot.commands.RunArmIn; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.utility.controller.XboxController; @@ -89,8 +85,8 @@ public class RobotContainer { .withName("SwerveDrive DefaultCommand")); m_robotArm.setDefaultCommand(new RunCommand(() -> { - m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY(), false); - m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY(), false); + m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY()); + m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY()); }, m_robotArm) .withName("Arm DefaultCommand")); @@ -161,23 +157,13 @@ public class RobotContainer { .onTrue(new PivotCommand(m_robotArm, 135)); new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new PivotCommand(m_robotArm, 225)); + .onTrue(new PivotCommand(m_robotArm, 210)); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw)); + .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits())); - - // TODO: put this into a variable - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new ParallelCommandGroup( - new InstantCommand(() -> m_robotClaw.toggle()), - new SequentialCommandGroup( - new RunArmIn(m_robotArm), - new PivotCommand(m_robotArm, 135) - ) - )); // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm)); diff --git a/src/main/java/frc4388/robot/commands/ExtendArmTo.java b/src/main/java/frc4388/robot/commands/ExtendArmTo.java deleted file mode 100644 index d2db4b4..0000000 --- a/src/main/java/frc4388/robot/commands/ExtendArmTo.java +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Arm; - -public class ExtendArmTo extends CommandBase { - private final Arm m_arm; - private final double m_target; - private final boolean m_direction; - - public ExtendArmTo(Arm arm, double units) { - m_arm = arm; - m_target = units; - m_direction = m_target > 1; - addRequirements(m_arm); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_arm.setTeleVel(m_direction ? 1 : -1, false); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (m_direction) - return m_arm.getTeleUnit() > m_target; - else - return m_arm.getTeleUnit() < m_target; - } -} diff --git a/src/main/java/frc4388/robot/commands/RunArmIn.java b/src/main/java/frc4388/robot/commands/LimeAlign.java similarity index 68% rename from src/main/java/frc4388/robot/commands/RunArmIn.java rename to src/main/java/frc4388/robot/commands/LimeAlign.java index c1ac04b..c811a3e 100644 --- a/src/main/java/frc4388/robot/commands/RunArmIn.java +++ b/src/main/java/frc4388/robot/commands/LimeAlign.java @@ -5,25 +5,22 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Arm; +import frc4388.robot.subsystems.SwerveDrive; -public class RunArmIn extends CommandBase { - private final Arm m_arm; - - public RunArmIn(Arm arm) { - m_arm = arm; - addRequirements(); +public class LimeAlign extends CommandBase { + public LimeAlign(SwerveDrive drive) { + addRequirements(drive); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - m_arm.setTeleVel(1, true); - } + public void execute() {} // Called once the command ends or is interrupted. @Override @@ -32,6 +29,6 @@ public class RunArmIn extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return m_arm.isTeleIn(); + return false; } } diff --git a/src/main/java/frc4388/robot/commands/PivotCommand.java b/src/main/java/frc4388/robot/commands/PivotCommand.java index fcb54d1..a84690d 100644 --- a/src/main/java/frc4388/robot/commands/PivotCommand.java +++ b/src/main/java/frc4388/robot/commands/PivotCommand.java @@ -27,6 +27,6 @@ public class PivotCommand extends PelvicInflammatoryDisease { @Override public void runWithOutput(double output) { SmartDashboard.putNumber("pivot output", output); - arm.setRotVel(output, true); + arm.setRotVel(output); } } diff --git a/src/main/java/frc4388/robot/commands/TeleCommand.java b/src/main/java/frc4388/robot/commands/TeleCommand.java index 87270c8..02c8c27 100644 --- a/src/main/java/frc4388/robot/commands/TeleCommand.java +++ b/src/main/java/frc4388/robot/commands/TeleCommand.java @@ -27,6 +27,6 @@ public class TeleCommand extends PelvicInflammatoryDisease { @Override public void runWithOutput(double output) { - arm.setTeleVel(output, true); + arm.setTeleVel(output); } } diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index ac33f65..06fab10 100644 --- a/src/main/java/frc4388/robot/subsystems/Arm.java +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -47,7 +47,7 @@ public class Arm extends SubsystemBase { this(pivot, tele, encoder, false); } - public void setRotVel(double vel, boolean fast) { + public void setRotVel(double vel) { var degrees = Math.abs(getArmRotation()) - 135; SmartDashboard.putNumber("arm degrees", degrees); SmartDashboard.putNumber("arm rot vel", vel); @@ -55,23 +55,14 @@ 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, .2 * vel); + m_pivot.set(ControlMode.PercentOutput, .15 * vel); } else { - m_pivot.set(ControlMode.PercentOutput, (fast ? .8 : .3) * vel); + m_pivot.set(ControlMode.PercentOutput, .3 * vel); } } - public void setTeleVel(double vel, boolean speed) { - m_tele.set(ControlMode.PercentOutput, (speed ? -.9 : .7) * vel); - } - - public boolean isTeleIn() { - return m_tele.isRevLimitSwitchClosed() == 1 || - m_tele.getSelectedSensorPosition() < reverse_tele; - } - - public double getTeleUnit() { - return m_tele.getSelectedSensorPosition() - reverse_tele; + public void setTeleVel(double vel) { + m_tele.set(ControlMode.PercentOutput, -0.5 * vel); } public void armSetRotation(double rot) { @@ -108,8 +99,8 @@ public class Arm extends SubsystemBase { (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT); if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) { - setRotVel(pivot, false); - setTeleVel(tele, false); + setRotVel(pivot); + setTeleVel(tele); } } @@ -136,8 +127,6 @@ public class Arm extends SubsystemBase { m_tele.configReverseSoftLimitThreshold(tele_soft); m_tele.configForwardSoftLimitEnable(true); m_tele.configReverseSoftLimitEnable(true); - - reverse_tele = tele_soft; } else { m_tele.configForwardSoftLimitEnable(false); m_tele.configReverseSoftLimitEnable(false); @@ -146,9 +135,8 @@ public class Arm extends SubsystemBase { tele_softLimit = !tele_softLimit; } - boolean resetable = true; - boolean tele_reset = true; - double reverse_tele = 0; + boolean resetable = true; + boolean tele_reset = true; @Override public void periodic() { @@ -160,8 +148,6 @@ public class Arm extends SubsystemBase { m_tele.configReverseSoftLimitThreshold(1000 - tele_soft); m_tele.configForwardSoftLimitEnable(true); m_tele.configReverseSoftLimitEnable(true); - - reverse_tele = 1000 - tele_soft; tele_reset = false; } else if (m_tele.isFwdLimitSwitchClosed() == 0) { tele_reset = true; @@ -173,6 +159,12 @@ 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 556c63e..6db02ad 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -12,7 +12,6 @@ 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; @@ -38,7 +37,7 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default - public double rotTarget = 0; + public Rotation2d rotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ @@ -58,6 +57,7 @@ public class SwerveDrive extends SubsystemBase { if (fieldRelative) { double rot = 0; + if (rightStick.getNorm() > 0.05) { rotTarget = gyro.getAngle(); rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; @@ -71,15 +71,15 @@ public class SwerveDrive extends SubsystemBase { 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. Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - // Translation2d speed = leftStick.times(speedAdjust / leftStick.getNorm()); 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 * cubedSpeed.getX(), cubedSpeed.getY(), rot, gyro.getRotation2d().times(-1)); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); @@ -106,7 +106,7 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); - rotTarget = 0; + rotTarget = new Rotation2d(0); } public void stopModules() {