diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5fafcc4..5be3747 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -26,8 +27,10 @@ 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.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.constants.IntakeConstants; import frc.robot.constants.OIConstants; import frc.robot.constants.PhotonConstants; import frc.robot.constants.ShooterConstants; @@ -55,6 +58,7 @@ public class RobotContainer { private Trigger isTeleopTrigger; private Trigger isEnabledTrigger; + private Trigger indexerBeamBreakWatcher; private SendableChooser autoChooser; @@ -67,6 +71,7 @@ public class RobotContainer { private int speakerTag; private boolean setAmpAngle; + private boolean intakeDown; private boolean invertXYDrive; public RobotContainer() { @@ -111,8 +116,10 @@ public class RobotContainer { isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled); isEnabledTrigger = new Trigger(DriverStation::isEnabled); + indexerBeamBreakWatcher = new Trigger(indexer::getBeamBreak); setAmpAngle = false; + intakeDown = false; configureBindings(); shuffleboardSetup(); @@ -157,7 +164,35 @@ public class RobotContainer { ); //intake.setDefaultCommand(intake.intakeUpCommand()); - intake.setDefaultCommand(intake.manualPivot(() -> -operator.getLeftY(), operator::getRightY)); + //intake.setDefaultCommand(intake.manualPivot(() -> -operator.getLeftY(), operator::getRightY)); + intake.setDefaultCommand( + intake.intakeControl( + () -> { + if (intakeDown) { + return IntakeConstants.kDownAngle; + } else { + return IntakeConstants.kUpAngle; + } + }, + operator::getRightY + ) + ); + + operator.povUp().onTrue( + new InstantCommand( + () -> { + intakeDown = false; + } + ) + ); + + operator.povDown().onTrue( + new InstantCommand( + () -> { + intakeDown = true; + } + ) + ); shooter.setDefaultCommand( shooter.angleSpeedsSetpoints( @@ -179,6 +214,20 @@ public class RobotContainer { ); //shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis)); + driver.leftBumper().onTrue( + shooter.angleSpeedsSetpoints( + () -> ShooterConstants.kShooterLoadAngle, + () -> 1.0 + ).withTimeout(.5).andThen( + shooter.angleSpeedsSetpoints( + () -> ShooterConstants.kShooterLoadAngle, + () -> 1.0 + ).alongWith( + indexer.shootNote(() -> 1.0) + ).withTimeout(2) + ) + ); + climber.setDefaultCommand(climber.stop()); indexer.setDefaultCommand(indexer.shootNote( @@ -191,6 +240,16 @@ public class RobotContainer { } )); + indexerBeamBreakWatcher.onFalse( + new RunCommand(() -> { + driver.getHID().setRumble(RumbleType.kBothRumble, 1); + }).withTimeout(.5).andThen( + new InstantCommand(() -> { + driver.getHID().setRumble(RumbleType.kBothRumble, 0); + }) + ) + ); + /* driver.povCenter().onFalse( drivetrain.driveCardinal( @@ -284,8 +343,8 @@ public class RobotContainer { operator.start().onTrue(shooter.zeroEncoder()); - operator.povDown().whileTrue(intake.intakeDownCommand()); - operator.povUp().whileTrue(intake.intakeUpCommand()); + //operator.povDown().whileTrue(intake.intakeDownCommand()); + //operator.povUp().whileTrue(intake.intakeUpCommand()); driver.leftTrigger().whileTrue(Commands.parallel( intake.manualPivot(() -> 0.0, () -> 1.0), indexer.advanceNote())); @@ -312,7 +371,7 @@ public class RobotContainer { teleopTab.addBoolean("shooter beam break", shooter::getBeamBreak); teleopTab.addDouble("shooter angle", shooter::getShooterAngle); teleopTab.addDouble("intake angle", intake::getIntakeDegrees); - teleopTab.addDouble("intake pid", intake::getIntakePID); + //teleopTab.addDouble("intake pid", intake::getIntakePID); teleopTab.addDouble("heading swerve", drivetrain::getHeading); teleopTab.addDouble("steering encoder", drivetrain::getEncoderSteering); } diff --git a/src/main/java/frc/robot/constants/IntakeConstants.java b/src/main/java/frc/robot/constants/IntakeConstants.java index 0b0d832..658db17 100644 --- a/src/main/java/frc/robot/constants/IntakeConstants.java +++ b/src/main/java/frc/robot/constants/IntakeConstants.java @@ -8,13 +8,13 @@ public class IntakeConstants { public static final int kPivotCurrentLimit = 40; - public static final double kPIntake = 2.5; + public static final double kPIntake = 3;//4; //2.5; public static final double kIIntake = 0; - public static final double kDIntake = 0; + public static final double kDIntake = 0.01; public static final double kSFeedForward = 0; - public static final double kGFeedForward = 0;//1.11; - public static final double kVFeedForward = 0;//0.73; + public static final double kGFeedForward = 1;//1.11; + public static final double kVFeedForward = .5;//0.73; public static final double kStartingAngle = Math.toRadians(105.0); public static final double kUpAngle = Math.toRadians(90.0); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 306ac24..698a4c6 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -61,6 +61,22 @@ public class Intake extends SubsystemBase{ armOffset = getIntakeAngle()-IntakeConstants.kStartingAngle; } + public Command intakeControl(DoubleSupplier pivotAngle, DoubleSupplier intakeSpeed) { + return run(() -> { + intakeRoller.set(intakeSpeed.getAsDouble()); + + intakePivot.setVoltage( + intakeAnglePID.calculate( + getIntakeAngle(), + pivotAngle.getAsDouble() + ) + intakeFeedForward.calculate( + pivotAngle.getAsDouble(), + 0.0 + ) + ); + }); + } + public Command intakeDownCommand() { return run(() -> { intakeRoller.set(0.0);