diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5408f11..4a04665 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -173,6 +173,10 @@ public final class Constants { public static final int SHOULDER_ID = 30; public static final int ELBOW_ID = 31; public static final int GYRO_ID = 14; + + public static final double INPUT_MULTIPLIER = 0.7; + public static final double ELBOW_SOFT_LIMIT_TOLERANCE = 20000.0; + public static final double SHOULDER_SOFT_LIMIT_TOLERANCE = 12000.0; // TODO Update this stuff too public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm @@ -192,10 +196,10 @@ public final class Constants { public static final double ELBOW_GB_RATIO = 1.d; public static final double SHOULDER_GB_RATIO = 1.d; - public static final double SHOULDER_SOFT_LIMIT_FORWARD = 53869; - public static final double SHOULDER_SOFT_LIMIT_REVERSE = 0; - public static final double ELBOW_SOFT_LIMIT_FORWARD = 281717; - public static final double ELBOW_SOFT_LIMIT_REVERSE = 0; + public static final double SHOULDER_FORWARD_SOFT_LIMIT = 53869; + public static final double SHOULDER_REVERSE_SOFT_LIMIT = 0; + public static final double ELBOW_FORWARD_SOFT_LIMIT = 281717; + public static final double ELBOW_REVERSE_SOFT_LIMIT = 0; // PID Constants public static final int SHOULDER_SLOT_IDX = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 947f92a..625b1de 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -61,7 +61,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Claws; -import frc4388.robot.commands.RunClaw; import frc4388.robot.subsystems.ClimberRewrite; import frc4388.robot.subsystems.Claws.ClawType; import frc4388.robot.Constants.OIConstants; @@ -69,6 +68,7 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.StorageConstants; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; +import frc4388.robot.commands.ClimberCommands.RunClaw; // import frc4388.robot.commands.ButtonBoxCommands.TurretManual; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; @@ -158,9 +158,9 @@ public class RobotContainer { /* Default Commands */ // moves climber in xy space with two-axis input from the operator controller - m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightX() * 0.7, -getOperatorController().getRightY() * 0.7), - m_robotClimber)); + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightX() * 0.7, -getOperatorController().getRightY() * 0.7), + // m_robotClimber)); // IK command @@ -206,8 +206,8 @@ public class RobotContainer { new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); - // m_robotHood.setDefaultCommand( - // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); + m_robotHood.setDefaultCommand( + new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); // m_robotTurret.setDefaultCommand( // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); @@ -318,8 +318,22 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); - // new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - // .whileHeld(new TurretManual(m_robotTurret)); + new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) + + .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null))) + .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null))) + .whenPressed(new InstantCommand(() -> m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getLeftY(), -getOperatorController().getRightY()), m_robotClimber)))) + + .whenReleased(new InstantCommand(() -> m_robotClimber.setDefaultCommand(null))) + .whenReleased(new InstantCommand(() -> m_robotTurret.setDefaultCommand( + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), m_robotTurret)))) + .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand( + new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)))); + + // .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) + // .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); + // .whenReleased(EnableClimber())); // control turret manual mode // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) @@ -407,7 +421,6 @@ public class RobotContainer { * Finally, adds the existing path files to the auto chooser */ private void autoInit() { - try { WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, diff --git a/src/main/java/frc4388/robot/commands/RunClaw.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java similarity index 96% rename from src/main/java/frc4388/robot/commands/RunClaw.java rename to src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java index 0c4e8b7..fdcd151 100644 --- a/src/main/java/frc4388/robot/commands/RunClaw.java +++ b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClaw.java @@ -2,7 +2,7 @@ // 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; +package frc4388.robot.commands.ClimberCommands; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.ClawConstants; diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java similarity index 97% rename from src/main/java/frc4388/robot/commands/climber/RunClimberPath.java rename to src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java index 38ad651..23f37e6 100644 --- a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java +++ b/src/main/java/frc4388/robot/commands/ClimberCommands/RunClimberPath.java @@ -2,7 +2,7 @@ // 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.climber; +package frc4388.robot.commands.ClimberCommands; import org.opencv.core.Point; diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index 620e6fe..78cc123 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -37,6 +37,8 @@ public class ClimberRewrite extends SubsystemBase { private Point tPoint; private boolean groundRelative; + private double shoulderSpeedLimiter; + private double elbowSpeedLimiter; /** Creates a new ClimberRewrite. */ public ClimberRewrite(WPI_TalonFX shoulder, WPI_TalonFX elbow, WPI_Pigeon2 gyro, boolean _groundRelative) { @@ -55,14 +57,14 @@ public class ClimberRewrite extends SubsystemBase { // m_shoulder.setSelectedSensorPosition(((ClimberConstants.SHOULDER_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO); // m_elbow.setSelectedSensorPosition(((ClimberConstants.ELBOW_RESTING_ANGLE * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI) * ClimberConstants.SHOULDER_GB_RATIO); - m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD); + m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); m_elbow.configForwardSoftLimitEnable(true); // m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); // m_elbow.configReverseSoftLimitEnable(true); - m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD); + m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); m_shoulder.configForwardSoftLimitEnable(true); - m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE); + m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); m_shoulder.configReverseSoftLimitEnable(false); m_shoulder.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); @@ -78,6 +80,7 @@ public class ClimberRewrite extends SubsystemBase { m_gyro = gyro; groundRelative = _groundRelative; + this.elbowSpeedLimiter = 1.0; } public void setClimberGains() { @@ -129,8 +132,8 @@ public class ClimberRewrite extends SubsystemBase { } public void setMotors(double shoulderOutput, double elbowOutput) { - m_shoulder.set(shoulderOutput); - m_elbow.set(elbowOutput); + m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter); + m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter); } public double[] getJointAngles() { @@ -179,6 +182,40 @@ public class ClimberRewrite extends SubsystemBase { SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); // setJointAngles(jointAngles); + + // * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). + double currentElbowPos = this.m_elbow.getSelectedSensorPosition(); + double forwardElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); + double reverseElbowDistance = Math.abs(currentElbowPos - ClimberConstants.ELBOW_REVERSE_SOFT_LIMIT); + + if (forwardElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { + this.elbowSpeedLimiter = 0.15 + (forwardElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); + } + + if (reverseElbowDistance < ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) { + this.elbowSpeedLimiter = 0.15 + (reverseElbowDistance * (1 / ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE) && (reverseElbowDistance > ClimberConstants.ELBOW_SOFT_LIMIT_TOLERANCE)) { + this.elbowSpeedLimiter = 1.0; + } + + // * speed limiting near SHOULDER soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output). + double currentShoulderPos = this.m_shoulder.getSelectedSensorPosition(); + double forwardShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_FORWARD_SOFT_LIMIT); + double reverseShoulderDistance = Math.abs(currentShoulderPos - ClimberConstants.SHOULDER_REVERSE_SOFT_LIMIT); + + if (forwardShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { + this.shoulderSpeedLimiter = 0.15 + (forwardShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); + } + + if (reverseShoulderDistance < ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) { + this.shoulderSpeedLimiter = 0.15 + (reverseShoulderDistance * (1 / ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)); + } + + if ((forwardShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE) && (reverseShoulderDistance > ClimberConstants.SHOULDER_SOFT_LIMIT_TOLERANCE)) { + this.shoulderSpeedLimiter = 1.0; + } } /** diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 0652f4b..5f720d4 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -68,19 +68,16 @@ public class Hood extends SubsystemBase { double reverseDistance = Math.abs(currentPos - ShooterConstants.HOOD_REVERSE_SOFT_LIMIT); if (forwardDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (forwardDistance * 0.05); + this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); } if (reverseDistance < ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (reverseDistance * 0.05); + this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)); } if ((forwardDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.HOOD_SOFT_LIMIT_TOLERANCE)) { this.speedLimiter = 1.0; } - - double hoodCurrent = m_angleAdjusterMotor.getOutputCurrent(); - } /** diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index de1c9eb..9d9e35c 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -152,11 +152,11 @@ public class Turret extends SubsystemBase { double reverseDistance = Math.abs(currentPos - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT); if (forwardDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (forwardDistance * 0.05); + this.speedLimiter = 0.2 + (forwardDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); } if (reverseDistance < ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) { - this.speedLimiter = 0.2 + (reverseDistance * 0.05); + this.speedLimiter = 0.2 + (reverseDistance * (1 / ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)); } if ((forwardDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE) && (reverseDistance > ShooterConstants.TURRET_SOFT_LIMIT_TOLERANCE)) {