Shooter assist (#116)

* driverOI handlePre

* changed autons to old shootNow

* adjusted auton path

* re-added DriverShootVelocityIntake in driverOI
changed auton to shoot off of subwoofer for first shot

* added timeout to auto intake and removed incorrect context ownership

* resets gyro to 180 degrees of current after auton is done

* stop shooter at the end of an auton and other context stuff

* Started making a more complete set of autons

* renamed autoIntake

* defualt 4800

* Shooter 4800 Changes and added center auton

* slight path tweaks

* spotless

* updated path

* Added midfield autons

* increased moveClimbersToBottom speed

* slightly decreased the intake feeding time into shooter for auton

* lowered climber speed back down, since it doesn't actually slow down the auton

* moved prox sensor threshold into a constant
we should increase to above 200

* Added minimal changes for shooter assist.

* Added some comments.

* reverted changes

* changed intake feeding time to 1.2 seconds

* Changed logic to rely on should run instead of target speed.

* commented log

* spotless

* Merge remote-tracking branch 'origin/Better-Auton-Paths' into Shooter-Assist

---------

Co-authored-by: qntmcube <[email protected]>
Co-authored-by: Leek <[email protected]>
Co-authored-by: Debajit Ghosh <[email protected]>

src/main/java/com/team766/robot/reva/BoxOpOI.java

@@ -100,14 +100,19 @@ protected void handleOI(Context context) {
                     shoulder.rotate(ShoulderPosition.SHOOT_LOW);
                     context.releaseOwnership(shoulder);
                 } else if (gamepad.getButtonPressed(InputConstants.XBOX_X)) {
-                    // other shoot pos
+                    // amp shot
                     context.takeOwnership(shoulder);
                     shoulder.rotate(ShoulderPosition.AMP);
                     context.releaseOwnership(shoulder);
                 } else if (gamepad.getButtonPressed(InputConstants.XBOX_Y)) {
-                    // amp shot
+                    // shooter assist
                     context.takeOwnership(shoulder);
-                    shoulder.rotate(ShoulderPosition.TOP);
+                    shoulder.rotate(ShoulderPosition.SHOOTER_ASSIST);
+                    // Currently it will only modify the speed if the right trigger is already held.
+                    // TODO: Make this more tolerant for when Y is pressed before right trigger.
+                    if (shooter.getShouldRun()) {
+                        shooter.shoot(Shooter.SHOOTER_ASSIST_SPEED);
+                    }
                     context.releaseOwnership(shoulder);
                 } else if (gamepad.getPOV() == 0) {
                     context.takeOwnership(shoulder);

src/main/java/com/team766/robot/reva/mechanisms/Shooter.java

@@ -11,8 +11,9 @@
 import com.team766.library.RateLimiter;
 
 public class Shooter extends Mechanism {
-    private static final double DEFAULT_SPEED =
+    public static final double DEFAULT_SPEED =
             4800.0; // motor shaft rps, does not take gearing into account
+    public static final double SHOOTER_ASSIST_SPEED = 4000.0;
     private static final double NUDGE_INCREMENT = 100.0;
     private static final double CURRENT_LIMIT = 40.0; // needs tuning
     private static final double MAX_SPEED = 5600.0; // spec is 6000.0
@@ -57,6 +58,10 @@ private double getShooterSpeedBottom() {
         return shooterMotorBottom.getSensorVelocity();
     }
 
+    public boolean getShouldRun() {
+        return shouldRun;
+    }
+
     public void shoot(double speed) {
         targetSpeed = com.team766.math.Math.clamp(speed, MIN_SPEED, MAX_SPEED);
         shoot();

src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java

@@ -20,6 +20,7 @@ public enum ShoulderPosition {
         BOTTOM(0),
         INTAKE_FLOOR(0),
         SHOOT_LOW(15),
+        SHOOTER_ASSIST(18.339),
         SHOOT_MEDIUM(30),
         SHOOT_HIGH(80),
         AMP(90),