Implement commands for elevator levels

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

@@ -38,4 +38,8 @@ public static class Elevator {
     public static final int CANcoderId = 19;
     public static final int supplyCurrentLimit = 10;
   }
+
+  public static class Claw {
+    public static final int motorId = 12;
+  }
 }

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

@@ -4,20 +4,19 @@
 
 package frc.robot;
 
-import frc.robot.Constants.OperatorConstants;
-import frc.robot.commands.Autos;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.subsystems.ExampleSubsystem;
-import frc.robot.subsystems.Wrist;
-import frc.robot.subsystems.Pivot;
-import frc.robot.subsystems.Elevator;
-
 import static edu.wpi.first.units.Units.Degrees;
-
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Constants.OperatorConstants;
+import frc.robot.commands.Autos;
+import frc.robot.commands.ExampleCommand;
+import frc.robot.subsystems.Claw;
+import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.Pivot;
+import frc.robot.subsystems.Wrist;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -38,6 +37,7 @@ public class RobotContainer {
   private final Wrist wrist = new Wrist();
   private final Pivot pivot = new Pivot();
   private final Elevator elevator = new Elevator();
+  private final Claw claw = new Claw();
 
   boolean isIntaking = false;
 
@@ -82,16 +82,18 @@ private void configureBindings() {
     //   wrist.setWristPosition(0.0); // Set to 0 degrees from the origin
     // }));
 
-    joystick.a().whileTrue(Commands.runOnce(RobotContainer.this::goToIntake));
-
-    joystick.a().whileFalse(Commands.runOnce(RobotContainer.this::goToHold));
+    // use left bumper for intake and hold
+    joystick.leftBumper().onTrue(Commands.runOnce(RobotContainer.this::goToIntake));
 
-    joystick.a().onTrue(Commands.runOnce(() -> {
-      isIntaking = true;
-    }));
-
-
+    joystick.leftBumper().onFalse(Commands.runOnce(RobotContainer.this::goToHold));
 
+    joystick.rightBumper().onFalse(Commands.runOnce(() -> claw.outtake()));
+
+    // use a, b, x and y for l1, l2, l3 and l4 scoring respectively 
+    joystick.a().onTrue(Commands.runOnce(RobotContainer.this::moveL1));
+    joystick.b().onTrue(Commands.runOnce(RobotContainer.this::moveL2));
+    joystick.x().onTrue(Commands.runOnce(RobotContainer.this::moveL3));
+    joystick.y().onTrue(Commands.runOnce(RobotContainer.this::moveL4));
   }
 
   /**
@@ -105,18 +107,41 @@ public Command getAutonomousCommand() {
   }
 
   public void goToIntake() {
-    if (isIntaking) {
-      wrist.setWristPosition(0);
-      pivot.setPivotPosition(Degrees.of(0));
-      elevator.setElevatorPosition(0.2);
-    }
+    wrist.setWristPosition(0);
+    pivot.setPivotPosition(Degrees.of(0));
+    elevator.setElevatorPosition(0.2);
+    claw.intake();
   }
 
   public void goToHold() {
-    if (isIntaking) {
-      wrist.setWristPosition(0);
-      pivot.setPivotPosition(Degrees.of(75));
-      elevator.setElevatorPosition(0.2);
-    }
+    wrist.setWristPosition(0);
+    pivot.setPivotPosition(Degrees.of(75));
+    elevator.setElevatorPosition(0.2);
+    claw.stop();
+  }
+  public void moveL1() {
+    wrist.setWristPosition(0.5);
+    pivot.setPivotPosition(75);
+    elevator.setElevatorPosition(0.4);
+
+  }
+  public void moveL2() {
+    wrist.setWristPosition(0.5);
+    pivot.setPivotPosition(75);
+    elevator.setElevatorPosition(0.6);
+  }
+
+  public void moveL3() {
+    wrist.setWristPosition(0.5);
+    pivot.setPivotPosition(75);
+    elevator.setElevatorPosition(0.8);
+
+  }
+
+  public void moveL4() {
+    wrist.setWristPosition(0.5);
+    pivot.setPivotPosition(75);
+    elevator.setElevatorPosition(0.9);
+
   }
-}
+}

src/main/java/frc/robot/subsystems/Claw.java

@@ -0,0 +1,44 @@
+// 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 frc.robot.subsystems;
+
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class Claw extends SubsystemBase {
+    private final TalonFX clawMotor; // Creates claw motor
+    private final DutyCycleOut dutyCycleReq = new DutyCycleOut(0);
+
+  /** Creates a new ExampleSubsystem. */
+  public Claw() {
+    clawMotor = new TalonFX(Constants.Claw.motorId); // Initializes claw motor
+  }
+
+  /**
+   * Example command factory method.
+   *
+   * @return a command
+   */
+  public void intake() {
+    this.setPower(0.5);
+  }
+
+  public void outtake() {
+    this.setPower(-0.5);
+  }
+
+  public void stop() {
+    this.setPower(0);
+  }
+
+  public void setPower(double power) {
+    // Between -0.5 and 0.5 rotations; 1 rotation = 360 degrees
+    // 1.5 rotations = 540 degrees (up)
+    clawMotor.setControl(dutyCycleReq.withOutput(power));
+  }
+}