Collect note extra

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);

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.

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);
+  }
+}