diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index afb50f94..9fdbd3b8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import frc.robot.commands.Autos; import frc.robot.commands.CollectNote; import frc.robot.commands.Drive; +import frc.robot.commands.DriveTimeCommand; import frc.robot.commands.ExampleCommand; import frc.robot.settings.Constants.Field; @@ -65,6 +66,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -234,7 +236,12 @@ private void configureBindings() { () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL), driverController::getL2Button)); - new Trigger(driverController::getR1Button).onTrue(new CollectNote(driveTrain, limelight)); + new Trigger(driverController::getR1Button).onTrue(new SequentialCommandGroup( + new CollectNote(driveTrain, limelight), + new DriveTimeCommand(-2, 0, 0, 0.5, driveTrain) + )); + new Trigger(driverController::getTouchpadPressed).onTrue(new InstantCommand(driveTrain::stop, driveTrain)); + if(shooterExists) { AngleShooter shooterUpCommand = new AngleShooter(angleShooterSubsystem, () -> Constants.ShooterConstants.shooterup); new Trigger(()->driverController.getPOV() == 180).whileTrue(shooterUpCommand); diff --git a/src/main/java/frc/robot/commands/CollectNote.java b/src/main/java/frc/robot/commands/CollectNote.java index cb0949dd..61df94e8 100644 --- a/src/main/java/frc/robot/commands/CollectNote.java +++ b/src/main/java/frc/robot/commands/CollectNote.java @@ -9,6 +9,9 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.settings.Constants.DriveConstants; import frc.robot.settings.Constants.Vision; import frc.robot.settings.LimelightDetectorData; @@ -55,13 +58,14 @@ public void initialize() { tyController.setSetpoint(0); txController.setTolerance(3.5, 0.25); tyController.setTolerance(1, 0.25); - + detectorData = Limelight.latestDetectorValues; + SmartDashboard.putBoolean("note seen", detectorData.isResultValid); } - + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - detectorData = Limelight.latestDetectorValues; + detectorData = Limelight.latestDetectorValues; if (detectorData == null) { drivetrain.stop(); @@ -86,7 +90,7 @@ public void execute() { // drives the robot forward faster if the object is higher up on the screen, and turns it more based on how far away the object is from x=0 drivetrain.drive(new ChassisSpeeds( - tyLimiter.calculate(-tyController.calculate(ty)), + tyLimiter.calculate(tyController.calculate(ty)), 0, txController.calculate(tx))); } @@ -96,7 +100,6 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - drivetrain.stop(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/DriveTimeCommand.java b/src/main/java/frc/robot/commands/DriveTimeCommand.java new file mode 100644 index 00000000..9478a59e --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveTimeCommand.java @@ -0,0 +1,61 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.DrivetrainSubsystem; + +public class DriveTimeCommand extends Command { + double forwardSpeed; + double sidewaysSpeed; + double radiansPerSecond; + double time; + Timer timer; + DrivetrainSubsystem drivetrain; + /** Creates a new DriveTimeCommand. */ + public DriveTimeCommand(double forwardSpeed, double sidewaysSpeed, double radiansPerSecond, double time, DrivetrainSubsystem drivetrain) { + // Use addRequirements() here to declare subsystem dependencies. + this.forwardSpeed = forwardSpeed; + this.sidewaysSpeed = sidewaysSpeed; + this.radiansPerSecond = radiansPerSecond; + this.time = time; + this.drivetrain = drivetrain; + addRequirements(drivetrain); + timer = new Timer(); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + timer.start(); + timer.reset(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + drivetrain.drive(new ChassisSpeeds( + forwardSpeed, + sidewaysSpeed, + radiansPerSecond + )); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + timer.reset(); + timer.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (timer.get()>=time)||!SmartDashboard.getBoolean("note seen", true); + } +}