run claw and claw stuff (vcool)

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

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))
+      .whenPressed(new RunClaw(m_claws, ClawType.LEFT, true))
-        .whenReleased(() -> m_claws.setSpeed(0.0));
+      .whenPressed(new RunClaw(m_claws, ClawType.RIGHT, true));
+    
     new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
-        .whenPressed(() -> m_claws.setSpeed(-0.5))
+      .whenPressed(new RunClaw(m_claws, ClawType.LEFT, false))
-        .whenReleased(() -> m_claws.setSpeed(0.0));
+      .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));
   }
 
   /**

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);
+  }
+}

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;
+  public CANSparkMax m_leftClaw;
-  private CANSparkMax m_rightClaw;
+  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_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset);
-      m_rightClaw.getEncoder().setPosition(ClawConstants.OPEN_POSITION + m_rightOffset);
+      // 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_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset);
-      m_rightClaw.getEncoder().setPosition(ClawConstants.CLOSE_POSITION + m_rightOffset);
+      // 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();
   }
-}
+}