commit cones

src/main/java/frc4388/robot/Robot.java

@@ -68,6 +68,8 @@ public class Robot extends TimedRobot {
   @Override
   public void disabledInit() {
     m_robotTime.endMatchTime();
+
+    // SmartDashboard.putBoolean("Cones?", true);
   }
 
   @Override

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

@@ -14,6 +14,7 @@ import com.revrobotics.CANSparkMax;
 import com.revrobotics.REVLibError;
 import com.revrobotics.CANSparkMax.FaultID;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -80,6 +81,9 @@ public class RobotContainer {
 
     private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw);
 
+    private Command toggleClawCones = new InstantCommand(() -> m_robotClaw.toggleCones(), m_robotClaw);
+    private Command toggleClawCubes = new InstantCommand(() -> m_robotClaw.toggleCubes(), m_robotClaw);
+
     public boolean readyForPlacement = true;
     private Boolean isPole = null;
 
@@ -91,17 +95,7 @@ public class RobotContainer {
             new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04),
             new RotateToAngle(m_robotSwerveDrive, 0.0),
             new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
-            // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape())
         ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true));
-
-    // private SequentialCommandGroup alignToShelf = 
-    //     new SequentialCommandGroup(
-    //         new RotateToAngle(m_robotSwerveDrive, 0.0),
-    //         new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
-    //         new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
-    //         new RotateToAngle(m_robotSwerveDrive, 0.0),
-    //         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
-    //     ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
     
     private SequentialCommandGroup alignToShelf = 
         new SequentialCommandGroup(
@@ -155,7 +149,7 @@ public class RobotContainer {
     );
 
     private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(
-        new PivotCommand(m_robotArm, 188),
+        new PivotCommand(m_robotArm, 185),
         new WaitCommand(0.3),
         new TeleCommand(m_robotArm, 29500)
         // toggleClaw.asProxy(),
@@ -172,14 +166,6 @@ public class RobotContainer {
     /* Autos */
     public SendableChooser<Command> chooser = new SendableChooser<>();
 
-    // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
-    
-    // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt");
-    // private Command blue1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt").andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
-    
-    // private Command red1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1);
-    // private Command red1PathWithBalance = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt", -1).andThen(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
-
     // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
 
     private Command wait3 = new WaitCommand(3);
@@ -230,28 +216,15 @@ public class RobotContainer {
         chooser.addOption("placeCubeMid", placeCubeMid);
         chooser.addOption("placeLow", placeLow);
 
-        // chooser.addOption("Blue1Path", blue1Path);
-        // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
-
-        // chooser.addOption("Red1Path", red1Path);
-        // chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
-
         playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
             .addOption("PlaceRed1Balance", placeRed1Balance)
             .addOption("Wait3", wait3.asProxy())
             .addOption("Wait5", wait5.asProxy())
             .addOption("TaxiFar", taxiFar.asProxy())
-            // .addOption("Red1Balance", new JoystickPlayback(m_robotSwerveDrive, "idk", 1))
             .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
             .buildDisplay();
     }
 
-    // here be dragons - enter if you dare
-    private static class TapState {
-        public boolean gearTapped = true;
-        public long    gearTime   = 0;
-    }
-
     /**
      * Use this method to define your button->command mappings. Buttons can be
      * created by instantiating a {@link GenericHID} or one of its subclasses
@@ -261,38 +234,17 @@ public class RobotContainer {
     private void configureButtonBindings() {
 
         // * TEST BUTTONS
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
-
 
         // * Driver Buttons
         new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
         
-        // because closure reasons - ask Daniel Thomas
-        // final TapState tap = new TapState();
         new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
             .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.setToTurbo()))
             .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
-            // .onTrue(new InstantCommand(() -> {
-            //     tap.gearTapped = true;
-            //     tap.gearTime   = System.currentTimeMillis();
-            // }))
-            // .whileTrue(new RunCommand(() -> {
-            //     if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) {
-            //         m_robotSwerveDrive.setToTurbo();
-            //         tap.gearTapped = false;
-            //     }
-            // }))
-            // .onFalse(new InstantCommand(() -> {
-            //     if (tap.gearTapped)
-            //         m_robotSwerveDrive.setToFast();
-            //     else
-            //         m_robotSwerveDrive.setToSlow();
-            // }));
         
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
+        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
-        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
             .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final
@@ -300,18 +252,18 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
             .onTrue(interruptCommand.asProxy()); // final
 
-        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_TRIGGER_AXIS)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_TRIGGER_AXIS)
-            .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
+        //     .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
-                                            () -> getDeadbandedDriverController().getLeftX(),
+        //                                     () -> getDeadbandedDriverController().getLeftX(),
-                                            () -> getDeadbandedDriverController().getLeftY(),
+        //                                     () -> getDeadbandedDriverController().getLeftY(),
-                                            () -> getDeadbandedDriverController().getRightX(),
+        //                                     () -> getDeadbandedDriverController().getRightX(),
-                                            () -> getDeadbandedDriverController().getRightY(),
+        //                                     () -> getDeadbandedDriverController().getRightY(),
-                                            "Red1Balance.txt"))
+        //                                     "Red1Balance.txt"))
-            .onFalse(new InstantCommand());
+        //     .onFalse(new InstantCommand());
 
-        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
-            .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Red1Balance.txt"))
+        //     .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Red1Balance.txt"))
-            .onFalse(new InstantCommand()); 
+        //     .onFalse(new InstantCommand()); 
 
         // * Operator Buttons
         
@@ -328,8 +280,9 @@ public class RobotContainer {
         // toggle claw
         new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
             .onTrue(toggleClaw.asProxy());
+
         // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
-        //     .onTrue(new InstantCommand(() -> m_robotClaw.setAngle(45), m_robotClaw));
+        //     .onTrue(new ConditionalCommand(toggleClawCones.asProxy(), toggleClawCubes.asProxy(), () -> SmartDashboard.getBoolean("Cones?", true)));
         
         // kill soft limits
         new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final

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

@@ -51,19 +51,28 @@ public class Claw extends SubsystemBase {
 		// ! THIS IS FOR CUBE
 		// m_leftMotor.setAngle(m_open ? 90 : 180);
 		// m_rightMotor.setAngle(m_open ? 90 : 0);
-
-		// if (m_open)
-		// 	m_spinnyspin.set(0.2);
-		// else
-		// 	m_spinnyspin.set(-0.2);	
-		// new Timer().schedule(new TimerTask() {
-		// 	@Override
-		// 	public void run() {
-		// 		nospinnyspin();
-		// 	}
-		// }, 750);
     }
 
+	public void setClawCones(boolean open) {
+		m_open = open;
+		m_leftMotor.setAngle(m_open ? 0 : 180);
+		m_rightMotor.setAngle(m_open ? 180 : 0);
+	}
+
+	public void setClawCubes(boolean open) {
+		m_open = open;
+		m_leftMotor.setAngle(m_open ? 90 : 180);
+		m_rightMotor.setAngle(m_open ? 90 : 0);
+	}
+
+	public void toggleCones() {
+		setClawCones(!m_open);
+	}
+
+	public void toggleCubes() {
+		setClawCubes(!m_open);
+	}
+
 	public void toggle() {
 		setClaw(!m_open);
 	}