diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 450a43b..e306ca1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,11 +17,12 @@ public class Constants { public static int actuateur = 2; public static int brakedroit = 3; public static int brakegauche = 4; - // DIO Brando + // DIO public static int limitbd = 0; public static int limitbg = 2; public static int limithd = 3; public static int limithg = 1; + public static int photocell = 4; } diff --git a/src/main/java/frc/robot/commands/GratteBaisser.java b/src/main/java/frc/robot/commands/GratteBaisser.java index b423033..de68b8f 100644 --- a/src/main/java/frc/robot/commands/GratteBaisser.java +++ b/src/main/java/frc/robot/commands/GratteBaisser.java @@ -3,10 +3,10 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.commands; - import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Gratte; + public class GratteBaisser extends CommandBase { private Gratte gratte; /** Creates a new GratteBaisser. */ @@ -23,7 +23,20 @@ public class GratteBaisser extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - gratte.baiser(0.5); + if(gratte.basd()){ + gratte.baiser(0); + } + else{ + gratte.baiser(0.5); + } + if(gratte.basg()){ + gratte.baiser(0); + } + else{ + gratte.baiser(0.5); + } + + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/GratteMonte.java b/src/main/java/frc/robot/commands/GratteMonte.java index 15f2b31..950dcd2 100644 --- a/src/main/java/frc/robot/commands/GratteMonte.java +++ b/src/main/java/frc/robot/commands/GratteMonte.java @@ -23,7 +23,18 @@ public class GratteMonte extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - gratte.Lever(0.5); + if(gratte.hautd()){ + gratte.Lever(0); + } + else{ + gratte.Lever(0.5); + } + if(gratte.hautg()) { + gratte.Lever(0.5); + } + else{ + gratte.Lever(0.5); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java b/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java index 9624497..e9af669 100644 --- a/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java +++ b/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java @@ -33,6 +33,9 @@ public class PivotBrasRentre extends CommandBase { if (pivot.distance()>1){ pivot.monteDescendre(0.5); } + else if(brasTelescopique.photocell()){ + brasTelescopique.Reset(); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java b/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java index f4f1458..2808ceb 100644 --- a/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java +++ b/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java @@ -8,6 +8,7 @@ package frc.robot.subsystems.bras; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -15,6 +16,7 @@ public class BrasTelescopique extends SubsystemBase { /** Creates a new BrasTelescopique. */ public BrasTelescopique() {} final CANSparkMax Winch = new CANSparkMax(Constants.BrasTelescopique,MotorType.kBrushless); + private DigitalInput photocell = new DigitalInput(Constants.photocell); public void AvanceRecule(double vitesse) { Winch.set(vitesse); } @@ -24,7 +26,9 @@ public class BrasTelescopique extends SubsystemBase { public void Reset() { Winch.getEncoder().setPosition(0); } - + public boolean photocell(){ + return photocell.get(); + } @Override public void periodic() { // This method will be called once per scheduler run