diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e464598..05484f3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -479,7 +479,7 @@ public class IntakeFlywheelConstants { public static final Angle MIN_ANGLE = Rotations.of(0.0); public static final Angle MAX_ANGLE = Rotations.of(1); public static final Angle STARTING_ANGLE = Rotations.of(0.0); - public static final double PICKUP_SPEED = 0.0; + public static final double PICKUP_SPEED = 30.0; public static final Distance WHEEL_RADIUS = Meters.of(0.05); public static final Translation3d OFFSET = Translation3d.kZero; public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.0028125); @@ -615,7 +615,7 @@ public static VisionSystemSim getSystemSim() { public class IntakePivotConstants { public static final String NAME = "Intake"; - public static final Angle PICKUP_ANGLE = Degrees.of(130.0); + public static final Angle PICKUP_ANGLE = Degrees.of(123.0); public static final Angle STOW_ANGLE = Degrees.of(0.0); public static final Angle TOLERANCE = Degrees.of(1.0); @@ -628,7 +628,7 @@ public class IntakePivotConstants { private static final double SENSOR_TO_MECHANISM = 17.35; public static final Angle MIN_ANGLE = Degrees.of(0.0); - public static final Angle MAX_ANGLE = Degrees.of(130.0); + public static final Angle MAX_ANGLE = Degrees.of(126.0); public static final Angle STARTING_ANGLE = Degrees.of(0.0); public static final Distance ARM_LENGTH = Foot.one(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f031ec6..b9c96a1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.BaseStatusSignal; import com.pathplanner.lib.auto.AutoBuilder; @@ -73,7 +72,7 @@ public class RobotContainer { // Subsystems public final Drive drive; - private final Hopper hopper; + public final Hopper hopper; private final Shooter shooter; public final Intake intake; // private final BallCounter ballCounter; @@ -366,28 +365,36 @@ private void configureButtonBindings() { controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(100))); controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); - // Intake + Spindexer + Tower + // Intake Rollers 11 Motor: 9 Intake + + // ALPHA KATIE REQUESTS INTAKE RIGHT TRIGGER, SHOOT LEFT TRIGGER, AUTO ALIGN "A" + + // Intake + controller.rightTrigger().whileTrue(intake.intake()); + controller.rightTrigger().onFalse(intake.stowAndStopRollers()); + + // Shoot controller - .rightBumper() + .leftTrigger() .whileTrue( Commands.parallel( - // intake.runRollers(RotationsPerSecond.of(30)), - hopper.runSpindexer(15), shooter.runTower(RotationsPerSecond.of(30)))); + shooter.prepareToShoot(RotationsPerSecond.of(100), RotationsPerSecond.of(30)), + hopper.runSpindexer(RotationsPerSecond.of(15)))); controller - .rightBumper() + .leftTrigger() .onFalse( Commands.parallel( - intake.runRollers(RotationsPerSecond.of(0)), - hopper.runSpindexer(0), - shooter.runTower(RotationsPerSecond.of(0)))); + shooter.prepareToShoot(RotationsPerSecond.of(0), RotationsPerSecond.of(0)), + hopper.runSpindexer(RotationsPerSecond.of(0)))); - // Intake Rollers 11 Motor: 9 Intake + // Align + // controller.a().onTrue(); - controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(22.5)))); - controller - .a() - .onFalse(new RunCommand(() -> intake._rollerIO.runVoltage(Volts.of(0.0)), intake)); + // controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(22.5)))); + // controller + // .a() + // .onFalse(new RunCommand(() -> intake._rollerIO.runVoltage(Volts.of(0.0)), intake)); // Spindexer 1:1 // controller.x().whileTrue(hopper.runSpindexer(18)); @@ -399,7 +406,7 @@ private void configureButtonBindings() { // controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); // controller.b().onFalse(shooter.setHoodAngle(ShooterRotaryConstants.STARTING_ANGLE.magnitude())); - controller.povLeft().onTrue(shooter.calibrateHood()); + // controller.povLeft().onTrue(shooter.calibrateHood()); // controller.povLeft().onTrue(shooter.setHoodAngle(10)); // controller.povDown().onTrue(shooter.setHoodAngle(15)); // controller.povRight().onTrue(shooter.setHoodAngle(20)); diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index 46c4b82..aaf8913 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -30,16 +30,13 @@ public void setGoal(double position) { PIDSlot.SLOT_0); } - // Velocity of Rollers - public void setVelocity(double velocity) { - AngularVelocity angVelo = RotationsPerSecond.of(velocity); - - _io.runVelocity(angVelo, HopperConstants.ACCELERATION, PIDSlot.SLOT_0); - targetVelocity = angVelo; - } - - public Command runSpindexer(double velocity) { - return Commands.runOnce(() -> setVelocity(velocity), this); + public Command runSpindexer(AngularVelocity velocity) { + return Commands.runOnce( + () -> { + _io.runVelocity(velocity, HopperConstants.ACCELERATION, PIDSlot.SLOT_0); + targetVelocity = velocity; + }, + this); } @Override diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0ff58c8..5c60f22 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -76,13 +76,12 @@ public void stop() { } public Command runRollers(AngularVelocity velocity) { - return Commands.run(() -> setVelocity(velocity), this); + return Commands.run(() -> setVelocity(velocity)); } public Command intake() { - return Commands.sequence( - Commands.run( - () -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), + return Commands.parallel( + runRollers(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED)), setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); } @@ -98,17 +97,14 @@ public boolean canIntake() { } public Command stowAndStopRollers() { - return Commands.sequence( - Commands.run( - () -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), - setStowAngle(IntakePivotConstants.STOW_ANGLE)); + return Commands.parallel(runRollers(RotationsPerSecond.of(0.0)), setStowAngle()); } - private Command setStowAngle(Angle stowAngle) { + private Command setStowAngle() { return this.runOnce( () -> _pivotIO.runPosition( - stowAngle, + IntakePivotConstants.STOW_ANGLE, IntakePivotConstants.CRUISE_VELOCITY, IntakePivotConstants.ACCELERATION, IntakePivotConstants.JERK, @@ -132,7 +128,7 @@ public void periodic() { Logger.recordOutput("Intake/TargetSpeed", targetSpeed); _pivotIO.periodic(); - // Logger.recordOutput("Intake/TargetPivot", null); + Logger.recordOutput("Intake/TargetPivot", _pivotIO.getPosition().in(Degrees)); Logger.recordOutput( "3DField/1_Intake", new Pose3d( diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index c7ca63c..4cb36ee 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -181,17 +181,12 @@ public Command calibrateHood() { // _hood.setEncoderPosition(Angle.ofBaseUnits(0, Degrees)))); // } - public Command shoot(AngularVelocity velocity) { + public Command prepareToShoot(AngularVelocity flywheelVelocity, AngularVelocity towerVelocity) { // Prepare targets - return Commands.sequence( - // Set and wait in parallel for both hood and flywheel - Commands.parallel( - Commands.run(() -> setFlywheelVelocity(velocity)).until(this::flyAtVelocity), - Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle)), - // feed once ready - Commands.runOnce(() -> runFeeder(FeederConstants.FEED_SPEED)), - // stop flywheel when finished - Commands.runOnce(() -> setFlywheelVelocity(RotationsPerSecond.of(0.0)))); + return Commands.parallel( + Commands.run(() -> setFlywheelVelocity(flywheelVelocity)).until(this::flyAtVelocity), + Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle), + runTower(towerVelocity)); } public Command runFlywheel(AngularVelocity velocity) {