Bindings

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

@@ -41,7 +41,7 @@ public static final class LEDs {
   }
 
   public static final class Hopper {
-    public static final int MOTOR = -1;
-    public static final int BEAMBREAK = -1;
+    public static final int MOTOR = 3;
+    public static final int BEAMBREAK = 4;
   }
 }

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

@@ -1,16 +1,38 @@
 package org.sciborgs1155.robot;
 
+import java.util.Set;
+
+import org.littletonrobotics.urcl.URCL;
+import org.sciborgs1155.lib.CommandRobot;
+import org.sciborgs1155.lib.FaultLogger;
+import org.sciborgs1155.lib.InputStream;
+import org.sciborgs1155.lib.Test;
+import static org.sciborgs1155.robot.Constants.DEADBAND;
+import static org.sciborgs1155.robot.Constants.PERIOD;
+import org.sciborgs1155.robot.Ports.OI;
+import org.sciborgs1155.robot.arm.Arm;
+import static org.sciborgs1155.robot.arm.ArmConstants.ALGAE_INTAKE;
+import static org.sciborgs1155.robot.arm.ArmConstants.CLIMB_INTAKE_ANGLE;
+import static org.sciborgs1155.robot.arm.ArmConstants.CORAL_INTAKE;
+import org.sciborgs1155.robot.commands.Autos;
+import org.sciborgs1155.robot.commands.Corolling;
+import org.sciborgs1155.robot.commands.Scoraling;
+import org.sciborgs1155.robot.coroller.Coroller;
+import org.sciborgs1155.robot.drive.Drive;
+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 org.sciborgs1155.robot.elevator.Elevator;
+import org.sciborgs1155.robot.elevator.ElevatorConstants.Level;
+import org.sciborgs1155.robot.hopper.Hopper;
+import org.sciborgs1155.robot.led.LEDStrip;
+import org.sciborgs1155.robot.scoral.Scoral;
+import org.sciborgs1155.robot.vision.Vision;
+
 import static edu.wpi.first.units.Units.MetersPerSecond;
 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.DEADBAND;
-import static org.sciborgs1155.robot.Constants.Field.*;
-import static org.sciborgs1155.robot.Constants.PERIOD;
-import static org.sciborgs1155.robot.drive.DriveConstants.*;
-
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
@@ -22,25 +44,11 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import java.util.Set;
+import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.autonomous;
+import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.test;
 import monologue.Annotations.Log;
 import monologue.Logged;
 import monologue.Monologue;
-import org.littletonrobotics.urcl.URCL;
-import org.sciborgs1155.lib.CommandRobot;
-import org.sciborgs1155.lib.FaultLogger;
-import org.sciborgs1155.lib.InputStream;
-import org.sciborgs1155.lib.Test;
-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.elevator.ElevatorConstants.Level;
-import org.sciborgs1155.robot.led.LEDStrip;
-import org.sciborgs1155.robot.scoral.Scoral;
-import org.sciborgs1155.robot.vision.Vision;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -63,9 +71,12 @@ 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();
 
   // COMMANDS
   @Log.NT private final SendableChooser<Command> autos = Autos.configureAutos(drive);
+  private final Scoraling scoraling = new Scoraling(hopper, scoral, elevator);
+  private final Corolling corolling = new Corolling(arm, coroller);
 
   @Log.NT private double speedMultiplier = Constants.FULL_SPEED_MULTIPLIER;
 
@@ -108,12 +119,27 @@ 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));
 
-    // x and y are switched: we use joystick Y axis to control field x motion
+    // scoral commands
+    operator.povDown().onTrue(scoraling.scoral(Level.L1));
+    operator.povRight().onTrue(scoraling.scoral(Level.L2));
+    operator.povLeft().onTrue(scoraling.scoral(Level.L3));
+    operator.povUp().onTrue(scoraling.scoral(Level.L4));
+
+    // corolling commands
+    // climb
+    operator.a().onTrue(corolling.intake(CLIMB_INTAKE_ANGLE));
+    operator.b().onTrue(arm.climbExecute());
+    // coral
+    operator.rightBumper().onTrue(corolling.trough());
+    operator.rightTrigger().onTrue(corolling.intake(CORAL_INTAKE));
+    // algae
+    operator.x().onTrue(corolling.intake(ALGAE_INTAKE));
+    operator.y().onTrue(corolling.processor());
+    operator.leftBumper().onTrue(scoraling.cleanAlgae(Level.L2));
+    operator.leftTrigger().onTrue(scoraling.cleanAlgae(Level.L3));
+
+    // x and y are switched: we use joystick y axis to control field x motion
     InputStream x = InputStream.of(driver::getLeftY).negate();
     InputStream y = InputStream.of(driver::getLeftX).negate();
 
