robot-2023/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java
2023-03-15 20:26:42 -04:00

65 lines
1.8 KiB
Java

// 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 PivotChercheHaut extends CommandBase {
private BrasTelescopique brasTelescopique;
private Pivot pivot;
/** Creates a new PivotChercheHaut. */
public PivotChercheHaut(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() {
brasTelescopique.fermer();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if(brasTelescopique.photocell()){
brasTelescopique.ouvrir();
brasTelescopique.Reset();
brasTelescopique.AvanceRecule(0);
}
else{
brasTelescopique.AvanceRecule(0.3);
}
if (pivot.distance()<43.5){
pivot.monteDescendre(0.5);
}
else if(pivot.distance()>44.5) {
pivot.monteDescendre(-0.5);
}
else{
pivot.monteDescendre(0);
}
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
brasTelescopique.AvanceRecule(0);
pivot.monteDescendre(0);
brasTelescopique.ouvrir();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}