120 lines
4.7 KiB
Java
120 lines
4.7 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 com.pathplanner.lib.auto.AutoBuilder;
|
|
import com.pathplanner.lib.auto.NamedCommands;
|
|
import edu.wpi.first.cameraserver.CameraServer;
|
|
import edu.wpi.first.math.MathUtil;
|
|
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
|
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
|
import edu.wpi.first.wpilibj2.command.RunCommand;
|
|
|
|
// Manettes
|
|
import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
|
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
|
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
|
// Commands
|
|
import frc.robot.command.Balayer;
|
|
import frc.robot.command.GrimpeurDroit;
|
|
import frc.robot.command.GrimpeurGauche;
|
|
import frc.robot.command.GuiderBas;
|
|
import frc.robot.command.GuiderHaut;
|
|
import frc.robot.command.Lancer;
|
|
import frc.robot.command.LancerAmp;
|
|
import frc.robot.command.LancerNote;
|
|
import frc.robot.command.Lancerampli;
|
|
import frc.robot.command.Limelight_tracker;
|
|
import frc.robot.command.PistonFerme;
|
|
import frc.robot.command.PistonOuvre;
|
|
// Subsystems
|
|
import frc.robot.subsystem.Accumulateur;
|
|
import frc.robot.subsystem.Balayeuse;
|
|
import frc.robot.subsystem.Drive;
|
|
import frc.robot.subsystem.Grimpeur;
|
|
import frc.robot.subsystem.Guideur;
|
|
import frc.robot.subsystem.Lanceur;
|
|
import frc.robot.subsystem.Limelight;
|
|
|
|
|
|
public class RobotContainer {
|
|
ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
|
|
private final SendableChooser<Command> autoChooser;
|
|
Drive drive = new Drive();
|
|
Accumulateur accumulateur = new Accumulateur();
|
|
Balayeuse balayeuse = new Balayeuse();
|
|
Grimpeur grimpeur = new Grimpeur();
|
|
Guideur guideur = new Guideur();
|
|
Lanceur lanceur = new Lanceur();
|
|
Limelight limelight = new Limelight();
|
|
|
|
//joystick
|
|
CommandJoystick joystick = new CommandJoystick(0);
|
|
CommandXboxController manette = new CommandXboxController(1);
|
|
|
|
//command
|
|
PistonFerme pistonFerme = new PistonFerme(grimpeur);
|
|
Balayer balayer = new Balayer(balayeuse, accumulateur);
|
|
GuiderHaut guiderHaut = new GuiderHaut(guideur);
|
|
GuiderBas guiderBas = new GuiderBas(guideur);
|
|
Lancer lancer = new Lancer(lanceur,limelight);
|
|
LancerNote lancernote = new LancerNote(lanceur, accumulateur);
|
|
Lancerampli lancerampli = new Lancerampli(lanceur,limelight);
|
|
GrimpeurDroit grimpeurDroit = new GrimpeurDroit(grimpeur, manette::getLeftY);
|
|
GrimpeurGauche grimpeurGauche = new GrimpeurGauche(grimpeur, manette::getRightY);
|
|
public RobotContainer() {
|
|
|
|
dashboard.addCamera("limelight", "limelight","limelight.local:5800")
|
|
.withSize(3,4)
|
|
.withPosition(7,0);
|
|
|
|
dashboard.addCamera("USB Camera 0", "USB Camera 0","http://roborio-5618-frc.local:1181/?action=stream")
|
|
.withSize(4, 4)
|
|
.withPosition(3, 0);
|
|
|
|
CameraServer.startAutomaticCapture();
|
|
|
|
NamedCommands.registerCommand("balayer",new Balayer(balayeuse, accumulateur));
|
|
NamedCommands.registerCommand("lancer", new LancerNote(lanceur, accumulateur));
|
|
NamedCommands.registerCommand("piston", new PistonFerme(grimpeur));
|
|
autoChooser = AutoBuilder.buildAutoChooser();
|
|
|
|
manette.a().whileTrue(new GuiderBas(guideur));
|
|
manette.b().whileTrue(new GuiderHaut(guideur));
|
|
manette.x().whileTrue(new PistonFerme(grimpeur));
|
|
manette.y().whileTrue(new PistonOuvre(grimpeur));
|
|
joystick.button(3).whileTrue(new Balayer(balayeuse, accumulateur));
|
|
joystick.button(1).whileTrue(new LancerNote(lanceur, accumulateur));
|
|
joystick.button(5).whileTrue(new LancerAmp(lanceur, accumulateur));
|
|
joystick.button(4).whileTrue(new ParallelCommandGroup(new Lancer(lanceur,limelight),new Limelight_tracker(limelight,drive)));
|
|
joystick.button(2).whileTrue(new ParallelCommandGroup(new Lancerampli(lanceur,limelight),new Limelight_tracker(limelight,drive)));
|
|
|
|
// deplacement
|
|
configureBindings();
|
|
drive.setDefaultCommand(new RunCommand(()->{
|
|
drive.drive(MathUtil.applyDeadband(joystick.getY(),0.2), MathUtil.applyDeadband(joystick.getX(),0.2), MathUtil.applyDeadband(joystick.getZ(), 0.2));
|
|
},drive));
|
|
|
|
// grimpeur manuel
|
|
grimpeur.setDefaultCommand(new RunCommand(()->{
|
|
grimpeur.droit(MathUtil.applyDeadband(-manette.getLeftY(), 0.2));
|
|
grimpeur.gauche(MathUtil.applyDeadband(-manette.getRightY(),0.2 ));}
|
|
,grimpeur));
|
|
|
|
dashboard.add("autochooser",autoChooser)
|
|
.withSize(2,1)
|
|
.withPosition(1,1);
|
|
}
|
|
|
|
private void configureBindings() {}
|
|
|
|
public Command getAutonomousCommand(){
|
|
return autoChooser.getSelected();
|
|
}
|
|
}
|