diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index eca423a..9cc0c4d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -372,23 +372,19 @@ private void configureButtonBindings() { // intake.simBalls = 0; // })); - // Shoot - controller.leftTrigger().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(67))); - controller.leftTrigger().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); - - // Feed + // Feed and Shoot controller .leftBumper() .whileTrue( Commands.parallel( - shooter.runTower(RotationsPerSecond.of(40)), - hopper.runSpindexer(RotationsPerSecond.of(20)), + shooter.score(), + hopper.runSpindexer(RotationsPerSecond.of(15)), intake.intake())); controller .leftBumper() .onFalse( Commands.parallel( - shooter.runTower(RotationsPerSecond.of(0)), + shooter.stopScore(), hopper.runSpindexer(RotationsPerSecond.of(0)), Commands.run(() -> intake.stop()))); @@ -415,12 +411,12 @@ private void configureButtonBindings() { // controller.povDown().whileTrue(climber.lowerClimber()); // controller.povDown().onFalse(climber.stopClimber()); - controller.povLeft().onTrue(intake.zeroEncoder()); + // controller.povLeft().onTrue(intake.zeroEncoder()); // Testing Commands - // controller.povLeft().onTrue(shooter.setHoodAngle(6.8)); - // controller.povDown().onTrue(shooter.setHoodAngle(12.1)); - // controller.povRight().onTrue(shooter.setHoodAngle(18.6)); + controller.povUp().onTrue(shooter.setHoodAngle(2.3)); + controller.povRight().onTrue(shooter.setHoodAngle(8)); + controller.povDown().onTrue(shooter.setHoodAngle(9.8)); // controller.povLeft().onTrue(shooter.setAngleForDistance(2.0)); } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 845e8c3..8108fc8 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -57,7 +57,7 @@ public class TunerConstants { // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - // The stator current at which the wheels start to slip; + // The stator current at which the wheels start to slip // This needs to be tuned to your individual robot private static final Current kSlipCurrent = Amps.of(120); diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 78d0aa3..31926ec 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -4,6 +4,7 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; @@ -186,6 +187,14 @@ public Command runTower(AngularVelocity velocity) { return Commands.run(() -> runFeeder(velocity), this); } + public Command score() { + return Commands.run(() -> {runFeeder(RotationsPerSecond.of(30)); setFlywheelVelocity(RotationsPerSecond.of(67));}); + } + + public Command stopScore() { + return Commands.run(() -> {runFeeder(RotationsPerSecond.of(0)); setFlywheelVelocity(RotationsPerSecond.of(0));}); + } + public void simShoot() { if (Robot.robotContainer.intake.simBalls <= 0) return;