Bindings

src/main/java/org/sciborgs1155/robot/Robot.java

@@ -4,10 +4,16 @@
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Seconds;
-import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.*;
-import static org.sciborgs1155.robot.Constants.*;
-import static org.sciborgs1155.robot.Constants.Field.*;
-import static org.sciborgs1155.robot.drive.DriveConstants.*;
+import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.autonomous;
+import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.test;
+import static org.sciborgs1155.robot.Constants.DEADBAND;
+import static org.sciborgs1155.robot.Constants.PERIOD;
+import static org.sciborgs1155.robot.arm.ArmConstants.INTAKE_ANGLE;
+import static org.sciborgs1155.robot.arm.ArmConstants.PROCESSOR_OUTTAKE_ANGLE;
+import static org.sciborgs1155.robot.arm.ArmConstants.TROUGH_OUTTAKE_ANGLE;
+import static org.sciborgs1155.robot.drive.DriveConstants.MAX_ANGULAR_ACCEL;
+import static org.sciborgs1155.robot.drive.DriveConstants.MAX_SPEED;
+import static org.sciborgs1155.robot.drive.DriveConstants.TELEOP_ANGULAR_SPEED;
 
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -29,10 +35,14 @@
 import org.sciborgs1155.lib.FaultLogger;
 import org.sciborgs1155.lib.InputStream;
 import org.sciborgs1155.lib.Test;
+import org.sciborgs1155.robot.Constants.Field.Level;
 import org.sciborgs1155.robot.Ports.OI;
+import org.sciborgs1155.robot.arm.Arm;
 import org.sciborgs1155.robot.commands.Autos;
+import org.sciborgs1155.robot.coroller.Coroller;
 import org.sciborgs1155.robot.drive.Drive;
 import org.sciborgs1155.robot.elevator.Elevator;
+import org.sciborgs1155.robot.hopper.Hopper;
 import org.sciborgs1155.robot.led.LEDStrip;
 import org.sciborgs1155.robot.scoral.Scoral;
 import org.sciborgs1155.robot.vision.Vision;
@@ -56,6 +66,9 @@ public class Robot extends CommandRobot implements Logged {
   private final LEDStrip led = new LEDStrip();
   private final Elevator elevator = Elevator.create();
   private final Scoral scoral = Scoral.create();
+  private final Hopper hopper = Hopper.create();
+  private final Arm arm = Arm.create();
+  private final Coroller coroller = Coroller.create();
 
   // COMMANDS
   @Log.NT private final SendableChooser<Command> autos = Autos.configureAutos(drive);
@@ -100,10 +113,39 @@ private void configureGameBehavior() {
 
   /** Configures trigger -> command bindings. */
   private void configureBindings() {
-    operator.a().onTrue(elevator.scoreLevel(Level.L1));
-    operator.b().onTrue(elevator.scoreLevel(Level.L2));
-    operator.x().onTrue(elevator.scoreLevel(Level.L3));
-    operator.y().onTrue(elevator.scoreLevel(Level.L4));
+    operator
+        .povDown()
+        .onTrue(
+            elevator
+                .scoreLevel(Level.L1)
+                .until(() -> elevator.atGoal())
+                .andThen(scoral.outtake().until(() -> scoral.beambreak())));
+    operator
+        .povLeft()
+        .onTrue(
+            elevator
+                .scoreLevel(Level.L2)
+                .until(() -> elevator.atGoal())
+                .andThen(scoral.outtake().until(() -> scoral.beambreak())));
+    operator
+        .povRight()
+        .onTrue(
+            elevator
+                .scoreLevel(Level.L3)
+                .until(() -> elevator.atGoal())
+                .andThen(scoral.outtake().until(() -> scoral.beambreak())));
+    operator
+        .povUp()
+        .onTrue(
+            elevator
+                .scoreLevel(Level.L4)
+                .until(() -> elevator.atGoal())
+                .andThen(scoral.outtake().until(() -> scoral.beambreak())));
+    operator.leftBumper().onTrue(arm.climbSetup());
+    operator.leftTrigger().onTrue(arm.climbExecute());
+    operator.rightBumper().onTrue(arm.goTo(TROUGH_OUTTAKE_ANGLE).alongWith(coroller.outtake()));
+    operator.rightTrigger().onTrue(arm.goTo(INTAKE_ANGLE).alongWith(coroller.intake()));
+    operator.a().onTrue(arm.goTo(PROCESSOR_OUTTAKE_ANGLE).alongWith(coroller.outtake()));
 
     // x and y are switched: we use joystick Y axis to control field x motion
     InputStream x = InputStream.of(driver::getLeftY).negate();
@@ -152,7 +194,7 @@ private void configureBindings() {
         .onTrue(Commands.runOnce(() -> speedMultiplier = Constants.SLOW_SPEED_MULTIPLIER))
         .onFalse(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED_MULTIPLIER));
 
-    // TODO: Add any additional bindings.
+    driver.leftTrigger().onTrue(hopper.intake().alongWith(scoral.intake()));
   }
 
   /**
@@ -189,6 +231,12 @@ public void close() {
     super.close();
     try {
       drive.close();
+      coroller.close();
+      led.close();
+      elevator.close();
+      scoral.close();
+      hopper.close();
+      arm.close();
     } catch (Exception e) {
     }
   }

src/test/java/org/sciborgs1155/robot/RobotTest.java

@@ -1,12 +1,14 @@
 package org.sciborgs1155.robot;
 
 import static org.sciborgs1155.lib.UnitTestingUtil.reset;
+import static org.sciborgs1155.lib.UnitTestingUtil.setupTests;
 
 import org.junit.jupiter.api.Test;
 
 public class RobotTest {
   @Test
   void setup() throws Exception {
+    setupTests();
     new Robot().close();
     reset();
   }