diff --git a/src/main/java/frc/robot/command/Depart.java b/src/main/java/frc/robot/command/Depart.java new file mode 100644 index 0000000..4d101e9 --- /dev/null +++ b/src/main/java/frc/robot/command/Depart.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.command; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Elevateur; + +/* 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 Depart extends Command { + private Elevateur elevateur; + /** Creates a new L2. */ + public Depart(Elevateur elevateur) { + this.elevateur = elevateur; + addRequirements(elevateur); + // 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(elevateur.limit2()==true){ + elevateur.vitesse(0); + elevateur.reset(); + } + else{ + elevateur.vitesse(-.5); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + elevateur.vitesse(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return elevateur.limit2()==true; + } +} diff --git a/src/main/java/frc/robot/command/ElevateurManuel.java b/src/main/java/frc/robot/command/ElevateurManuel.java new file mode 100644 index 0000000..c75b4f9 --- /dev/null +++ b/src/main/java/frc/robot/command/ElevateurManuel.java @@ -0,0 +1,48 @@ +// 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.command; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Elevateur; + +/* 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 ElevateurManuel extends Command { + private DoubleSupplier doubleSupplier; + private Elevateur elevateur; + /** Creates a new ElevateurManuel. */ + public ElevateurManuel(Elevateur elevateur,DoubleSupplier doubleSupplier) { + this.doubleSupplier = doubleSupplier; + this.elevateur = elevateur; + addRequirements(elevateur); + // 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(elevateur.limit2()==true){ + elevateur.vitesse(0); + } + elevateur.vitesse(doubleSupplier.getAsDouble()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + elevateur.vitesse(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/command/L2.java b/src/main/java/frc/robot/command/L2.java index 9804df8..36c6522 100644 --- a/src/main/java/frc/robot/command/L2.java +++ b/src/main/java/frc/robot/command/L2.java @@ -24,11 +24,11 @@ public class L2 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(elevateur.position(500)>=500){ + if(elevateur.position()>=500){ elevateur.vitesse(0); } else{ - elevateur.vitesse(.5); + elevateur.vitesse(.3); } } @@ -41,6 +41,6 @@ public class L2 extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return elevateur.position(500)>=500; + return elevateur.position()>=500; } } diff --git a/src/main/java/frc/robot/command/L3.java b/src/main/java/frc/robot/command/L3.java index e5ff0da..92dfd18 100644 --- a/src/main/java/frc/robot/command/L3.java +++ b/src/main/java/frc/robot/command/L3.java @@ -24,7 +24,7 @@ public class L3 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(elevateur.position(700)>=700){ + if(elevateur.position()>=700){ elevateur.vitesse(0); } else{ @@ -41,6 +41,6 @@ public class L3 extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return elevateur.position(700)>=700; + return elevateur.position()>=700; } } diff --git a/src/main/java/frc/robot/command/L4.java b/src/main/java/frc/robot/command/L4.java index b0b3c1c..df01def 100644 --- a/src/main/java/frc/robot/command/L4.java +++ b/src/main/java/frc/robot/command/L4.java @@ -24,7 +24,7 @@ public class L4 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(elevateur.position(800)>=800){ + if(elevateur.position()>=800){ elevateur.vitesse(0); } else{ @@ -41,6 +41,6 @@ public class L4 extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return elevateur.position(800)>=800; + return elevateur.position()>=800; } } diff --git a/src/main/java/frc/robot/subsystems/Elevateur.java b/src/main/java/frc/robot/subsystems/Elevateur.java index e81d5ef..b37f6f0 100644 --- a/src/main/java/frc/robot/subsystems/Elevateur.java +++ b/src/main/java/frc/robot/subsystems/Elevateur.java @@ -13,16 +13,18 @@ public class Elevateur extends SubsystemBase { public Elevateur() {} final SparkMax monte = new SparkMax(0, MotorType.kBrushless); final DigitalInput limit2 = new DigitalInput(0); - public double position(double position){ + public double position(){ return monte.getEncoder().getPosition(); } public void vitesse(double vitesse){ monte.set(vitesse); } - - - - + public boolean limit2(){ + return limit2.get(); + } + public void reset(){ + monte.getEncoder().setPosition(0); + } @Override public void periodic() { // This method will be called once per scheduler run