DriveCommand.java

package frc.robot.commands;

import java.util.function.DoubleSupplier;

import edu.wpi.first.wpilibj2.command.Command;

import frc.robot.subsystems.Drivetrain;

public class DriveCommand extends Command {
    
    Drivetrain drivetrain;
    DoubleSupplier xSupplier;
    DoubleSupplier ySupplier;
    DoubleSupplier omegaSupplier;
    DoubleSupplier triggerSupplier;
    /** drive robot according to the x and y of the drive controller, drive at half speed by default, increase by half the value of the drive trigger (0-1)**/
    public DriveCommand(Drivetrain drivetrain, DoubleSupplier xSupplier, DoubleSupplier ySupplier,
		      DoubleSupplier omegaSupplier, DoubleSupplier triggerSupplier) {
      addRequirements(drivetrain);
    this.drivetrain = drivetrain;
    this.xSupplier = xSupplier;
    this.ySupplier = ySupplier;
    this.omegaSupplier = omegaSupplier;
    this.triggerSupplier = triggerSupplier;
  }

  @Override
  public void initialize() {
  }

  @Override
  public void execute() {
      double scaleFactor = triggerSupplier.getAsDouble() * 0.5 + 0.5;
      drivetrain.drive(ySupplier.getAsDouble() * scaleFactor, xSupplier.getAsDouble() * scaleFactor, omegaSupplier.getAsDouble());
  }

  @Override
  public void end(boolean interrupted) {
    drivetrain.drive(0.0, 0.0, 0.0);
  }

  @Override
  public boolean isFinished() {
    return false;
  }
}