diff --git a/src/main/java/frc/robot/commands/Elevateur/L2.java b/src/main/java/frc/robot/commands/Elevateur/L2.java index ac4c59f..f1f1e7e 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L2.java +++ b/src/main/java/frc/robot/commands/Elevateur/L2.java @@ -3,11 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.commands.Elevateur; -import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; - import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Pince; @@ -15,13 +11,6 @@ import frc.robot.subsystems.Pince; public class L2 extends Command { private Elevateur elevateur; private Pince pince; - - NetworkTableInstance networktable = NetworkTableInstance.getDefault(); - NetworkTable tabelevateur = networktable.getTable("tabelevateur"); - private DoubleSubscriber encodeur1 = tabelevateur.getDoubleTopic("encodeurelevateurbasL2").subscribe(-1); - private DoubleSubscriber encodeur2 = tabelevateur.getDoubleTopic("encodeurelevateurhautL2").subscribe(-0.9); - private DoubleSubscriber encodeur3 = tabelevateur.getDoubleTopic("encodeurpincebasL2").subscribe(-1); - private DoubleSubscriber encodeur4 = tabelevateur.getDoubleTopic("encodeurpinceautL2").subscribe(-0.9); /** Creates a new L2. */ public L2(Elevateur elevateur,Pince pince) { this.elevateur = elevateur; @@ -37,14 +26,10 @@ public class L2 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double encodeurbase = encodeur1.get(); - double encodeurhaute = encodeur2.get(); - double encodeurbasp = encodeur3.get(); - double encodeurhautp = encodeur4.get(); - if(elevateur.position()<=encodeurbase && elevateur.position()>=-encodeurhaute){ + if(elevateur.position()<=elevateur.encodeurelevateurL2bas() && elevateur.position()>=elevateur.encodeurelevateurL2haut()){ elevateur.vitesse(0); } - else if(elevateur.position()>=encodeurbase){ + else if(elevateur.position()>=elevateur.encodeurelevateurL2bas()){ elevateur.vitesse(-0.2); } else{ diff --git a/src/main/java/frc/robot/commands/Elevateur/L3.java b/src/main/java/frc/robot/commands/Elevateur/L3.java index 7b77690..b8a4bf2 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L3.java +++ b/src/main/java/frc/robot/commands/Elevateur/L3.java @@ -4,9 +4,6 @@ package frc.robot.commands.Elevateur; -import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevateur; @@ -16,12 +13,6 @@ import frc.robot.subsystems.Pince; public class L3 extends Command { private Elevateur elevateur; private Pince pince; - NetworkTableInstance networktable = NetworkTableInstance.getDefault(); - NetworkTable tabelevateur = networktable.getTable("tabelevateur"); - private DoubleSubscriber encodeur1 = tabelevateur.getDoubleTopic("encodeurhautL3").subscribe(-2.9); - private DoubleSubscriber encodeur2 = tabelevateur.getDoubleTopic("encodeurbasL3").subscribe(-3); - private DoubleSubscriber encodeur3 = tabelevateur.getDoubleTopic("encodeurpincebasL3").subscribe(-1); - private DoubleSubscriber encodeur4 = tabelevateur.getDoubleTopic("encodeurpincehautL3").subscribe(-0.9); /** Creates a new L2. */ public L3(Elevateur elevateur,Pince pince) { this.elevateur = elevateur; @@ -37,14 +28,10 @@ public class L3 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double encodeurHaute = encodeur1.get(); - double encodeurBase = encodeur2.get(); - double encodeurbasp = encodeur3.get(); - double encodeurhautp = encodeur4.get(); - if(elevateur.position()<=-encodeurHaute && elevateur.position()>=encodeurBase){ + if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){ elevateur.vitesse(0); } - else if(elevateur.position()>=-encodeurHaute){ + else if(elevateur.position()>=elevateur.encodeurelevateurL3bas()){ elevateur.vitesse(-0.2); } else{ diff --git a/src/main/java/frc/robot/commands/Elevateur/L4.java b/src/main/java/frc/robot/commands/Elevateur/L4.java index e83f55d..6663827 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L4.java +++ b/src/main/java/frc/robot/commands/Elevateur/L4.java @@ -4,13 +4,7 @@ package frc.robot.commands.Elevateur; -import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.DoubleTopic; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; - import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Pince; @@ -18,18 +12,10 @@ import frc.robot.subsystems.Pince; public class L4 extends Command { private Elevateur elevateur; private Pince pince; - NetworkTableInstance networktable = NetworkTableInstance.getDefault(); - NetworkTable tabelevateur = networktable.getTable("tabelevateur"); - private DoubleTopic encodeur1topic = tabelevateur.getDoubleTopic("encodeurbasL4"); - DoubleSubscriber encodeur1 = encodeur1topic.subscribe(-6.4); - private DoubleSubscriber encodeur2 = tabelevateur.getDoubleTopic("encodeurhautL4").subscribe(-6.5); - private DoubleSubscriber encodeur3 = tabelevateur.getDoubleTopic("encodeurpincebasL4").subscribe(-1); - private DoubleSubscriber encodeur4 = tabelevateur.getDoubleTopic("encodeurpincehautL4").subscribe(-0.9); /** Creates a new L2. */ public L4(Elevateur elevateur,Pince pince) { this.elevateur = elevateur; this.pince = pince; - encodeur1topic.setPersistent(true); addRequirements(elevateur,pince); // Use addRequirements() here to declare subsystem dependencies. } @@ -41,14 +27,10 @@ public class L4 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double encodeurBase = encodeur1.get(); - double encodeurhaute = encodeur2.get(); - double encodeurbasp = encodeur3.get(); - double encodeurhautp = encodeur4.get(); - if(elevateur.position()<=encodeurhaute && elevateur.position()>=encodeurBase){ + if(elevateur.position()<=elevateur.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){ elevateur.vitesse(0); } - else if(elevateur.position()>=encodeurhaute){ + else if(elevateur.position()>=elevateur.encodeurelevateurL4bas()){ elevateur.vitesse(-0.2); } else{ diff --git a/src/main/java/frc/robot/commands/Elevateur/StationPince.java b/src/main/java/frc/robot/commands/Elevateur/StationPince.java index e97fb01..05c1ebd 100644 --- a/src/main/java/frc/robot/commands/Elevateur/StationPince.java +++ b/src/main/java/frc/robot/commands/Elevateur/StationPince.java @@ -4,9 +4,6 @@ package frc.robot.commands.Elevateur; -import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; @@ -18,12 +15,6 @@ public class StationPince extends Command { ShuffleboardTab teb = Shuffleboard.getTab("teb"); private Pince pince; private Elevateur elevateur; - NetworkTableInstance networktable = NetworkTableInstance.getDefault(); - NetworkTable tabelevateur = networktable.getTable("tabelevateur"); - private DoubleSubscriber encodeur1 = tabelevateur.getDoubleTopic("encodeur bas Station").subscribe(-0.5); - private DoubleSubscriber encodeur2 = tabelevateur.getDoubleTopic("encodeur haut Station").subscribe(-0.4); - private DoubleSubscriber encodeur3 = tabelevateur.getDoubleTopic("encodeur pince bas Station").subscribe(-1); - private DoubleSubscriber encodeur4 = tabelevateur.getDoubleTopic("encodeur pince haut Station").subscribe(-0.9); /** Creates a new L2Pince. */ public StationPince(Pince pince,Elevateur elevateur) { this.elevateur = elevateur; @@ -39,15 +30,11 @@ public class StationPince extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double encodeurBase = encodeur1.get(); - double encodeurhaute = encodeur2.get(); - double encodeurbasp = encodeur3.get(); - double encodeurhautp = encodeur4.get(); pince.aspirecoral(0.5); - if(pince.encodeurpivot()<=encodeurBase && pince.encodeurpivot()>=encodeurhaute){ + if(pince.encodeurpivot()<=elevateur.encodeurelevateurstationbas() && pince.encodeurpivot()>=elevateur.encodeurelevateurstationhaut()){ pince.pivote(0); } - else if(pince.encodeurpivot()>=encodeurBase){ + else if(pince.encodeurpivot()>=elevateur.encodeurelevateurstationbas()){ pince.pivote(0.2); } else{ diff --git a/src/main/java/frc/robot/subsystems/Elevateur.java b/src/main/java/frc/robot/subsystems/Elevateur.java index 9562af3..61088a3 100644 --- a/src/main/java/frc/robot/subsystems/Elevateur.java +++ b/src/main/java/frc/robot/subsystems/Elevateur.java @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; +import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -12,8 +13,25 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; public class Elevateur extends SubsystemBase { ShuffleboardTab teb = Shuffleboard.getTab("teb"); /** Creates a new Elevateur. */ + private GenericEntry encodeurelevateurL2bas = + teb.add("encodeurelevateurL2bas", -1).getEntry(); + private GenericEntry encodeurelevateurL2haut = + teb.add("encodeurelevateurL2haut", -0.9).getEntry(); + private GenericEntry encodeurelevateurL3bas = + teb.add("ncodeurelevateurL3bas", -2.9).getEntry(); + private GenericEntry encodeurelevateurL3haut = + teb.add("encodeurelevateurL3haut", -0.9).getEntry(); + private GenericEntry encodeurelevateurL4bas = + teb.add("encodeurelevateurL4bas", -0.9).getEntry(); + private GenericEntry encodeurelevateurL4haut = + teb.add("encodeurelevateurL4haut", -0.9).getEntry(); + private GenericEntry encodeurelevateurstationbas = + teb.add("encodeurelevateursationbas", -0.9).getEntry(); + private GenericEntry encodeurelevateurstationhaut = + teb.add("encodeurelevateursationhaut", -0.9).getEntry(); + public Elevateur() { - teb.addDouble("encodeur elevateur",this::position); + teb.addDouble("encodeur elevateur L2 haut",this::position); teb.addBoolean("limit elevateur", this::limit2); } final SparkMax monte = new SparkMax(22, MotorType.kBrushless); @@ -22,6 +40,7 @@ public class Elevateur extends SubsystemBase { public double position(){ return monte.getEncoder().getPosition(); } + public void vitesse(double vitesse){ if (limit2()) { if (vitesse > 0) { @@ -41,6 +60,31 @@ public class Elevateur extends SubsystemBase { public void reset(){ monte.getEncoder().setPosition(0); } + public double encodeurelevateurL2bas(){ + return encodeurelevateurL2bas.getDouble(-1); + } + public double encodeurelevateurL2haut(){ + return encodeurelevateurL2haut.getDouble(-0.9); + } + public double encodeurelevateurL3bas(){ + return encodeurelevateurL3bas.getDouble(-2.9); + } + public double encodeurelevateurL3haut(){ + return encodeurelevateurL3haut.getDouble(-3); + } + public double encodeurelevateurL4bas(){ + return encodeurelevateurL4bas.getDouble(-6.4); + } + public double encodeurelevateurL4haut(){ + return encodeurelevateurL4haut.getDouble(-6.5); + } + public double encodeurelevateurstationbas(){ + return encodeurelevateurstationbas.getDouble(-0.5); + } + public double encodeurelevateurstationhaut(){ + return encodeurelevateurstationhaut.getDouble(-0.4); + } + @Override public void periodic() { // This method will be called once per scheduler run