diff --git a/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java b/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java new file mode 100644 index 0000000..9624497 --- /dev/null +++ b/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java @@ -0,0 +1,47 @@ +// 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.bras; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.bras.BrasTelescopique; +import frc.robot.subsystems.bras.Pivot; + +public class PivotBrasRentre extends CommandBase { + private BrasTelescopique brasTelescopique; + private Pivot pivot; + /** Creates a new PivotBrasRentre. */ + public PivotBrasRentre(BrasTelescopique brasTelescopique, Pivot pivot) { + this.brasTelescopique = brasTelescopique; + this.pivot = pivot; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(brasTelescopique); + addRequirements(pivot); + } + + // 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(brasTelescopique.distance()>1){ + brasTelescopique.AvanceRecule(0.5); + } + if (pivot.distance()>1){ + pivot.monteDescendre(0.5); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +}