From 11359e4c02268893fab3d072ebe98c960faa77e1 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 5 Mar 2022 14:38:54 -0700 Subject: [PATCH] run claw and claw stuff (vcool) --- src/main/java/frc4388/robot/Constants.java | 2 + .../java/frc4388/robot/RobotContainer.java | 21 ++++-- .../java/frc4388/robot/commands/RunClaw.java | 57 +++++++++++++++ .../java/frc4388/robot/subsystems/Claws.java | 73 ++++++++++++++++--- 4 files changed, 139 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/RunClaw.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index cfc8496..e3508db 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -124,6 +124,8 @@ public final class Constants { public static final double CLOSE_POSITION = 0; public static final double CALIBRATION_SPEED = 0; + + public static final double CURRENT_LIMIT = 0.0; // TODO: set actual current limit; } /** * 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 1b15a07..b74b0b4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,9 +11,11 @@ import edu.wpi.first.wpilibj2.command.RunCommand; 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.Climber; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Claws.ClawType; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -86,13 +88,22 @@ public class RobotContainer { /*new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));*/ - + + // run claws new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whenPressed(() -> m_claws.setSpeed(0.5)) - .whenReleased(() -> m_claws.setSpeed(0.0)); + .whenPressed(new RunClaw(m_claws, ClawType.LEFT, true)) + .whenPressed(new RunClaw(m_claws, ClawType.RIGHT, true)); + new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whenPressed(() -> m_claws.setSpeed(-0.5)) - .whenReleased(() -> m_claws.setSpeed(0.0)); + .whenPressed(new RunClaw(m_claws, ClawType.LEFT, false)) + .whenPressed(new RunClaw(m_claws, ClawType.RIGHT, false)); + + // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + // .whenPressed(() -> m_claws.setSpeed(0.5)) + // .whenReleased(() -> m_claws.setSpeed(0.0)); + // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + // .whenPressed(() -> m_claws.setSpeed(-0.5)) + // .whenReleased(() -> m_claws.setSpeed(0.0)); } /** diff --git a/src/main/java/frc4388/robot/commands/RunClaw.java b/src/main/java/frc4388/robot/commands/RunClaw.java new file mode 100644 index 0000000..8f2cfa0 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunClaw.java @@ -0,0 +1,57 @@ +// 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.Constants.ClawConstants; +import frc4388.robot.subsystems.Claws; + +public class RunClaw extends CommandBase { + + // parameters + public Claws m_claws; + public Claws.ClawType clawType; + public boolean open; + + // current limit + public double currentLimit; + + /** + * Creates a new RunClaw, which runs a claw. + * @param sClaws Claws subsystem. + * @param which Which claw to run. + * @param open Whether to open or close the claw. + */ + public RunClaw(Claws sClaws, Claws.ClawType which, boolean open) { + // Use addRequirements() here to declare subsystem dependencies. + m_claws = sClaws; + clawType = which; + this.open = open; + + currentLimit = ClawConstants.CURRENT_LIMIT; + + addRequirements(m_claws); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_claws.runClaw(clawType, open); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_claws.checkSwitchAndCurrent(clawType, currentLimit); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index d76c7c1..cbebc47 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -9,8 +9,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ClawConstants; public class Claws extends SubsystemBase { - private CANSparkMax m_leftClaw; - private CANSparkMax m_rightClaw; + public CANSparkMax m_leftClaw; + public CANSparkMax m_rightClaw; private SparkMaxLimitSwitch m_leftLimitSwitchF; private SparkMaxLimitSwitch m_rightLimitSwitchF; @@ -21,6 +21,7 @@ public class Claws extends SubsystemBase { private double m_rightOffset; private boolean m_open; + public static enum ClawType {LEFT, RIGHT} public Claws(CANSparkMax leftClaw, CANSparkMax rightClaw) { m_leftClaw = leftClaw; @@ -49,28 +50,82 @@ public class Claws extends SubsystemBase { m_rightClaw.set(speed); } + /** + * Run a specific claw to open or close. + * @param which Which claw to run. + * @param open Whether to open or close the claw. + */ + public void runClaw(ClawType which, boolean open) { + 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.set(0.1); + + } 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.set(0.1); + } + } + public void setOpen(boolean open) { if(open) { - m_leftClaw.getEncoder().setPosition(ClawConstants.OPEN_POSITION + m_leftOffset); - m_rightClaw.getEncoder().setPosition(ClawConstants.OPEN_POSITION + m_rightOffset); + // m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset); + // m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset); + m_leftClaw.set(0.1); + m_rightClaw.set(0.1); } else { - m_leftClaw.getEncoder().setPosition(ClawConstants.CLOSE_POSITION + m_leftOffset); - m_rightClaw.getEncoder().setPosition(ClawConstants.CLOSE_POSITION + m_rightOffset); + // m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset); + // m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset); + m_leftClaw.set(-0.1); + m_rightClaw.set(-0.1); } m_open = open; } + public double[] getOffsets() { + return new double[] {m_leftOffset, m_rightOffset}; + } + public boolean getOpen() { return m_open; } + /** + * Check if a limit switch is pressed or current limit exceeded for a claw. + * @param which Which claw to check. + * @param limit The current limit. + * @return Whether to interrupt the RunClaw command or not. + */ + public boolean checkSwitchAndCurrent(ClawType which, double limit) { + + // if still calibrating, stop RunClaw + /*if (((Double) m_leftOffset == null) || ((Double) m_rightOffset == null)) { + return true; + }*/ + + if (which == ClawType.LEFT) { + if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= limit) { + return true; + } + } else if (which == ClawType.RIGHT) { + if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= limit) { + return true; + } + } + + return false; + } + @Override public void periodic() { - if(m_leftLimitSwitchR.isPressed()) + if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed()) m_leftOffset = m_leftClaw.getEncoder().getPosition(); - if(m_rightLimitSwitchR.isPressed()) + if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed()) m_rightOffset = m_rightClaw.getEncoder().getPosition(); } -} +} \ No newline at end of file