diff --git a/src/main/java/frc/robot/Commands/FollowAprilTag.java b/src/main/java/frc/robot/Commands/FollowAprilTag.java new file mode 100644 index 0000000..57e8169 --- /dev/null +++ b/src/main/java/frc/robot/Commands/FollowAprilTag.java @@ -0,0 +1,54 @@ +// 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.wpilibj2.command.Command; +import frc.robot.Subsystems.Drive; +import frc.robot.Subsystems.Limelight3G; + +public class FollowAprilTag extends Command { + + private Limelight3G enlignement; + private Drive drive; + /** Creates a new Limelight3g. */ + public FollowAprilTag(Limelight3G enlignement, Drive drive) { + // Use addRequirements() here to declare subsystem dependencies. + this.drive = drive; + this.enlignement = enlignement; + + addRequirements(drive, enlignement); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + if (enlignement.getv()==1) + { + drive.drive(0, 0, enlignement.getx()/30); + } + else + { + drive.drive(0, 0, 0); + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + drive.drive(0, 0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +}