diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2880b5b..c96a2e3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,85 +43,85 @@ import frc.robot.subsystems.Limelight3G; import frc.robot.subsystems.Pince; public class RobotContainer { - private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - /* 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 + /* 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 - private final Telemetry logger = new Telemetry(MaxSpeed); + private final Telemetry logger = new Telemetry(MaxSpeed); - private final CommandXboxController manette1 = new CommandXboxController(0); - private final CommandXboxController manette2 = new CommandXboxController(1); - - public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - 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 + private final CommandXboxController manette1 = new CommandXboxController(0); + private final CommandXboxController manette2 = new CommandXboxController(1); + + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + 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)); - } - private void configureBindings() { - elevateur.setDefaultCommand(new RunCommand(()->{ - elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.2)); - }, elevateur)); - 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 will execute this command periodically - drivetrain.applyRequest(() -> - drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.2)) // Drive forward with negative Y (forward) - .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.2)) // Drive left with negative X (left) - .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.15)) // Drive counterclockwise with negative X (left) - ) - ); + 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)); + } + 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 will execute this command periodically + drivetrain.applyRequest(() -> + drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*MaxSpeed, 0.2)) // Drive forward with negative Y (forward) + .withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*MaxSpeed, 0.2)) // Drive left with negative X (left) + .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*MaxAngularRate, 0.15)) // Drive counterclockwise with negative X (left) + ) + ); - // 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)); - manette1.b().whileTrue(new L2(elevateur, pince)); - - - // manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); - - - - 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)); + manette1.rightTrigger().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); + manette1.a().whileTrue(new reset(elevateur,pince)); + 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; - if(flip){ - drivetrain.resetPose(FlippingUtil.flipFieldPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose())); - } - else{ - drivetrain.resetPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose()); - } - }),autoChooser.getSelected(), new RainBow(bougie)); - } - -} + return new SequentialCommandGroup(Commands.runOnce(()->{ + boolean flip = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red; + if(flip){ + drivetrain.resetPose(FlippingUtil.flipFieldPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose())); + } + else{ + drivetrain.resetPose(((PathPlannerAuto)autoChooser.getSelected()).getStartingPose()); + } + }),autoChooser.getSelected(), new RainBow(bougie)); + } +} \ No newline at end of file