diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a370362..c9eb30f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,11 @@ import frc.robot.commands.AlgueExpire; import frc.robot.commands.Algue_inspire; import frc.robot.commands.AprilTag3; import frc.robot.commands.AprilTag3G; +import frc.robot.commands.CorailAspir; +import frc.robot.commands.CorailTest; +import frc.robot.commands.CoralAlgueInspire; +import frc.robot.commands.CoralExpire; +import frc.robot.commands.Depart; import frc.robot.commands.ElevateurManuel; import frc.robot.commands.Forme3; import frc.robot.commands.L2; @@ -38,6 +43,7 @@ import frc.robot.commands.L4; import frc.robot.commands.PinceManuel; import frc.robot.commands.PinceManuel2; import frc.robot.commands.RainBow; +import frc.robot.commands.StationPince; import frc.robot.commands.reset; import frc.robot.subsystems.Bougie; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -69,7 +75,7 @@ public class RobotContainer { Elevateur elevateur = new Elevateur(); Pince pince = new Pince(); ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY); - PinceManuel pinceManuel = new PinceManuel(pince); + PinceManuel pinceManuel = new PinceManuel(pince,manette2::getRightY); PinceManuel2 pinceManuel2 = new PinceManuel2(pince); Bougie bougie = new Bougie(); Limelight3G limelight3g = new Limelight3G(); @@ -83,22 +89,15 @@ public class RobotContainer { SmartDashboard.putData("Auto Mode", autoChooser); configureBindings(); NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null)); + NamedCommands.registerCommand("Station",new StationPince(pince, elevateur)); + NamedCommands.registerCommand("L4", new L4(elevateur, pince)); + NamedCommands.registerCommand("L3", new L3(elevateur, pince)); + NamedCommands.registerCommand("CoralExpire",new CoralExpire(pince,bougie)); + NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie)); } private void configureBindings() { // Elevateur manuel - - elevateur.setDefaultCommand(new RunCommand(()->{ - elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.2)); - }, elevateur)); - - //Pince manuel - pince.setDefaultCommand(new RunCommand(()->{ - pince.pivote(MathUtil.applyDeadband(manette2.getRightY()/5, 0.2)); - }, pince)); - - // Note that X is defined as forward according to WPILib convention, - // and Y is defined as to the left according to WPILib convention. - drivetrain.setDefaultCommand( + drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.2)) // Drive forward with negative Y (forward) @@ -107,12 +106,19 @@ public class RobotContainer { ) ); drivetrain.registerTelemetry(logger::telemeterize); + elevateur.setDefaultCommand(new RunCommand(()->{ + elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.2)); + }, elevateur)); + //Pince manuel + pince.setDefaultCommand(new RunCommand(()->{ + pince.pivote(MathUtil.applyDeadband(manette2.getRightY()/5, 0.2)); + }, pince)); + // reset the field-centric heading on left bumper press manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); manette1.leftTrigger().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); manette1.rightTrigger().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); - manette1.a().whileTrue(new reset(elevateur,pince)); manette2.a().whileTrue(new Algue_inspire(pince)); @@ -120,9 +126,32 @@ public class RobotContainer { manette2.x().whileTrue(new L3(elevateur, pince)); manette2.y().whileTrue(new L4(elevateur, pince)); manette2.rightBumper().whileTrue(new AlgueExpire(pince, bougie)); + // manette1 + manette1.a().whileTrue(new Depart(elevateur, pince)); + manette1.b().whileTrue(new L2(elevateur,pince)); + manette1.x().whileTrue(new L3(elevateur, pince)); + manette1.y().whileTrue(new L4(elevateur, pince)); + manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie)); + manette1.rightBumper().whileTrue(new StationPince(pince, elevateur)); + //manette2 + manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie)); + manette2.a().whileTrue(new CorailAspir(pince)); + manette2.b().whileTrue(new CoralExpire(pince, bougie)); + manette2.povLeft().whileTrue(new Algue1Test(pince)); + manette2.povRight().whileTrue(new Algue2Test(pince)); + manette2.x().whileTrue(new CorailTest(pince)); + manette2.start().whileTrue(new reset(elevateur)); + } + + + + + // Note that X is defined as forward according to WPILib convention, + // and Y is defined as to the left according to WPILib convention. + // manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); - } + public Command getAutonomousCommand() { return new SequentialCommandGroup(Commands.runOnce(()->{ boolean flip = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red; diff --git a/src/main/java/frc/robot/commands/PinceManuel.java b/src/main/java/frc/robot/commands/PinceManuel.java index acb39a3..20abde8 100644 --- a/src/main/java/frc/robot/commands/PinceManuel.java +++ b/src/main/java/frc/robot/commands/PinceManuel.java @@ -3,17 +3,19 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.commands; +import java.util.function.DoubleSupplier; + import edu.wpi.first.wpilibj2.command.Command; 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 PinceManuel extends Command { private Pince pince; + private DoubleSupplier x; /** Creates a new PinceManuel. */ - public PinceManuel(Pince pince - //, DoubleSupplier doubleSupplier - ) { + public PinceManuel(Pince pince, DoubleSupplier x) { this.pince = pince; + this.x = x; //this.doubleSupplier = doubleSupplier; addRequirements(pince); // Use addRequirements() here to declare subsystem dependencies. @@ -26,12 +28,12 @@ public class PinceManuel extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // if(pince.position()){ - // pince.pivote(0); - // } - // else{ - pince.pivote(0.2); - // } + if(pince.position()){ + pince.pivote(0); + } + else{ + pince.pivote(x.getAsDouble()); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/reset.java b/src/main/java/frc/robot/commands/reset.java index 9a61223..d6ba79b 100644 --- a/src/main/java/frc/robot/commands/reset.java +++ b/src/main/java/frc/robot/commands/reset.java @@ -15,7 +15,7 @@ public class reset extends Command { /** Creates a new reset. */ private Elevateur elevateur; private Pince pince; - public reset(Elevateur elevateur, Pince pince) { + public reset(Elevateur elevateur) { // Use addRequirements() here to declare subsystem dependencies. this.elevateur = elevateur; this.pince = pince;