From 2c79346ce98c80f71d749f5cfbe0025b5dedf77a Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Wed, 22 Feb 2023 18:29:18 -0500 Subject: [PATCH] dgbxb v --- src/main/java/frc/robot/Constants.java | 2 ++ .../frc/robot/commands/bras/PivotBrasRentre.java | 4 +++- .../frc/robot/commands/bras/PivotChercheBas.java | 2 ++ .../frc/robot/commands/bras/PivotChercheHaut.java | 2 ++ .../java/frc/robot/commands/bras/PivoteBrasBas.java | 2 ++ .../java/frc/robot/commands/bras/PivoteBrasHaut.java | 2 ++ .../frc/robot/commands/bras/PivoteBrasMilieux.java | 2 ++ .../frc/robot/subsystems/bras/BrasTelescopique.java | 12 +++++++++++- 8 files changed, 26 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f0fb8a8..1518cc8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -18,6 +18,8 @@ public class Constants { public static int actuateur = 2; public static int brakedroit = 3; public static int brakegauche = 4; + public static int brakewinchf = 5; + public static int brakewinchb = 5; // DIO public static int limitbd = 0; public static int limitbg = 2; diff --git a/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java b/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java index 192dc88..6efbb90 100644 --- a/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java +++ b/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java @@ -29,6 +29,7 @@ public class PivotBrasRentre extends CommandBase { public void execute() { if(brasTelescopique.distance()>1){ brasTelescopique.AvanceRecule(0.5); + brasTelescopique.fermer(); } if (pivot.distance()>1){ pivot.monteDescendre(0.5); @@ -36,9 +37,10 @@ public class PivotBrasRentre extends CommandBase { else if(brasTelescopique.photocell()){ brasTelescopique.Reset(); brasTelescopique.AvanceRecule(0); + brasTelescopique.ouvrir(); } else{ - brasTelescopique.AvanceRecule(.5); + brasTelescopique.AvanceRecule(0.5); } if(pivot.limitpivot()){ pivot.Reset(); diff --git a/src/main/java/frc/robot/commands/bras/PivotChercheBas.java b/src/main/java/frc/robot/commands/bras/PivotChercheBas.java index 33cc965..76367e4 100644 --- a/src/main/java/frc/robot/commands/bras/PivotChercheBas.java +++ b/src/main/java/frc/robot/commands/bras/PivotChercheBas.java @@ -29,12 +29,14 @@ public class PivotChercheBas extends CommandBase { public void execute() { if(brasTelescopique.distance()<10){ brasTelescopique.AvanceRecule(0.5); + brasTelescopique.fermer(); } else if(brasTelescopique.distance()>11) { brasTelescopique.AvanceRecule(-0.5); } else { brasTelescopique.AvanceRecule(0); + brasTelescopique.ouvrir(); } if (pivot.distance()<10){ pivot.monteDescendre(0.5); diff --git a/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java b/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java index 4288827..1bdd45a 100644 --- a/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java +++ b/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java @@ -29,12 +29,14 @@ public class PivotChercheHaut extends CommandBase { public void execute() { if(brasTelescopique.distance()<10){ brasTelescopique.AvanceRecule(0.5); + brasTelescopique.fermer(); } else if(brasTelescopique.distance()>11) { brasTelescopique.AvanceRecule(-0.5); } else { brasTelescopique.AvanceRecule(0); + brasTelescopique.ouvrir(); } if (pivot.distance()<10){ pivot.monteDescendre(0.5); diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java b/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java index 038ec51..226454e 100644 --- a/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java +++ b/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java @@ -31,12 +31,14 @@ public class PivoteBrasBas extends CommandBase { public void execute() { if(brasTelescopique.distance()<10){ brasTelescopique.AvanceRecule(0.5); + brasTelescopique.fermer(); } else if(brasTelescopique.distance()>11) { brasTelescopique.AvanceRecule(-0.5); } else { brasTelescopique.AvanceRecule(0); + brasTelescopique.ouvrir(); } if (pivot.distance()<10){ pivot.monteDescendre(0.5); diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java b/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java index 2ba3c74..1968013 100644 --- a/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java +++ b/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java @@ -29,12 +29,14 @@ public class PivoteBrasHaut extends CommandBase { public void execute() { if(brasTelescopique.distance()<10){ brasTelescopique.AvanceRecule(0.5); + brasTelescopique.fermer(); } else if(brasTelescopique.distance()>11) { brasTelescopique.AvanceRecule(-0.5); } else { brasTelescopique.AvanceRecule(0); + brasTelescopique.ouvrir(); } if (pivot.distance()<10){ pivot.monteDescendre(0.5); diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java b/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java index da8939b..c230cfc 100644 --- a/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java +++ b/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java @@ -29,12 +29,14 @@ public class PivoteBrasMilieux extends CommandBase { public void execute() { if(brasTelescopique.distance()<10){ brasTelescopique.AvanceRecule(0.5); + brasTelescopique.fermer(); } else if(brasTelescopique.distance()>11) { brasTelescopique.AvanceRecule(-0.5); } else { brasTelescopique.AvanceRecule(0); + brasTelescopique.ouvrir(); } if (pivot.distance()<10){ pivot.monteDescendre(0.5); diff --git a/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java b/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java index e015295..98bc79c 100644 --- a/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java +++ b/src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java @@ -9,6 +9,9 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -18,6 +21,7 @@ public class BrasTelescopique extends SubsystemBase { public BrasTelescopique() {} final CANSparkMax Winch = new CANSparkMax(Constants.BrasTelescopique,MotorType.kBrushless); private DigitalInput photocell = new DigitalInput(Constants.photocell); + private DoubleSolenoid brakewinch = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakewinchf, Constants.brakewinchb); public void AvanceRecule(double vitesse) { Winch.set(vitesse); } @@ -28,8 +32,14 @@ public class BrasTelescopique extends SubsystemBase { Winch.getEncoder().setPosition(0); } public boolean photocell(){ - return photocell.get(); + return photocell.get(); } + public void fermer() { + brakewinch.set(Value.kReverse); + } + public void ouvrir() { + brakewinch.set(Value.kForward); + } @Override public void periodic() { Shuffleboard.getTab("SmartDashBoard") .add("photocell",0.1);