From 05d75591086ccb694c699c0c382e51c985098f3e Mon Sep 17 00:00:00 2001 From: Rowan Flood Date: Mon, 12 Feb 2024 16:49:24 -0600 Subject: [PATCH 1/3] added drive forward at the end of collectNote, and made it run backwards --- src/main/java/frc/robot/commands/CollectNote.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/CollectNote.java b/src/main/java/frc/robot/commands/CollectNote.java index cb0949dd..90c784cb 100644 --- a/src/main/java/frc/robot/commands/CollectNote.java +++ b/src/main/java/frc/robot/commands/CollectNote.java @@ -86,7 +86,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 +96,10 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - drivetrain.stop(); + new SequentialCommandGroup( + drivetrain.drive(new ChassisSpeeds(2, 0, 0)), + new WaitCommand(0.25), + drivetrain.stop()) } // Returns true when the command should end. From 22d4cc15413d1643e80274f13dad3ff6b7443ac2 Mon Sep 17 00:00:00 2001 From: Rowan Flood Date: Mon, 12 Feb 2024 18:21:39 -0600 Subject: [PATCH 2/3] changged direction of note pickup to be correct --- src/main/java/frc/robot/commands/CollectNote.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/CollectNote.java b/src/main/java/frc/robot/commands/CollectNote.java index 90c784cb..15df7c42 100644 --- a/src/main/java/frc/robot/commands/CollectNote.java +++ b/src/main/java/frc/robot/commands/CollectNote.java @@ -97,7 +97,7 @@ public void execute() { @Override public void end(boolean interrupted) { new SequentialCommandGroup( - drivetrain.drive(new ChassisSpeeds(2, 0, 0)), + drivetrain.drive(new ChassisSpeeds(-2, 0, 0)), new WaitCommand(0.25), drivetrain.stop()) } From 46c64591d5dd6884a6e3fdd58c3a6af612ea15b3 Mon Sep 17 00:00:00 2001 From: Liam Sykes <78829557+trixydevs@users.noreply.github.com> Date: Tue, 13 Feb 2024 22:03:29 -0600 Subject: [PATCH 3/3] made command for driving forward after lining up with a note --- src/main/java/frc/robot/RobotContainer.java | 7 ++- .../java/frc/robot/commands/CollectNote.java | 14 ++--- .../frc/robot/commands/DriveTimeCommand.java | 61 +++++++++++++++++++ 3 files changed, 74 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DriveTimeCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a82267be..240121b9 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; @@ -63,6 +64,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; @@ -239,7 +241,10 @@ private void configureBindings() { () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL), driverController::getR1Button)); - new Trigger(driverController::getCircleButton).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) { diff --git a/src/main/java/frc/robot/commands/CollectNote.java b/src/main/java/frc/robot/commands/CollectNote.java index 15df7c42..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(); @@ -96,10 +100,6 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - new SequentialCommandGroup( - drivetrain.drive(new ChassisSpeeds(-2, 0, 0)), - new WaitCommand(0.25), - 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); + } +}