diff --git a/src/main/java/frc/robot/Commands/Lancer.java b/src/main/java/frc/robot/Commands/Lancer.java index fa2258c..e26ed07 100644 --- a/src/main/java/frc/robot/Commands/Lancer.java +++ b/src/main/java/frc/robot/Commands/Lancer.java @@ -4,14 +4,18 @@ package frc.robot.Commands; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.Subsystems.Accumulateur; import frc.robot.Subsystems.Lanceur; public class Lancer extends Command { private Lanceur lanceur; + private Accumulateur accumulateur; /** Creates a new Lancer. */ - public Lancer(Lanceur lanceur) { + public Lancer(Lanceur lanceur,Accumulateur accumulateur) { this.lanceur = lanceur; - addRequirements(lanceur); + this.accumulateur = accumulateur; + addRequirements(lanceur, accumulateur); // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @@ -22,7 +26,10 @@ public class Lancer extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - lanceur.lance(0.5); + lanceur.lance(); + accumulateur.desaccumule(0.2); + + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/Subsystems/Lanceur.java b/src/main/java/frc/robot/Subsystems/Lanceur.java index 2adc4a3..0414745 100644 --- a/src/main/java/frc/robot/Subsystems/Lanceur.java +++ b/src/main/java/frc/robot/Subsystems/Lanceur.java @@ -13,10 +13,12 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; 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; import edu.wpi.first.wpilibj2.command.SubsystemBase; + public class Lanceur extends SubsystemBase { /** Creates a new Lanceur. */ public Lanceur() {} @@ -25,7 +27,8 @@ public class Lanceur extends SubsystemBase { final WPI_TalonSRX lanceur1 = new WPI_TalonSRX(0); final WPI_TalonSRX lanceur2 = new WPI_TalonSRX(1); final CANSparkMax tourelle = new CANSparkMax(2, MotorType.kBrushed); - final + final DigitalInput limitswitch1 = new DigitalInput(0); + final DigitalInput limitswitch2 = new DigitalInput(1); private GenericEntry vitesse = dashboard.add("vitesselanceur", 0.2) .withSize(0,0) @@ -41,12 +44,13 @@ public class Lanceur extends SubsystemBase { lanceur2.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative); lanceur1.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1); } + public void masterslave(){ lanceur2.follow(lanceur1); lanceur2.setInverted(true); } public void lance(double vitesse){ - lanceur1.set(vitesse);lanceur2.set(vitesse); + lanceur1.set(vitesse); } public void lance(){ lance(vitesse.getDouble(0.2)); @@ -68,6 +72,12 @@ public class Lanceur extends SubsystemBase { lanceur1.config_kI(0, i); lanceur1.config_kD(0, d); } + public boolean limitswitch1(){ + return limitswitch1.get(); + } + public boolean limitswitch2(){ + return limitswitch2.get(); + } @Override public void periodic() { // This method will be called oncehttps://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json per scheduler run