From 1b6265c65adf858ad7cd818dd74e0aa98a6bbab8 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 14:26:03 -0600 Subject: [PATCH 1/3] new try at turret or climber thing --- src/main/java/frc4388/robot/Constants.java | 1 + .../java/frc4388/robot/RobotContainer.java | 45 +++++-------------- .../robot/subsystems/ClimberRewrite.java | 7 +-- 3 files changed, 13 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7a06fd9..b2ba4d6 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -175,6 +175,7 @@ public final class Constants { public static final int GYRO_ID = 14; public static final double INPUT_MULTIPLIER = 0.7; + public static final double CLIMBER_SOFT_LIMIT_TOLERANCE = 20.0; // TODO Update this stuff too public static final double UPPER_ARM_LENGTH = 26; // Units should be in cm diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5cb54c8..29d213b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,14 +112,10 @@ public class RobotContainer { /* Subsystems */ public final ClimberRewrite m_robotClimber = new ClimberRewrite(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); -<<<<<<< Updated upstream private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); // Subsystems -======= - public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); ->>>>>>> Stashed changes public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor); @@ -203,7 +199,6 @@ public class RobotContainer { m_robotSerializer.setDefaultCommand( new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); -<<<<<<< Updated upstream // Turret Manual @@ -217,12 +212,6 @@ public class RobotContainer { .until(() -> this.currentMode.equals(CurrentMode.TURRET))); // EnableTurret() -======= - // Turret Manual - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); ->>>>>>> Stashed changes // m_robotHood.setDefaultCommand( // new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood)); @@ -330,8 +319,17 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) - .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); + + .whenPressed(new InstantCommand(() -> m_robotTurret.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)))); + + // .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER)) + // .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET)); // .whenReleased(EnableClimber())); // control turret manual mode @@ -425,26 +423,6 @@ public class RobotContainer { * Finally, adds the existing path files to the auto chooser */ private void autoInit() { -<<<<<<< Updated upstream - // try { - // WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), - // StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, - // StandardWatchEventKinds.ENTRY_DELETE); - // // TODO: Store this and other commands as fields so they can be rescheduled. - // new NotifierCommand(() -> updateAutoChooser(watchKey), 0.5) { - // @Override - // public boolean runsWhenDisabled() { - // return true; - // } - // }.withName("Path Watcher").schedule(); - // } catch (IOException exception) { - // LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception); - // } - // Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles()) - // .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) - // .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); - // SmartDashboard.putData("Auto Chooser", autoChooser); -======= try { WatchKey watchKey = PATHPLANNER_DIRECTORY.register(FileSystems.getDefault().newWatchService(), StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY, @@ -463,7 +441,6 @@ public class RobotContainer { .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified)) .forEachOrdered(file -> autoChooser.addOption(file.getName(), file)); SmartDashboard.putData("Auto Chooser", autoChooser); ->>>>>>> Stashed changes } /** diff --git a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index a9d208a..621b213 100644 --- a/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java +++ b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java @@ -179,6 +179,7 @@ public class ClimberRewrite extends SubsystemBase { SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); // double[] jointAngles = getTargetJointAngles(tPoint, 0.d); // setJointAngles(jointAngles); + } /** @@ -286,11 +287,6 @@ public class ClimberRewrite extends SubsystemBase { return getClimberPosition(jointAngles[0], jointAngles[1]); } -<<<<<<< Updated upstream - public double getCurrent() { - return (this.m_elbow.getSupplyCurrent() + this.m_shoulder.getSupplyCurrent()); - } -======= public void setClimberSoftLimits(boolean set){ m_elbow.configForwardSoftLimitEnable(set); m_shoulder.configForwardSoftLimitEnable(set); @@ -305,5 +301,4 @@ public class ClimberRewrite extends SubsystemBase { return (m_shoulder.getSupplyCurrent() + m_elbow.getSupplyCurrent()); } ->>>>>>> Stashed changes } From cb3f1942b611ce5a1e360174cdac455e0f0c1b18 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 14:45:27 -0600 Subject: [PATCH 2/3] speed limiters for elbow and shoulder --- src/main/java/frc4388/robot/Constants.java | 11 +++-- .../robot/subsystems/ClimberRewrite.java | 48 ++++++++++++++++--- .../java/frc4388/robot/subsystems/Hood.java | 7 +-- .../java/frc4388/robot/subsystems/Turret.java | 4 +- 4 files changed, 52 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b2ba4d6..9c2e1a0 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -175,7 +175,8 @@ public final class Constants { public static final int GYRO_ID = 14; public static final double INPUT_MULTIPLIER = 0.7; - public static final double CLIMBER_SOFT_LIMIT_TOLERANCE = 20.0; + 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 @@ -195,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/subsystems/ClimberRewrite.java b/src/main/java/frc4388/robot/subsystems/ClimberRewrite.java index 621b213..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 * ClimberConstants.INPUT_MULTIPLIER); - m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER); + m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter); + m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter); } public double[] getJointAngles() { @@ -179,7 +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)) { From 517053ea50da372d969366a9db52eef156d719ed Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Mar 2022 15:05:07 -0600 Subject: [PATCH 3/3] file change --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../frc4388/robot/commands/{ => ClimberCommands}/RunClaw.java | 2 +- .../commands/{climber => ClimberCommands}/RunClimberPath.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) rename src/main/java/frc4388/robot/commands/{ => ClimberCommands}/RunClaw.java (96%) rename src/main/java/frc4388/robot/commands/{climber => ClimberCommands}/RunClimberPath.java (97%) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 19c9887..5b1b59f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -62,7 +62,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; @@ -70,6 +69,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; 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;