diff --git a/src/main/java/frc/robot/subsystem/Grimpeur.java b/src/main/java/frc/robot/subsystem/Grimpeur.java index 62d22be..b61e3c5 100644 --- a/src/main/java/frc/robot/subsystem/Grimpeur.java +++ b/src/main/java/frc/robot/subsystem/Grimpeur.java @@ -4,14 +4,52 @@ package frc.robot.subsystem; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class Grimpeur extends SubsystemBase { /** Creates a new Acrocheur. */ + // moteur public Grimpeur() {} - + final CANSparkMax grimpeurd = new CANSparkMax(Constants.grimpeurd,MotorType.kBrushless); + final CANSparkMax grimpeurg = new CANSparkMax(Constants.grimpeurg,MotorType.kBrushless); + // limit switch + final DigitalInput limitdroite = new DigitalInput(Constants.limithaut); + final DigitalInput limitgauche = new DigitalInput(Constants.limitbas); + //fonction +public void droit(double vitesse){ + grimpeurd.set(vitesse); +} +public void gauche(double vitesse){ + grimpeurg.set(vitesse); +} +public boolean droite(){ + return limitdroite.get(); +} +public boolean gauche(){ + return limitgauche.get(); +} +public void resetencodeurd(){ + grimpeurd.getEncoder().setPosition(0); +} +public void resetencodeurg(){ + grimpeurg.getEncoder().setPosition(0); +} +public void grimpeur(){ + grimpeurg.follow(grimpeurd); +} @Override public void periodic() { // This method will be called once per scheduler run + if(droite()) { + resetencodeurd(); + } + if(gauche()) { + resetencodeurg(); + } } } diff --git a/src/main/java/frc/robot/subsystem/Lanceur.java b/src/main/java/frc/robot/subsystem/Lanceur.java index dc7ee09..45e0a1e 100644 --- a/src/main/java/frc/robot/subsystem/Lanceur.java +++ b/src/main/java/frc/robot/subsystem/Lanceur.java @@ -18,11 +18,17 @@ public class Lanceur extends SubsystemBase { public Lanceur() {} final CANSparkMax lancer = new CANSparkMax(Constants.lanceur, MotorType.kBrushless); final CANSparkMax lancer2 = new CANSparkMax(Constants.lancer2, MotorType.kBrushless); + final CANSparkMax lancer3 = new CANSparkMax(Constants.lanceur, MotorType.kBrushless); + final CANSparkMax lancer4 = new CANSparkMax(Constants.lancer2, MotorType.kBrushless); public void lancer(double vitesse){ lancer.set(vitesse); } public void lanceur(){ lancer2.follow(lancer); + lancer3.follow(lancer); + lancer4.follow(lancer); + lancer3.setInverted(true); + lancer4.setInverted(true); }