diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e306ca1..f0fb8a8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,8 +6,9 @@ public class Constants { public static int arrieredroit = 2; public static int arrieregauche = 3; public static int BrasTelescopique = 4; + public static int pivot = 5; - //moteur gris + //moteur public static int leverGratte = 0; public static int baisserGratte = 1; @@ -23,6 +24,7 @@ public class Constants { public static int limithd = 3; public static int limithg = 1; public static int photocell = 4; + public static int limitpivot = 5; } diff --git a/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java b/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java index e9af669..192dc88 100644 --- a/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java +++ b/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java @@ -35,6 +35,17 @@ public class PivotBrasRentre extends CommandBase { } else if(brasTelescopique.photocell()){ brasTelescopique.Reset(); + brasTelescopique.AvanceRecule(0); + } + else{ + brasTelescopique.AvanceRecule(.5); + } + if(pivot.limitpivot()){ + pivot.Reset(); + pivot.monteDescendre(0); + } + else{ + pivot.monteDescendre(0.5); } } diff --git a/src/main/java/frc/robot/subsystems/bras/Pivot.java b/src/main/java/frc/robot/subsystems/bras/Pivot.java index e8d5dbb..b2e3214 100644 --- a/src/main/java/frc/robot/subsystems/bras/Pivot.java +++ b/src/main/java/frc/robot/subsystems/bras/Pivot.java @@ -4,14 +4,17 @@ package frc.robot.subsystems.bras; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import static frc.robot.Constants.*; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; public class Pivot extends SubsystemBase { // moteur - private CANSparkMax pivot = new CANSparkMax(actuateur, MotorType.kBrushless); + private CANSparkMax pivot = new CANSparkMax(Constants.pivot,MotorType.kBrushless); + private DigitalInput limitpivot = new DigitalInput(Constants.limitpivot); // function public void monteDescendre(double vitesse) { @@ -23,6 +26,10 @@ public class Pivot extends SubsystemBase { } public void Reset(){ pivot.getEncoder().setPosition(0); + + } + public boolean limitpivot(){ + return limitpivot.get(); } @Override public void periodic() {