diff --git a/src/main/java/frc/robot/commands/L2.java b/src/main/java/frc/robot/commands/L2.java index 3a201a8..0d11c7f 100644 --- a/src/main/java/frc/robot/commands/L2.java +++ b/src/main/java/frc/robot/commands/L2.java @@ -4,6 +4,11 @@ package frc.robot.commands; +import java.util.function.DoubleSupplier; + +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; @@ -13,6 +18,12 @@ 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("encodeur elevateur bas L2").subscribe(-1); + private DoubleSubscriber encodeur2 = tabelevateur.getDoubleTopic("encodeur elevateur haut L2").subscribe(-0.9); + private DoubleSubscriber encodeur3 = tabelevateur.getDoubleTopic("encodeur pince bas L2").subscribe(-1); + private DoubleSubscriber encodeur4 = tabelevateur.getDoubleTopic("encodeur pince haut L2").subscribe(-0.9); /** Creates a new L2. */ public L2(Elevateur elevateur,Pince pince) { this.elevateur = elevateur; @@ -28,10 +39,14 @@ public class L2 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(elevateur.position()<=-1 && elevateur.position()>=-0.9){ + double encodeurbase = encodeur1.get(); + double encodeurhaute = encodeur2.get(); + double encodeurbasp = encodeur3.get(); + double encodeurhautp = encodeur4.get(); + if(elevateur.position()<=encodeurbase && elevateur.position()>=-encodeurhaute){ elevateur.vitesse(0); } - else if(elevateur.position()>=-1){ + else if(elevateur.position()>=encodeurbase){ elevateur.vitesse(-0.2); } else{ diff --git a/src/main/java/frc/robot/commands/L3.java b/src/main/java/frc/robot/commands/L3.java index 7308950..90d5ddf 100644 --- a/src/main/java/frc/robot/commands/L3.java +++ b/src/main/java/frc/robot/commands/L3.java @@ -4,6 +4,9 @@ package frc.robot.commands; +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; @@ -13,6 +16,12 @@ 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("encodeur haut L3").subscribe(-2.9); + private DoubleSubscriber encodeur2 = tabelevateur.getDoubleTopic("encodeur bas L3").subscribe(-3); + private DoubleSubscriber encodeur3 = tabelevateur.getDoubleTopic("encodeur pince bas L3").subscribe(-1); + private DoubleSubscriber encodeur4 = tabelevateur.getDoubleTopic("encodeur pince haut L3").subscribe(-0.9); /** Creates a new L2. */ public L3(Elevateur elevateur,Pince pince) { this.elevateur = elevateur; @@ -28,10 +37,14 @@ public class L3 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(elevateur.position()<=-2.9 && elevateur.position()>=-3){ + double encodeurHaute = encodeur1.get(); + double encodeurBase = encodeur2.get(); + double encodeurbase = encodeur3.get(); + double encodeurhaute = encodeur4.get(); + if(elevateur.position()<=-encodeurHaute && elevateur.position()>=encodeurBase){ elevateur.vitesse(0); } - else if(elevateur.position()>=-2.9){ + else if(elevateur.position()>=-encodeurHaute){ elevateur.vitesse(-0.2); } else{ diff --git a/src/main/java/frc/robot/commands/L4.java b/src/main/java/frc/robot/commands/L4.java index 6612995..e7f7758 100644 --- a/src/main/java/frc/robot/commands/L4.java +++ b/src/main/java/frc/robot/commands/L4.java @@ -4,6 +4,9 @@ package frc.robot.commands; +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; @@ -13,6 +16,12 @@ 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 DoubleSubscriber encodeur1 = tabelevateur.getDoubleTopic("encodeur bas L4").subscribe(-6.4); + private DoubleSubscriber encodeur2 = tabelevateur.getDoubleTopic("encodeur haut L4").subscribe(-6.5); + private DoubleSubscriber encodeur3 = tabelevateur.getDoubleTopic("encodeur pince bas L4").subscribe(-1); + private DoubleSubscriber encodeur4 = tabelevateur.getDoubleTopic("encodeur pince haut L4").subscribe(-0.9); /** Creates a new L2. */ public L4(Elevateur elevateur,Pince pince) { this.elevateur = elevateur; @@ -28,10 +37,14 @@ public class L4 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(elevateur.position()<=-6.5 && elevateur.position()>=-6.4){ + double encodeurBase = encodeur1.get(); + double encodeurhaute = encodeur2.get(); + double encodeurbasp = encodeur3.get(); + double encodeurhautp = encodeur4.get(); + if(elevateur.position()<=encodeurhaute && elevateur.position()>=encodeurBase){ elevateur.vitesse(0); } - else if(elevateur.position()>=-6.5){ + else if(elevateur.position()>=encodeurhaute){ elevateur.vitesse(-0.2); } else{ diff --git a/src/main/java/frc/robot/commands/StationPince.java b/src/main/java/frc/robot/commands/StationPince.java index 59527d5..bdb69a1 100644 --- a/src/main/java/frc/robot/commands/StationPince.java +++ b/src/main/java/frc/robot/commands/StationPince.java @@ -4,6 +4,9 @@ package frc.robot.commands; +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; @@ -13,6 +16,12 @@ public class StationPince extends Command { 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; @@ -28,11 +37,15 @@ 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()>=900 && pince.encodeurpivot()<=910){ + if(pince.encodeurpivot()<=encodeurBase && pince.encodeurpivot()>=encodeurhaute){ pince.pivote(0); } - else if(pince.encodeurpivot()>=910){ + else if(pince.encodeurpivot()>=encodeurBase){ pince.pivote(0.2); } else{