diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index aea43a0..b9fbf40 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -207,6 +207,10 @@ public final class Constants { public static final double CALIBRATION_SPEED = 0; public static final double CURRENT_LIMIT = 0.0; // TODO: set actual current limit; + + public static final int TOP_LIMIT = 1800; + public static final int BOTTOM_LIMIT = 1200; + } /** * The OIConstants class contains the ID for the XBox controllers diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b94ae50..35ae23d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -137,9 +137,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().getLeftX() * 0.4, getOperatorController().getLeftY() * 0.4), - m_robotClimber)); + // m_robotClimber.setDefaultCommand( + // new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getLeftX() * 0.4, getOperatorController().getLeftY() * 0.4), + // m_robotClimber)); // IK command @@ -235,10 +235,10 @@ public class RobotContainer { // .whenReleased(new RunCommand(() -> m_robotClaws.runClaw(ClawType.RIGHT, 0.0))); new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(new RunCommand(() -> m_robotClaws.setOpen(true))); + .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whenPressed(new RunCommand(() -> m_robotClaws.setOpen(false))); + .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); } /* diff --git a/src/main/java/frc4388/robot/commands/RunClaw.java b/src/main/java/frc4388/robot/commands/RunClaw.java index f3c70ec..0c4e8b7 100644 --- a/src/main/java/frc4388/robot/commands/RunClaw.java +++ b/src/main/java/frc4388/robot/commands/RunClaw.java @@ -37,7 +37,7 @@ public class RunClaw extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_claws.runClaw(clawType, open); + // m_claws.runClaw(clawType, open); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java index 1866be3..38ad651 100644 --- a/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java +++ b/src/main/java/frc4388/robot/commands/climber/RunClimberPath.java @@ -42,23 +42,23 @@ public class RunClimberPath extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(!claws.fullyOpen()) + // if(!claws.fullyOpen()) return; - Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles()); + // Point climberPos = ClimberRewrite.getClimberPosition(climber.getJointAngles()); - Vector2D dir = new Vector2D(path[nextIndex]); - dir.subtract(new Vector2D(climberPos)); + // Vector2D dir = new Vector2D(path[nextIndex]); + // dir.subtract(new Vector2D(climberPos)); - if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1) - nextIndex++; - else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) { - endPath = true; - claws.setOpen(false); - } else if(!endPath) { - dir = dir.unit(); - climber.controlWithInput(dir.x, dir.y); - } + // if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD && nextIndex < path.length-1) + // nextIndex++; + // else if(!endPath && dir.magnitude() < ClimberConstants.THRESHOLD) { + // endPath = true; + // claws.setOpen(false); + // } else if(!endPath) { + // dir = dir.unit(); + // climber.controlWithInput(dir.x, dir.y); + // } } // Called once the command ends or is interrupted. @@ -68,6 +68,7 @@ public class RunClimberPath extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return endPath && claws.fullyClosed(); + // return endPath && claws.fullyClosed(); + return false; } } diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index 3ea40a8..300fb34 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -8,6 +8,7 @@ import com.revrobotics.SparkMaxLimitSwitch; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ClawConstants; import frc4388.robot.Constants.ClimberConstants; @@ -59,71 +60,76 @@ public class Claws extends SubsystemBase { * @param which Which claw to run. * @param open Whether to open or close the claw. */ - public void runClaw(ClawType which, boolean open) { + // public void runClaw(ClawType which, boolean open) { - int direction = open ? 1 : -1; + // int direction = open ? 1 : -1; - if (which == Claws.ClawType.LEFT) { + // if (which == Claws.ClawType.LEFT) { - // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset; - // m_leftClaw.getEncoder().setPosition(setPos); - m_leftClaw.setSpeed(direction * 0.1); + // // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset; + // // m_leftClaw.getEncoder().setPosition(setPos); + // m_leftClaw.setSpeed(direction * 0.1); - } else if (which == Claws.ClawType.RIGHT) { + // } else if (which == Claws.ClawType.RIGHT) { - // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset; - // m_rightClaw.getEncoder().setPosition(setPos); - m_rightClaw.setSpeed(direction * 0.1); - } + // // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset; + // // m_rightClaw.getEncoder().setPosition(setPos); + // m_rightClaw.setSpeed(direction * 0.1); + // } - m_open = open; - } + // m_open = open; + // } - public void runClaw(ClawType which, double input) { - if (which == Claws.ClawType.LEFT) { - m_leftClaw.setSpeed(input); + // public void runClaw(ClawType which, double input) { + // if (which == Claws.ClawType.LEFT) { + // m_leftClaw.setSpeed(input); - } else if (which == Claws.ClawType.RIGHT) { - m_rightClaw.setSpeed(input); - } - } + // } else if (which == Claws.ClawType.RIGHT) { + // m_rightClaw.setSpeed(input); + // } + // } - public void runClaws(double input) - { - m_leftClaw.setSpeed(input); - m_rightClaw.setSpeed(input); - } + // public void runClaws(double input) + // { + // m_leftClaw.setSpeed(input); + // m_rightClaw.setSpeed(input); + // } /** * Sets the state of both hooks * @param open The state of the hooks */ public void setOpen(boolean open) { + SmartDashboard.putBoolean("open", open); if(open) { // m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset); // m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset); - m_leftClaw.set(-ClawConstants.OPEN_POSITION); - m_rightClaw.set(ClawConstants.OPEN_POSITION); + // m_leftClaw.setPosition(.7); + // m_rightClaw.setPosition(.7); + m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT);//ClawConstants.OPEN_POSITION); + m_rightClaw.setRaw(ClawConstants.TOP_LIMIT);//ClawConstants.OPEN_POSITION); } else { // m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset); // m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset); - m_leftClaw.set(-ClawConstants.CLOSE_POSITION); - m_rightClaw.set(ClawConstants.CLOSE_POSITION); + // m_leftClaw.setPosition(.3); + // m_rightClaw.setPosition(.3); + m_leftClaw.setRaw(ClawConstants.TOP_LIMIT); + m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT); } } - public double[] getOffsets() { - return new double[] {m_leftOffset, m_rightOffset}; - } + // public double[] getOffsets() { + // return new double[] {m_leftOffset, m_rightOffset}; + // } - public boolean fullyOpen() { - return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD && - Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; } + // public boolean fullyOpen() { + // return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD && + // Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; } - public boolean fullyClosed() { - return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD && - Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD; - } + // public boolean fullyClosed() { + // return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD && + // Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD; + // } /** * Check if a limit switch is pressed or current limit exceeded for a claw. @@ -145,12 +151,12 @@ public class Claws extends SubsystemBase { // return false; // } - @Override - public void periodic() { - if (fullyOpen() || fullyClosed()) { - m_leftClaw.setSpeed(0.0); - m_rightClaw.setSpeed(0.0); - } + // @Override + // public void periodic() { + // if (fullyOpen() || fullyClosed()) { + // m_leftClaw.setSpeed(0.0); + // m_rightClaw.setSpeed(0.0); + // } // if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed()) // // m_leftOffset = m_leftClaw.getEncoder().getPosition(); @@ -159,5 +165,5 @@ public class Claws extends SubsystemBase { // if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed()) // // m_rightOffset = m_rightClaw.getEncoder().getPosition(); // m_rightOffset = m_rightClaw.getPosition(); - } + // } } \ No newline at end of file