diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c96a2e3..4ca743c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,11 +20,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.TunerConstants.TunerConstants; import frc.robot.commands.AprilTag3; import frc.robot.commands.AprilTag3G; @@ -49,35 +47,37 @@ public class RobotContainer { /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband - .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors + .withDriveRequestType(DriveRequestType.OpenLoopVoltage + ); // Use open-loop control for drive motors private final Telemetry logger = new Telemetry(MaxSpeed); - private final CommandXboxController manette1 = new CommandXboxController(0); private final CommandXboxController manette2 = new CommandXboxController(1); - + private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2 public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + private final SendableChooser autoChooser; + public double getAngle() { + return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot + } + Elevateur elevateur = new Elevateur(); Pince pince = new Pince(); ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY); PinceManuel pinceManuel = new PinceManuel(pince); PinceManuel2 pinceManuel2 = new PinceManuel2(pince); - private final SendableChooser autoChooser; Bougie bougie = new Bougie(); Limelight3G limelight3g = new Limelight3G(); Limelight3 limelight3 = new Limelight3(); Pose2d pose = new Pose2d(); - private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2 + + - public double getAngle() { - return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot - } public RobotContainer() { - autoChooser = AutoBuilder.buildAutoChooser("New Auto"); - SmartDashboard.putData("Auto Mode", autoChooser); - configureBindings(); - NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null)); - } + autoChooser = AutoBuilder.buildAutoChooser("New Auto"); + SmartDashboard.putData("Auto Mode", autoChooser); + configureBindings(); + NamedCommands.registerCommand("AprilTag", new AprilTag3G(limelight3g, drivetrain, null, null)); + } private void configureBindings() { // Elevateur manuel @@ -100,8 +100,8 @@ public class RobotContainer { .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.15)) // Drive counterclockwise with negative X (left) ) ); + drivetrain.registerTelemetry(logger::telemeterize); - // 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)); @@ -110,9 +110,7 @@ public class RobotContainer { manette1.b().whileTrue(new L2(elevateur, pince)); // manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); - drivetrain.registerTelemetry(logger::telemeterize); } - public Command getAutonomousCommand() { return new SequentialCommandGroup(Commands.runOnce(()->{ boolean flip = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red;