From 2b0dd5336036769afaaefbd8884d4a7192a4749d Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 10 Mar 2022 17:06:17 -0700 Subject: [PATCH] TestButtons + soft limits --- src/main/java/frc4388/robot/Constants.java | 5 +++++ .../java/frc4388/robot/RobotContainer.java | 21 +++++++++++++------ .../java/frc4388/robot/subsystems/Claws.java | 21 +++++++++++++++++++ .../frc4388/robot/subsystems/Climber.java | 17 +++++++++++++++ 4 files changed, 58 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index c2a3108..64f285c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -169,6 +169,11 @@ public final class Constants { public static final double GEAR_BOX_RATIO = 144.d * 60.d / 24.d; + public static final double SHOULDER_SOFT_LIMIT_FORWARD = 0; + public static final double SHOULDER_SOFT_LIMIT_REVERSE = 0; + public static final double ELBOW_SOFT_LIMIT_FORWARD = 0; + public static final double ELBOW_SOFT_LIMIT_REVERSE = 0; + // PID Constants public static final int SHOULDER_SLOT_IDX = 0; public static final int SHOULDER_PID_LOOP_IDX = 1; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 865208a..a48646f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -94,7 +94,7 @@ public class RobotContainer { /* Subsystems */ private final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); - private final Claws m_claws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); + private final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); @@ -137,9 +137,10 @@ public class RobotContainer { // moves climber in xy space with two-axis input from the operator controller m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX(), getOperatorController().getLeftY()), + new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.4, getOperatorController().getLeftY() * 0.4), m_robotClimber)); + // IK command // m_robotClimber.setDefaultCommand( // new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(), @@ -217,12 +218,20 @@ public class RobotContainer { /* Operator Buttons */ // run claws new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(new RunClaw(m_claws, ClawType.LEFT, true)) - .whenPressed(new RunClaw(m_claws, ClawType.RIGHT, true)); + .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.2))) + .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whenPressed(new RunClaw(m_claws, ClawType.LEFT, false)) - .whenPressed(new RunClaw(m_claws, ClawType.RIGHT, false)); + .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, -0.2))) + .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.LEFT, 0.0))); + + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.2))) + .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); + + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whileHeld(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, -0.2))) + .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); /* * // activates "BoomBoom" diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index cb1bb93..2382dbc 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -74,6 +74,27 @@ public class Claws extends SubsystemBase { m_open = open; } + + + + public void runClaw(ClawType which, double input) { + + + if (which == Claws.ClawType.LEFT) { + m_leftClaw.set(input); + + } else if (which == Claws.ClawType.RIGHT) { + m_rightClaw.set(input); + } + } + + + + public void runClaws(double input) + { + m_leftClaw.set(input); + m_rightClaw.set(input); + } /** * Sets the state of both hooks * @param open The state of the hooks diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 23478db..9ad1f97 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -17,6 +17,7 @@ import com.ctre.phoenix.sensors.WPI_PigeonIMU; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants; import frc4388.robot.Constants.ClimberConstants; @@ -49,6 +50,16 @@ public class Climber extends SubsystemBase { m_shoulderOffset = m_shoulder.getSelectedSensorPosition(); m_elbowOffset = m_elbow.getSelectedSensorPosition(); + m_elbow.configForwardSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_FORWARD); + m_elbow.configForwardSoftLimitEnable(false); + m_elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_SOFT_LIMIT_REVERSE); + m_elbow.configReverseSoftLimitEnable(false); + + m_shoulder.configForwardSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_FORWARD); + m_shoulder.configForwardSoftLimitEnable(false); + m_shoulder.configReverseSoftLimitThreshold(ClimberConstants.SHOULDER_SOFT_LIMIT_REVERSE); + m_shoulder.configReverseSoftLimitEnable(false); + if(groundRelative) m_gyro = gyro; @@ -257,5 +268,11 @@ public void setMotors(double shoulderOutput, double elbowOutput) { m_robotAngle = robotAngle; m_robotAngle = 45; //45 is placeholder } + + @Override + public void periodic(){ + SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition()); + SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition()); + } }