|
|
|
@ -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,10 +89,23 @@ 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
|
|
|
|
|
|
|
|
|
|
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)
|
|
|
|
|
)
|
|
|
|
|
);
|
|
|
|
|
drivetrain.registerTelemetry(logger::telemeterize);
|
|
|
|
|
elevateur.setDefaultCommand(new RunCommand(()->{
|
|
|
|
|
elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.2));
|
|
|
|
|
}, elevateur));
|
|
|
|
@ -96,23 +115,10 @@ public class RobotContainer {
|
|
|
|
|
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)
|
|
|
|
|
)
|
|
|
|
|
);
|
|
|
|
|
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));
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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;
|
|
|
|
|