diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 266d36c..bc70887 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -41,6 +41,9 @@ import frc.robot.commands.Pince.CorailAspir; import frc.robot.commands.Pince.CoralAlgueInspire; import frc.robot.commands.Pince.CoralExpire; import frc.robot.commands.Pince.PinceManuel; +import frc.robot.commands.requin.BalayeuseBas; +import frc.robot.commands.requin.BalayeuseHaut; +import frc.robot.commands.requin.exspire; import frc.robot.subsystems.Bougie; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Elevateur; @@ -48,6 +51,7 @@ import frc.robot.subsystems.Grimpeur; import frc.robot.subsystems.Limelight3; import frc.robot.subsystems.Limelight3G; import frc.robot.subsystems.Pince; +import frc.robot.subsystems.Requin; public class RobotContainer { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed @@ -78,6 +82,7 @@ public class RobotContainer { Limelight3 limelight3 = new Limelight3(); Pose2d pose = new Pose2d(); Grimpeur Grimpeur = new Grimpeur(); + Requin requin = new Requin(); public RobotContainer() { autoChooser = AutoBuilder.buildAutoChooser("New Auto"); @@ -91,17 +96,47 @@ public class RobotContainer { NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie)); } private void configureBindings() { - - drivetrain.setDefaultCommand( - // Drivetrain will execute this command periodically - drivetrain.applyRequest(() -> - drive.withVelocityX(MathUtil.applyDeadband(Math.pow(-manette1.getLeftX(), 2)*MaxSpeed, 0.1)) // Drive forward with negative Y (forward) - .withVelocityY(MathUtil.applyDeadband(Math.pow(-manette1.getLeftY(), 2)*MaxSpeed, 0.10000)) // Drive left with negative X (left) - .withRotationalRate(MathUtil.applyDeadband(Math.pow(-manette1.getRightX(), 2)*manette1.getRightX()*MaxAngularRate, 0.1)) // Drive counterclockwise with negative X (left) + drivetrain.registerTelemetry(logger::telemeterize); + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain.applyRequest(() -> + drive.withVelocityX(MathUtil.applyDeadband(Math.pow(-manette1.getLeftX(), 2)*MaxSpeed, 0.1)) // Drive forward with negative Y (forward) + .withVelocityY(MathUtil.applyDeadband(Math.pow(-manette1.getLeftY(), 2)*MaxSpeed, 0.10000)) // Drive left with negative X (left) + .withRotationalRate(MathUtil.applyDeadband(Math.pow(-manette1.getRightX(), 2)*manette1.getRightX()*MaxAngularRate, 0.1)) // Drive counterclockwise with negative X (left) ) ); - // Elevateur manuel - drivetrain.registerTelemetry(logger::telemeterize); + + + /* Manette 1 */ + // reset the field-centric heading on start press + manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); + + //pince + manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie)); + manette1.rightBumper().whileTrue(new StationPince(pince, elevateur)); + manette1.leftTrigger().whileTrue(new CoralExpire(pince, bougie)); + + //elevateur + 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)); + + /* Manette 2 */ + //pince + manette2.a().whileTrue(new CorailAspir(pince)); + manette2.b().whileTrue(new Algue_inspire(pince)); + manette2.y().whileTrue(new BalayeuseHaut(requin)); + manette2.x().whileTrue(new BalayeuseBas(requin)); + manette2.rightTrigger().whileTrue(new exspire(requin)); + manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie)); + + //Pince manuel + pince.setDefaultCommand(new RunCommand(()->{ + pince.pivote(MathUtil.applyDeadband(manette2.getRightY(), 0.1)); + }, pince)); + + //Elevateur manuel elevateur.setDefaultCommand(new RunCommand(()->{ if(elevateur.limit2()){ elevateur.vitesse(0); @@ -111,43 +146,13 @@ public class RobotContainer { } }, elevateur)); - //Pince manuel - pince.setDefaultCommand(new RunCommand(()->{ - pince.pivote(MathUtil.applyDeadband(manette2.getRightY(), 0.1)); - }, pince)); - - - // manette1 - - // reset the field-centric heading on start press - manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric())); - - //elevateur - 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)); - - //pince - manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie)); - manette1.rightBumper().whileTrue(new StationPince(pince, elevateur)); - manette1.leftTrigger().whileTrue(new CoralExpire(pince, bougie)); - - //manette2 - - //pince - manette2.a().whileTrue(new CorailAspir(pince)); - manette2.start().whileTrue(new reset(elevateur, pince)); - manette2.b().whileTrue(new Algue_inspire(pince)); - manette2.start().whileTrue(new reset(elevateur,pince)); - manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie)); - //limelight manette2.leftBumper().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); manette2.rightBumper().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); + + //Reset encodeur + manette2.start().whileTrue(new reset(elevateur, pince)); } - - // manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); public Command getAutonomousCommand() { return new SequentialCommandGroup(Commands.runOnce(()->{