diff --git a/src/main/java/frc/robot/commands/Force1.java b/src/main/java/frc/robot/commands/Force1.java index bd7a893..650d76b 100644 --- a/src/main/java/frc/robot/commands/Force1.java +++ b/src/main/java/frc/robot/commands/Force1.java @@ -31,7 +31,12 @@ public class Force1 extends CommandBase { double vitesse = SmartDashboard.getNumber("Force Lanceur", 100); lanceur.lancer(200); if (lanceur.vitesse() > vitesse ){ - accumulateur.tourneavant(); + if(accumulateur.tourneavant() < 1024){ + accumulateur.Deaccumuler(); + } + if(accumulateur.tournearriere()<-256){ + accumulateur.reaccumuler(); + } accumulateur.tournearriere(); } else { accumulateur.stop(); diff --git a/src/main/java/frc/robot/subsystems/Accumulateur.java b/src/main/java/frc/robot/subsystems/Accumulateur.java index 91b6f8a..daa68d0 100644 --- a/src/main/java/frc/robot/subsystems/Accumulateur.java +++ b/src/main/java/frc/robot/subsystems/Accumulateur.java @@ -13,7 +13,6 @@ import frc.robot.Constants; public class Accumulateur extends SubsystemBase { private WPI_TalonSRX moteuracc = new WPI_TalonSRX(Constants.accumulateur); - private DigitalInput limitacc = new DigitalInput(Constants.ballon); /** Creates a new Accumulateur. */ public Accumulateur() { @@ -21,14 +20,14 @@ public class Accumulateur extends SubsystemBase { public void Deaccumuler(){ moteuracc.set(0.5); } - + public void reaccumuler(){ + moteuracc.set(-0.5); + } public double tourneavant(){ - moteuracc.set(0.5); return(moteuracc.getSelectedSensorPosition()); } public double tournearriere(){ - moteuracc.set(-0.5); return(moteuracc.getSelectedSensorPosition()); }