diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f2d6b80..1e2ae3b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -48,7 +48,7 @@ public class RobotContainer { manette1.povDown().toggleOnTrue(new Depart(elevateur, pince)); //manette2 manette2.leftBumper().toggleOnTrue(new DepartPince(pince)); - manette2.a().whileTrue(new StationPince(pince)); + manette2.a().whileTrue(new StationPince(pince,elevateur)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/command/StationPince.java b/src/main/java/frc/robot/command/StationPince.java index 9721590..2f72e07 100644 --- a/src/main/java/frc/robot/command/StationPince.java +++ b/src/main/java/frc/robot/command/StationPince.java @@ -5,15 +5,18 @@ package frc.robot.command; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Pince; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class StationPince extends Command { private Pince pince; + private Elevateur elevateur; /** Creates a new L2Pince. */ - public StationPince(Pince pince) { + public StationPince(Pince pince,Elevateur elevateur) { + this.elevateur = elevateur; this.pince = pince; - addRequirements(pince); + addRequirements(pince,elevateur); // Use addRequirements() here to declare subsystem dependencies. } @@ -33,6 +36,15 @@ public class StationPince extends Command { else{ pince.pivote(0.5); } + if(elevateur.position()>=400 && elevateur.position()<=410){ + elevateur.vitesse(0); + } + else if(elevateur.position()>=510){ + elevateur.vitesse(-0.3); + } + else{ + elevateur.vitesse(.3); + } } // Called once the command ends or is interrupted.