diff --git a/src/main/java/frc/robot/commands/Grimper.java b/src/main/java/frc/robot/commands/GrimperHaut.java similarity index 92% rename from src/main/java/frc/robot/commands/Grimper.java rename to src/main/java/frc/robot/commands/GrimperHaut.java index 11c4304..e928ca4 100644 --- a/src/main/java/frc/robot/commands/Grimper.java +++ b/src/main/java/frc/robot/commands/GrimperHaut.java @@ -8,10 +8,10 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Grimpeur; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class Grimper extends Command { +public class GrimperHaut extends Command { private Grimpeur grimpeur; /** Creates a new Grimper. */ - public Grimper(Grimper grimper) { + public GrimperHaut(GrimperHaut grimper) { this.grimpeur = new Grimpeur(); addRequirements(grimpeur); // Use addRequirements() here to declare subsystem dependencies. @@ -26,6 +26,7 @@ public class Grimper extends Command { public void execute() { if(grimpeur.stop()==true){ grimpeur.grimpe(0); + grimpeur.reset(); } else{ grimpeur.grimpe(0.5); diff --git a/src/main/java/frc/robot/commands/GrimpeurBas.java b/src/main/java/frc/robot/commands/GrimpeurBas.java new file mode 100644 index 0000000..d6b071f --- /dev/null +++ b/src/main/java/frc/robot/commands/GrimpeurBas.java @@ -0,0 +1,44 @@ +// 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.Grimpeur; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class GrimpeurBas extends Command { + private Grimpeur grimpeur; + /** Creates a new GrimpeurBas. */ + public GrimpeurBas(Grimpeur grimpeur) { + this.grimpeur = grimpeur; + addRequirements(grimpeur); + // Use addRequirements() here to declare subsystem dependencies. + } + + // 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(grimpeur.encodeur()>=500){ + grimpeur.grimpe(0); + } + grimpeur.grimpe(-0.5); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + grimpeur.grimpe(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Grimpeur.java b/src/main/java/frc/robot/subsystems/Grimpeur.java index 7d0561f..09ca682 100644 --- a/src/main/java/frc/robot/subsystems/Grimpeur.java +++ b/src/main/java/frc/robot/subsystems/Grimpeur.java @@ -22,6 +22,12 @@ public class Grimpeur extends SubsystemBase { public boolean stop(){ return limit1.get(); } + public double encodeur(){ + return grimpeur.getEncoder().getPosition(); + } + public void reset(){ + grimpeur.getEncoder().setPosition(0); + } @Override public void periodic() { // This method will be called once per scheduler run