Jiggle coral (#147)

* add and bind coral jiggle command

* testing changes

Author: nlaverdure

src/main/java/frc/robot/Robot.java

@@ -404,6 +404,9 @@ public boolean getAsBoolean() {
     operator.rightBumper()
         .whileTrue(coralRoller.intake());
 
+    // Jiggle coral
+    operator.start().whileTrue(coralRoller.jiggle().repeatedly());
+
     // Force joystick operation of the elevator
     Trigger elevatorTriggerHigh = operator.axisGreaterThan(Axis.kLeftY.value, 0.9, loop).debounce(0.1);
     Trigger elevatorTriggerLow = operator.axisLessThan(Axis.kLeftY.value, -0.9, loop).debounce(0.1);

src/main/java/frc/robot/elevator/CoralRoller.java

@@ -11,6 +11,7 @@
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.MotorConstants.NEO550Constants;
@@ -97,4 +98,9 @@ public Command outtakeToL4() {
     return this.run(() -> setVoltage(CoralRollerConstants.kOuttakeToL4Voltage))
         .withName("Outtake to L4");
   }
+
+  public Command jiggle() {
+    return Commands.sequence(outtakeToL1().withTimeout(0.05), intake().withTimeout(0.3))
+        .withName("Jiggle");
+  }
 }