190 lines
9.3 KiB
Java
190 lines
9.3 KiB
Java
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
package frc.robot;
|
|
|
|
import static edu.wpi.first.units.Units.*;
|
|
|
|
import java.util.Map;
|
|
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
|
|
import com.ctre.phoenix6.hardware.Pigeon2;
|
|
import com.ctre.phoenix6.swerve.SwerveRequest;
|
|
import edu.wpi.first.cameraserver.CameraServer;
|
|
import edu.wpi.first.math.MathUtil;
|
|
import edu.wpi.first.math.geometry.Pose2d;
|
|
import edu.wpi.first.networktables.GenericEntry;
|
|
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
|
|
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
|
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
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 frc.robot.TunerConstants.TunerConstants;
|
|
import frc.robot.commands.RainBow;
|
|
import frc.robot.commands.reset;
|
|
import frc.robot.commands.Elevateur.Depart;
|
|
import frc.robot.commands.Elevateur.ElevateurManuel;
|
|
import frc.robot.commands.Elevateur.L2;
|
|
import frc.robot.commands.Elevateur.L3;
|
|
import frc.robot.commands.Elevateur.L4;
|
|
import frc.robot.commands.Elevateur.StationPince;
|
|
import frc.robot.commands.Limelight.AprilTag3;
|
|
import frc.robot.commands.Limelight.AprilTag3G;
|
|
import frc.robot.commands.Limelight.Forme3;
|
|
import frc.robot.commands.Pince.AlgueExpire;
|
|
import frc.robot.commands.Pince.Algue_inspire;
|
|
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.BalayeuseAlgue;
|
|
import frc.robot.commands.requin.BalayeuseCoral;
|
|
import frc.robot.commands.requin.BalayeuseHaut;
|
|
import frc.robot.commands.requin.ExpireCorail;
|
|
import frc.robot.commands.requin.L1Requin;
|
|
import frc.robot.subsystems.Bougie;
|
|
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
|
import frc.robot.subsystems.Elevateur;
|
|
import frc.robot.subsystems.Limelight3;
|
|
import frc.robot.subsystems.Limelight3G;
|
|
import frc.robot.subsystems.Pince;
|
|
import frc.robot.subsystems.Requin;
|
|
import frc.robot.commands.requin.exspire;
|
|
import frc.robot.commands.Pince.DepartPince;
|
|
import frc.robot.commands.Elevateur.balonL2;
|
|
import frc.robot.commands.Elevateur.balonL3;
|
|
|
|
public class RobotContainer {
|
|
ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto", BuiltInLayouts.kList)
|
|
.withSize(2, 2).withProperties(Map.of("Label position", "LEFT"));
|
|
GenericEntry L1 = layoutauto.add("choix L1",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
|
|
GenericEntry sortirAngle = layoutauto.add("Cote?",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
|
|
GenericEntry Reculer = layoutauto.add("Reculer",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
|
|
GenericEntry L4 = layoutauto.add("L4",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
|
|
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.RobotCentric drive = new SwerveRequest.RobotCentric()
|
|
.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 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();
|
|
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,manette2::getRightY);
|
|
Bougie bougie = new Bougie();
|
|
Limelight3G limelight3g = new Limelight3G();
|
|
Limelight3 limelight3 = new Limelight3();
|
|
Pose2d pose = new Pose2d();
|
|
Requin requin = new Requin();
|
|
CorailAspir corailAspir = new CorailAspir(pince, bougie);
|
|
public RobotContainer() {
|
|
CameraServer.startAutomaticCapture();
|
|
configureBindings();
|
|
}
|
|
|
|
private void configureBindings() {
|
|
drivetrain.registerTelemetry(logger::telemeterize);
|
|
drivetrain.setDefaultCommand(
|
|
// Drivetrain will execute this command periodically
|
|
drivetrain.applyRequest(() ->
|
|
drive.withVelocityY(MathUtil.applyDeadband(-manette1.getLeftX()*-manette1.getLeftX()*-manette1.getLeftX()*MaxSpeed, 0.05)) // Drive forward with negative Y (forward)
|
|
.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftY()*-manette1.getLeftY()*-manette1.getLeftY()*MaxSpeed, 0.05)) // Drive left with negative X (left)
|
|
.withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*-manette1.getRightX()*-manette1.getRightX()*MaxAngularRate, 0.05)) // Drive counterclockwise with negative X (left)
|
|
)
|
|
);
|
|
|
|
/* Manette 1 */
|
|
//pince
|
|
manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
|
|
manette1.rightBumper().toggleOnTrue(new StationPince(pince, elevateur,bougie));
|
|
manette1.povLeft().whileTrue(new AlgueExpire(pince, bougie));
|
|
manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
|
manette1.povRight().whileTrue(new CoralExpire(pince, bougie));
|
|
manette1.leftTrigger().whileTrue(new DepartPince(pince));
|
|
manette1.povDown().whileTrue(new Algue_inspire(pince,bougie));
|
|
manette1.leftStick().whileTrue(new CorailAspir(pince, bougie));
|
|
//elevateur
|
|
manette1.a().toggleOnTrue(new Depart(elevateur, pince));
|
|
manette1.b().toggleOnTrue(new L2(elevateur,pince));
|
|
manette1.x().toggleOnTrue(new L3(elevateur, pince));
|
|
manette1.y().toggleOnTrue(new L4(elevateur, pince));
|
|
manette1.povUp().toggleOnTrue(new balonL2(elevateur));
|
|
manette1.start().toggleOnTrue(new balonL3(elevateur));
|
|
|
|
|
|
/* Manette 2 */
|
|
//requin
|
|
manette2.rightBumper().whileTrue(new BalayeuseAlgue(requin,bougie));
|
|
manette2.leftBumper().whileTrue(new L1Requin(requin, bougie));
|
|
manette2.rightTrigger().whileTrue(new BalayeuseHaut(requin));
|
|
manette2.leftTrigger().whileTrue(new BalayeuseCoral(requin, bougie));
|
|
manette2.y().whileTrue(new exspire(requin, bougie));
|
|
manette2.x().whileTrue(new ExpireCorail(requin, bougie));
|
|
|
|
//limelight
|
|
manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
|
|
manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
|
|
|
|
//Pince manuel
|
|
pince.setDefaultCommand(new RunCommand(()->{
|
|
pince.pivote(MathUtil.applyDeadband((manette2.getRightY()*manette2.getRightY()*manette2.getRightY())/3, 0.05));
|
|
}, pince));
|
|
|
|
//Elevateur manuel
|
|
elevateur.setDefaultCommand(new RunCommand(()->{
|
|
elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05));
|
|
}, elevateur));
|
|
|
|
//Reset encodeur
|
|
manette2.start().whileTrue(new reset(elevateur, pince, requin));
|
|
}
|
|
|
|
public Command getAutonomousCommand() {
|
|
return new SequentialCommandGroup(
|
|
drivetrain.applyRequest(()->
|
|
drive.withVelocityX(-0.1*MaxSpeed)
|
|
.withVelocityY(0)
|
|
.withRotationalRate(0)).unless(()->!Reculer.getBoolean(true)).withTimeout(3.5),
|
|
drivetrain.applyRequest(()->
|
|
drive.withVelocityX(0.1*MaxSpeed)
|
|
.withVelocityY(0)
|
|
.withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(3.5),
|
|
drivetrain.applyRequest(()->
|
|
drive.withVelocityX(-0.1*MaxSpeed)
|
|
.withVelocityY(0)
|
|
.withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)).withTimeout(3),
|
|
drivetrain.applyRequest(()->
|
|
drive.withVelocityX(0)
|
|
.withVelocityY(0)
|
|
.withRotationalRate(0)).withTimeout(0.1),
|
|
new L4(elevateur, pince).unless(()->!L4.getBoolean(true)).withTimeout(4),
|
|
drivetrain.applyRequest(()->
|
|
drive.withVelocityX(0.1*MaxSpeed)
|
|
.withVelocityY(0)
|
|
.withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(0.25),
|
|
drivetrain.applyRequest(()->
|
|
drive.withVelocityX(0*MaxSpeed)
|
|
.withVelocityY(0)
|
|
.withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2),
|
|
new CoralExpire(pince, bougie).unless(()->!L4.getBoolean(true)).withTimeout(2),
|
|
new L1Requin(requin, bougie).unless(()-> !L1.getBoolean(true)).withTimeout(2),
|
|
new ExpireCorail(requin, bougie).unless(()->!L1.getBoolean(true)).withTimeout(2),
|
|
new RainBow(bougie));
|
|
}
|
|
} |