claw pickup maybe done

src/main/java/frc4388/robot/RobotContainer.java

@@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -60,6 +61,7 @@ public class RobotContainer {
     public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
 
     public final Claw m_robotClaw = new Claw(m_robotMap.leftClaw, m_robotMap.rightClaw, m_robotMap.spinnyspin);
+    private boolean isClawOpen = false;
 
     public final Limelight m_robotLimeLight = new Limelight();
 
@@ -89,6 +91,18 @@ public class RobotContainer {
     private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135));
 
     private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw);
+        
+    private ConditionalCommand toggleClawCube = new ConditionalCommand(
+            new ParallelCommandGroup(new InstantCommand(() -> m_robotClaw.pickupCube()), new RunCommand(() -> m_robotClaw.intake())),
+            new ParallelCommandGroup(new InstantCommand(() -> m_robotClaw.openClaw()), new RunCommand(() -> m_robotClaw.outtake())),
+            () -> isClawOpen
+        );
+
+    private ConditionalCommand toggleClawCone = new ConditionalCommand(
+            new ParallelCommandGroup(new InstantCommand(() -> m_robotClaw.pickupCone()), new RunCommand(() -> m_robotClaw.intake())),
+            new ParallelCommandGroup(new InstantCommand(() -> m_robotClaw.openClaw()), new RunCommand(() -> m_robotClaw.outtake())),
+            () -> isClawOpen
+        );
 
     public boolean readyForPlacement = true;
     private Boolean isPole = null;
@@ -299,26 +313,38 @@ public class RobotContainer {
             .onFalse(interruptCommand.asProxy());
 
         // toggle claw
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
+        //     .onTrue(toggleClaw.asProxy());
+
+        // toggle claw cube
         new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
-            .onTrue(toggleClaw.asProxy());
+            .whileTrue  (toggleClawCube.asProxy())
+            .onFalse    (new InstantCommand(() -> m_robotClaw.nospinnyspin()))
+            .onFalse    (new InstantCommand(() -> isClawOpen = !isClawOpen));
+
+        // toggle claw cone
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final
+            .onTrue     (toggleClawCone.asProxy())
+            .onFalse    (new InstantCommand(() -> m_robotClaw.nospinnyspin()))
+            .onFalse    (new InstantCommand(() -> isClawOpen = !isClawOpen));
         
         // kill soft limits
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final
             .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
 
         // outtake
         new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_TRIGGER_AXIS) // final
-            .onTrue  (new InstantCommand(() -> m_robotClaw.reversespinnyspin()))
+            .onTrue  (new InstantCommand(() -> m_robotClaw.intake()))
             .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
         
         // intake    
         new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_TRIGGER_AXIS) // final
-            .onTrue  (new InstantCommand(() -> m_robotClaw.yesspinnyspin()))
+            .onTrue  (new InstantCommand(() -> m_robotClaw.outtake()))
             .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
         
-        // arm to Home
+        // arm to home
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final
+        // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final
-            .onTrue(armToHome.asProxy());
+        //     .onTrue(armToHome.asProxy());
 
         // interupt
         new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final

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

@@ -35,20 +35,22 @@ public class Claw extends SubsystemBase {
 		setClaw(false);
 	}
 
+	public void openClaw() {
+		m_leftMotor.setAngle(0);
+		m_rightMotor.setAngle(180);
+	}
+
+	
+
     public void setClaw(boolean open) {
         // Open claw
 		m_open = open;
 		// m_leftMotor.setRaw(m_open ? 1000 : 2000);
 		// m_rightMotor.setRaw(m_open ? 2000 : 1000);
 
-		// m_leftMotor.setBounds(4000, 20000, 2000, 0, 0);
-
 		m_leftMotor.setAngle(m_open ? 0 : 180);
 		m_rightMotor.setAngle(m_open ? 180 : 0);
 
-		// m_leftMotor.setRaw(m_open ? 0 : 4000);
-		// m_rightMotor.setRaw(m_open ? 4000 : 0);
-
 		// m_leftMotor.setRaw(m_open ? 1500 : 2000);
 		// m_rightMotor.setRaw(m_open ? 1500 : 1000);
 
@@ -72,7 +74,17 @@ public class Claw extends SubsystemBase {
 		return m_open;
 	}
 
-	public void yesspinnyspin() {
+	public void pickupCube() { // ! left angle + right angle = 180 
+		m_leftMotor.setAngle(0);
+		m_rightMotor.setAngle(0);
+	}
+
+	public void pickupCone() { // ! left angle + right angle = 180 
+		m_leftMotor.setAngle(0);
+		m_rightMotor.setAngle(0);
+	}
+
+	public void outtake() {
 		m_spinnyspin.set(0.2);
 	}
 
@@ -80,7 +92,7 @@ public class Claw extends SubsystemBase {
 		m_spinnyspin.set(0);
 	}
 
-	public void reversespinnyspin() {
+	public void intake() {
 		m_spinnyspin.set(-0.2);
 	}