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

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

@@ -29,4 +29,14 @@ 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;
+    }
 }

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

@@ -17,12 +17,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 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 +35,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);
@@ -68,6 +66,7 @@ public RobotContainer() {
 
     // Instantiate subsystems
     tankSubsystem = new TankSubsystem();
+    clawSubsystem = new ClawSubsystem();
 
     // Instantiate commands
     tankCommand = new DriveTankCommand(tankSubsystem, 0, 0);
@@ -99,6 +98,22 @@ private void controllerBindings() {
       } else {
         tankSubsystem.setTankDrivePowers(-joystickLeft.getY(), -joystickRight.getY());
       }
+
+      // "A" button controls open and closing of the claw
+      if (mechControlXbox.getAButtonReleased()) {
+        clawSubsystem.clawIsOpen = !clawSubsystem.clawIsOpen;
+      }
+
+      // "B" button controls lifting of the claw
+      if (mechControlXbox.getBButtonPressed()) {
+        clawSubsystem.clawIsLifted = !clawSubsystem.clawIsLifted;
+      }
+
+      // "X" button: closes and lifts claw at the same time
+      if (mechControlXbox.getXButtonPressed()) {
+        clawSubsystem.closeAndLiftCommand();
+      }
+
     };
     tankSubsystem.setDefaultCommand(new RunCommand(tank, tankSubsystem));
   }

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

@@ -0,0 +1,75 @@
+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;
+
+  // 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 encoder
+    rightMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
+    rightMotor.setSelectedSensorPosition(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 (rightMotor.getSelectedSensorPosition() > 0) {
+        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);
+    }
+  }
+
+  /**
+   * Closes and lifts the claw simultaneously.
+   */
+  public void closeAndLiftCommand() {
+    clawIsOpen = false;
+    clawIsLifted = true;
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // This method will be called once per scheduler run during simulation
+  }
+}