Code for claw mech. RobotContainer updated with button bindings and s…

src/main/java/frc/robot/Constants.java

@@ -29,4 +29,16 @@ public static final class ElevatorConstants {
         public static final boolean elevatorUpIsPositive = true;
 
     }
+
+    public static final class ClawConstants {
+        public static final int rightMotorPort = 3;
+        public static final int leftMotorPort = 12;
+
+        public static final int pfftPCMPort = 0;
+
+        public static final int clawOpenPosition = 10;
+        public static final double clawMotorSpeed = 0.5;
+
+        public static final double stallDelta = 2;
+    }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -13,16 +13,15 @@
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj.GenericHID.Hand;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.commands.claw.CloseAndLiftClawCommand;
 import frc.robot.commands.tank.DriveTankCommand;
-import frc.robot.commands.elevator.ElevatorUpCommand;
-import frc.robot.commands.elevator.ElevatorDownCommand;
-import frc.robot.commands.elevator.ElevatorStopCommand;
-import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.claw.ClawSubsystem;
 import frc.robot.subsystems.tank.TankSubsystem;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -39,9 +38,11 @@ public class RobotContainer {
 
   // Subsystems
   private final TankSubsystem tankSubsystem;
+  private final ClawSubsystem clawSubsystem;
 
   // Controllers
   private XboxController controlXbox = new XboxController(0);
+  private XboxController mechControlXbox = new XboxController(1);
 
   // Joysticks
   private Joystick joystickLeft = new Joystick(1);
@@ -63,11 +64,12 @@ public RobotContainer() {
       FileInputStream stream = new FileInputStream(new File(Filesystem.getDeployDirectory(), CONFIG_PATH));
       config.load(stream);
     } catch (IOException ie) {
-      System.out.println("config file not found");
+      System.out.println("Config file not found.");
     }
 
     // Instantiate subsystems
     tankSubsystem = new TankSubsystem();
+    clawSubsystem = new ClawSubsystem();
 
     // Instantiate commands
     tankCommand = new DriveTankCommand(tankSubsystem, 0, 0);
@@ -101,6 +103,25 @@ private void controllerBindings() {
       }
     };
     tankSubsystem.setDefaultCommand(new RunCommand(tank, tankSubsystem));
+
+    // Set up controller bindings for claw subsystem
+
+    // "A" controls open and closing of the claw
+    JoystickButton aButton = new JoystickButton(mechControlXbox, Button.kA.value);
+    aButton.whenPressed(() -> {
+      clawSubsystem.clawIsOpen = !clawSubsystem.clawIsOpen;
+    });
+
+    // "B" controls lifting of the claw
+    JoystickButton bButton = new JoystickButton(mechControlXbox, Button.kB.value);
+    bButton.whenPressed(() -> {
+      clawSubsystem.clawIsLifted = !clawSubsystem.clawIsLifted;
+    });
+
+    // "X" button: closes and lifts claw at the same time
+    JoystickButton xButton = new JoystickButton(mechControlXbox, Button.kX.value);
+    xButton.whenPressed(new CloseAndLiftClawCommand(clawSubsystem));
+
   }
 
   /**

src/main/java/frc/robot/commands/claw/CloseAndLiftClawCommand.java

@@ -0,0 +1,40 @@
+package frc.robot.commands.claw;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.claw.ClawSubsystem;
+
+public class CloseAndLiftClawCommand extends CommandBase {
+
+    @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
+    private final ClawSubsystem clawSubsystem;
+
+    public CloseAndLiftClawCommand(ClawSubsystem clawSubsystem) {
+        this.clawSubsystem = clawSubsystem;
+
+        addRequirements(clawSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+        // Once this command is triggered, close the claw
+        clawSubsystem.clawIsOpen = false;
+    }
+
+    @Override
+    public void execute() {
+        // Repeatedly check if the claw-closing is finished
+        if (clawSubsystem.isMotorStalled()) {
+            // If finished closing claw, lift the claw
+            clawSubsystem.clawIsLifted = true;
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        // If command has completed operation
+        if (clawSubsystem.clawIsLifted) {
+            return true;
+        }
+        return false;
+    }
+}

src/main/java/frc/robot/commands/tank/DriveTankCommand.java

@@ -33,7 +33,6 @@ public DriveTankCommand(TankSubsystem tankSubsystem, double yScale, double angul
     this.squareInput = squareInput;
   }
 
-  // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void initialize() {
     tankSubsystem.setCarDrivePowers(yScale, angularScale, squareInput);

src/main/java/frc/robot/subsystems/claw/ClawSubsystem.java

@@ -0,0 +1,101 @@
+package frc.robot.subsystems.claw;
+
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import edu.wpi.first.wpilibj.Solenoid;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+
+import static frc.robot.Constants.ClawConstants.*;
+
+public class ClawSubsystem extends SubsystemBase {
+  private TalonSRX rightMotor;
+  private TalonSRX leftMotor;
+  private Solenoid pfft;
+
+  // Position variables for the motors
+  private double rightPos;
+  private double leftPos;
+
+  // Set initial state: claw open and not lifted
+  public boolean clawIsOpen = true;
+  public boolean clawIsLifted = false;
+
+  public ClawSubsystem() {
+    CommandScheduler.getInstance().registerSubsystem(this);
+
+    // Initialize motors and pneumatic
+    rightMotor = new TalonSRX(rightMotorPort);
+    leftMotor = new TalonSRX(leftMotorPort);
+
+    pfft = new Solenoid(pfftPCMPort);
+
+    // Configure the motors for the encoders
+    rightMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
+    rightMotor.setSelectedSensorPosition(0);
+
+    leftMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
+    leftMotor.setSelectedSensorPosition(0);
+
+    // Set initial motor position vars
+    rightPos = 0;
+    leftPos = 0;
+
+  }
+
+  @Override
+  public void periodic() {
+    // If claw is meant to be open
+    if (clawIsOpen) {
+
+      // Set motor
+      if (rightMotor.getSelectedSensorPosition() < clawOpenPosition) {
+        rightMotor.set(ControlMode.PercentOutput, clawMotorSpeed);
+        leftMotor.set(ControlMode.PercentOutput, -clawMotorSpeed);
+      }
+    } else {
+      // If claw is meant to be closed; give motors power until motors stall
+      if (!isMotorStalled()) {
+        rightMotor.set(ControlMode.PercentOutput, -clawMotorSpeed);
+        leftMotor.set(ControlMode.PercentOutput, clawMotorSpeed);
+      }
+    }
+
+    // If claw is meant to be lifted, turn pneumatic on
+    if (clawIsLifted) {
+      pfft.set(true);
+    } else {
+      // If not, turn pfft off
+      pfft.set(false);
+    }
+  }
+
+  /**
+   * Checks if the claw motors are not moving (ie. if change in position is less
+   * than a small delta value).
+   * 
+   * @return true if stalled; false if moving
+   */
+  public boolean isMotorStalled() {
+    double newRightPos = rightMotor.getSelectedSensorPosition();
+    double newLeftPos = leftMotor.getSelectedSensorPosition();
+
+    // If motors are barely changing position
+    boolean isStalled = (Math.abs(newRightPos - rightPos) <= stallDelta)
+        && (Math.abs(newLeftPos - leftPos) <= stallDelta);
+
+    // Save new motor positions
+    rightPos = newRightPos;
+    leftPos = newLeftPos;
+
+    return isStalled;
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // This method will be called once per scheduler run during simulation
+  }
+}