diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 83ac014..f82c168 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -24,9 +24,13 @@ 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; + 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 = 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 1b6fb4c..1e721ef 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,7 +14,10 @@ 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; @@ -25,6 +28,7 @@ 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; @@ -85,8 +89,8 @@ public class RobotContainer { .withName("SwerveDrive DefaultCommand")); m_robotArm.setDefaultCommand(new RunCommand(() -> { - m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY()); - m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY()); + m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY(), false); + m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY(), false); }, m_robotArm) .withName("Arm DefaultCommand")); @@ -119,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) @@ -149,13 +153,23 @@ public class RobotContainer { .onTrue(new PivotCommand(m_robotArm, 135)); new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(new PivotCommand(m_robotArm, 210)); + .onTrue(new PivotCommand(m_robotArm, 225)); new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotClaw.toggle())); + .onTrue(new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw)); 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 new file mode 100644 index 0000000..d2db4b4 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/ExtendArmTo.java @@ -0,0 +1,36 @@ +// 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/PivotCommand.java b/src/main/java/frc4388/robot/commands/PivotCommand.java index a84690d..fcb54d1 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); + arm.setRotVel(output, true); } } diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/RunArmIn.java similarity index 68% rename from src/main/java/frc4388/robot/commands/LimeAlign.java rename to src/main/java/frc4388/robot/commands/RunArmIn.java index c811a3e..c1ac04b 100644 --- a/src/main/java/frc4388/robot/commands/LimeAlign.java +++ b/src/main/java/frc4388/robot/commands/RunArmIn.java @@ -5,22 +5,25 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Arm; -public class LimeAlign extends CommandBase { - public LimeAlign(SwerveDrive drive) { - addRequirements(drive); +public class RunArmIn extends CommandBase { + private final Arm m_arm; + + public RunArmIn(Arm arm) { + m_arm = arm; + addRequirements(); } // 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() {} + public void execute() { + m_arm.setTeleVel(1, true); + } // Called once the command ends or is interrupted. @Override @@ -29,6 +32,6 @@ public class LimeAlign extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_arm.isTeleIn(); } } diff --git a/src/main/java/frc4388/robot/commands/TeleCommand.java b/src/main/java/frc4388/robot/commands/TeleCommand.java index 02c8c27..87270c8 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); + arm.setTeleVel(output, true); } } diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java index 06fab10..ac33f65 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) { + public void setRotVel(double vel, boolean fast) { var degrees = Math.abs(getArmRotation()) - 135; SmartDashboard.putNumber("arm degrees", degrees); SmartDashboard.putNumber("arm rot vel", vel); @@ -55,14 +55,23 @@ 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, .2 * vel); } else { - m_pivot.set(ControlMode.PercentOutput, .3 * vel); + m_pivot.set(ControlMode.PercentOutput, (fast ? .8 : .3) * vel); } } - public void setTeleVel(double vel) { - m_tele.set(ControlMode.PercentOutput, -0.5 * 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 armSetRotation(double rot) { @@ -99,8 +108,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); - setTeleVel(tele); + setRotVel(pivot, false); + setTeleVel(tele, false); } } @@ -127,6 +136,8 @@ 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); @@ -135,8 +146,9 @@ public class Arm extends SubsystemBase { tele_softLimit = !tele_softLimit; } - boolean resetable = true; - boolean tele_reset = true; + boolean resetable = true; + boolean tele_reset = true; + double reverse_tele = 0; @Override public void periodic() { @@ -148,6 +160,8 @@ 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; @@ -159,12 +173,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 fcaadb9..d4a27fa 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,19 +57,23 @@ public class SwerveDrive extends SubsystemBase { if (fieldRelative) { double rot = 0; - if (rightStick.getNorm() > 0.1) { - rotTarget = gyro.getRotation2d(); - rot = rightStick.getX(); + 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(); + 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 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(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); @@ -95,7 +100,7 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); - rotTarget = new Rotation2d(0); + rotTarget = 0; } public void stopModules() { @@ -113,11 +118,19 @@ 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.ROTATION_SPEED = SwerveDriveConstants.MAX_ROT_SPEED; + + SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MAX; } else { + this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED; + + SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; } }