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 double rightOpenPower = 0.5;
+        public static final double leftOpenPower = -0.5;
+
+        public static final double stallDelta = 1;
+    }
 }

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

@@ -13,16 +13,17 @@
 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.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.commands.claw.CloseClawCommand;
+import frc.robot.commands.claw.OpenClawCommand;
 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 +40,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 +66,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 +105,28 @@ private void controllerBindings() {
       }
     };
     tankSubsystem.setDefaultCommand(new RunCommand(tank, tankSubsystem));
+
+    // Set up controller bindings for claw subsystem
+
+    // While "X" is pressed, open the claw
+    JoystickButton xButton = new JoystickButton(mechControlXbox, Button.kX.value);
+    xButton.whenPressed(new OpenClawCommand(clawSubsystem, xButton));
+
+    // When "B" is pressed, close the claw
+    JoystickButton bButton = new JoystickButton(mechControlXbox, Button.kB.value);
+    bButton.whenPressed(new CloseClawCommand(clawSubsystem));
+
+    // When "A" is pressed, lift/lower the pneumatic
+    JoystickButton aButton = new JoystickButton(mechControlXbox, Button.kA.value);
+    aButton.whenPressed(() -> {
+      clawSubsystem.setClawLift(!clawSubsystem.getClawLift());
+    });
+
+    // When "Y" is pressed, close the claw then lift the pneumatic
+    JoystickButton yButton = new JoystickButton(mechControlXbox, Button.kY.value);
+    yButton.whenPressed(new SequentialCommandGroup(new CloseClawCommand(clawSubsystem), new RunCommand(() -> {
+      clawSubsystem.setClawLift(true);
+    })));
   }
 
   /**

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

@@ -0,0 +1,39 @@
+package frc.robot.commands.claw;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.claw.ClawSubsystem;
+
+public class CloseClawCommand extends CommandBase {
+    @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
+
+    private final ClawSubsystem clawSubsystem;
+
+    public CloseClawCommand(ClawSubsystem clawSubsystem) {
+        this.clawSubsystem = clawSubsystem;
+
+        addRequirements(clawSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        // Set motor powers to close the claw
+        clawSubsystem.setClosePowers();
+    }
+
+    @Override
+    public boolean isFinished() {
+        // If both motors are stalled, command is finished
+        return clawSubsystem.areBothMotorsStalled();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        // Stop motors
+        clawSubsystem.setNeutralPowers();
+    }
+
+}

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

@@ -0,0 +1,42 @@
+package frc.robot.commands.claw;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.subsystems.claw.ClawSubsystem;
+
+public class OpenClawCommand extends CommandBase {
+    @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
+
+    private final ClawSubsystem clawSubsystem;
+    private JoystickButton button;
+
+    public OpenClawCommand(ClawSubsystem clawSubsystem, JoystickButton button) {
+        this.clawSubsystem = clawSubsystem;
+        this.button = button;
+
+        addRequirements(clawSubsystem);
+    }
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        // Set motor powers to open the claw
+        clawSubsystem.setOpenPowers();
+    }
+
+    @Override
+    public boolean isFinished() {
+        // If button is still pressed, the command is not finished
+        return !button.get();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        // Stop motors
+        clawSubsystem.setNeutralPowers();
+    }
+
+}

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,133 @@
+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;
+
+  // Right and left motor powers -- state variables
+  private double rightMotorPower;
+  private double leftMotorPower;
+
+  // State variable for whether the pneumatic is lifted or not
+  private boolean liftClaw;
+
+  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 (for checking if motors are stalled)
+    rightPos = 0;
+    leftPos = 0;
+
+    liftClaw = false;
+
+  }
+
+  @Override
+  public void periodic() {
+    System.out.println("right: " + rightMotor.getSelectedSensorPosition());
+    System.out.println("left: " + leftMotor.getSelectedSensorPosition());
+
+    // Set motor powers
+    rightMotor.set(ControlMode.PercentOutput, rightMotorPower);
+    leftMotor.set(ControlMode.PercentOutput, leftMotorPower);
+
+    // Set pneumatic
+    pfft.set(liftClaw);
+  }
+
+  /**
+   * Set motor powers to open.
+   */
+  public void setOpenPowers() {
+    rightMotorPower = rightOpenPower;
+    leftMotorPower = leftOpenPower;
+  }
+
+  /**
+   * Set motor powers to close.
+   */
+  public void setClosePowers() {
+    rightMotorPower = -rightOpenPower;
+    leftMotorPower = -leftOpenPower;
+  }
+
+  /**
+   * Set motor powers to zero.
+   */
+  public void setNeutralPowers() {
+    rightMotorPower = 0;
+    leftMotorPower = 0;
+  }
+
+  public boolean getClawLift() {
+    return liftClaw;
+  }
+
+  public void setClawLift(boolean liftClaw) {
+    this.liftClaw = liftClaw;
+  }
+
+  /**
+   * 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(boolean right) {
+    double newRightPos = rightMotor.getSelectedSensorPosition();
+    double newLeftPos = leftMotor.getSelectedSensorPosition();
+
+    // If motors are barely changing position
+    boolean isRightStalled = Math.abs(newRightPos - rightPos) <= stallDelta;
+    boolean isLeftStalled = Math.abs(newLeftPos - leftPos) <= stallDelta;
+
+    // Save new motor positions
+    rightPos = newRightPos;
+    leftPos = newLeftPos;
+
+    return right ? isRightStalled : isLeftStalled;
+  }
+
+  /**
+   * Returns true if both motors are stalled.
+   * 
+   * @return true if both are stalled
+   */
+  public boolean areBothMotorsStalled() {
+    return isMotorStalled(true) && isMotorStalled(false);
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // This method will be called once per scheduler run during simulation
+  }
+}