diff --git a/src/main/java/com/team766/robot/reva/BoxOpOI.java b/src/main/java/com/team766/robot/reva/BoxOpOI.java index a5e396c7c..9f0a54e16 100644 --- a/src/main/java/com/team766/robot/reva/BoxOpOI.java +++ b/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); diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java b/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java index 62ba0d4b4..953bb5b16 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java +++ b/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(); diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java index b936558ae..536781ffa 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java +++ b/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),