@@ -160,8 +186,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(scoraling.hpsIntake());
   }
 
   /**
@@ -198,6 +223,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/main/java/org/sciborgs1155/robot/arm/ArmConstants.java

@@ -25,10 +25,11 @@ public class ArmConstants {
   public static final double kA = 0;
   public static final double kG = 0;
 
-  public static final Angle INTAKE_ANGLE = Radians.of(-Math.PI / 4);
   public static final Angle HP_OUTTAKE_ANGLE = Radians.of(Math.PI * 3 / 4);
   public static final Angle PROCESSOR_OUTTAKE_ANGLE = Radians.of(Math.PI * 3 / 4);
   public static final Angle TROUGH_OUTTAKE_ANGLE = Radians.of(Math.PI * 3 / 4);
+  public static final Angle CORAL_INTAKE = Radians.of(Math.PI * 1 / 4);
+  public static final Angle ALGAE_INTAKE = Radians.of(Math.PI * 3 / 8);
 
   public static final Angle STARTING_ANGLE = Radians.of(Math.PI / 2);
   public static final Angle DEFAULT_ANGLE = Radians.of(Math.PI * 5 / 8);

src/main/java/org/sciborgs1155/robot/commands/Corolling.java

@@ -1,8 +1,10 @@
 package org.sciborgs1155.robot.commands;
 
 import static edu.wpi.first.units.Units.Radians;
-import static org.sciborgs1155.robot.arm.ArmConstants.*;
+import static org.sciborgs1155.robot.arm.ArmConstants.PROCESSOR_OUTTAKE_ANGLE;
+import static org.sciborgs1155.robot.arm.ArmConstants.TROUGH_OUTTAKE_ANGLE;
 
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import org.sciborgs1155.robot.arm.Arm;
@@ -23,10 +25,10 @@ public Corolling(Arm arm, Coroller roller) {
    *
    * @return A command for ground intaking.
    */
-  public Command intake() {
-    return arm.goTo(INTAKE_ANGLE)
+  public Command intake(Angle IntakeAngle) {
+    return arm.goTo(IntakeAngle)
         .withDeadline(
-            Commands.waitUntil(() -> arm.atPosition(INTAKE_ANGLE.in(Radians)))
+            Commands.waitUntil(() -> arm.atPosition(IntakeAngle.in(Radians)))
                 .andThen(roller.intake().asProxy().withTimeout(1)))
         .withName("ground intake");
   }

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

@@ -7,8 +7,8 @@
 import static org.sciborgs1155.lib.UnitTestingUtil.reset;
 import static org.sciborgs1155.lib.UnitTestingUtil.runToCompletion;
 import static org.sciborgs1155.lib.UnitTestingUtil.setupTests;
+import static org.sciborgs1155.robot.arm.ArmConstants.CORAL_INTAKE;
 import static org.sciborgs1155.robot.arm.ArmConstants.DEFAULT_ANGLE;
-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;
 
@@ -61,10 +61,10 @@ public void processorTest() {
 
   @Test
   public void intakeTest() {
-    Command testcmd = corolling.intake();
+    Command testcmd = corolling.intake(CORAL_INTAKE);
     runToCompletion(testcmd);
     assertEquals(
-        arm.position(), INTAKE_ANGLE.in(Radians), ArmConstants.POSITION_TOLERANCE.in(Radians));
+        arm.position(), CORAL_INTAKE.in(Radians), ArmConstants.POSITION_TOLERANCE.in(Radians));
     assertTrue(testcmd::isFinished);
 
     assertTrue(arm.getDefaultCommand().equals(arm.getCurrentCommand()));

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();
   }