diff --git a/src/main/java/frc/robot/subsystems/Accumulateur.java b/src/main/java/frc/robot/subsystems/Accumulateur.java index 3437c27..823e9e0 100644 --- a/src/main/java/frc/robot/subsystems/Accumulateur.java +++ b/src/main/java/frc/robot/subsystems/Accumulateur.java @@ -5,14 +5,15 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.motorcontrol.Talon; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; public class Accumulateur extends SubsystemBase { - private Talon moteuracc = new Talon(Constants.accumulateur); - + private WPI_TalonSRX moteuracc = new WPI_TalonSRX(Constants.accumulateur); + private DigitalInput limitacc = new DigitalInput(Constants.ballon); /** Creates a new Accumulateur. */ public Accumulateur() { @@ -20,12 +21,17 @@ public class Accumulateur extends SubsystemBase { public void Deaccumuler(double vitesse){ moteuracc.set(vitesse); } - //public double distance() { - // return(moteuracc.getEncoder().getPosition()); - // } - //public void Reset() { - // moteuracc.getEncoder().setPosition(0); - // } + + public boolean limit(){ + return limitacc.get(); + } + + public double distance() { + return(moteuracc.getSelectedSensorPosition()); + } + public void Reset() { + moteuracc.setSelectedSensorPosition(0); + } @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/Lanceur.java b/src/main/java/frc/robot/subsystems/Lanceur.java index 1c61716..15ff030 100644 --- a/src/main/java/frc/robot/subsystems/Lanceur.java +++ b/src/main/java/frc/robot/subsystems/Lanceur.java @@ -18,10 +18,16 @@ public class Lanceur extends SubsystemBase { public void lancer(double vitesse){ lanceur.set(vitesse); } - // Encodeur - // public double distance(){ - // return(lanceur.getEncoder().getPosition()); - // } + + // encodeur + public double vitesse() { + return(lanceur.getEncoder().getPosition()); + + } + public void Reset() { + lanceur.getEncoder().setPosition(0); + + } @Override public void periodic() {