107 lines
3.1 KiB
Java
107 lines
3.1 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 edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.Commands;
|
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
|
import frc.robot.subsystems.Climber;
|
|
import frc.robot.subsystems.Drivetrain;
|
|
import frc.robot.subsystems.Hood;
|
|
import frc.robot.subsystems.Indexer;
|
|
import frc.robot.subsystems.IntakePivot;
|
|
import frc.robot.subsystems.IntakeRollers;
|
|
import frc.robot.subsystems.Sensors;
|
|
import frc.robot.subsystems.Shooter;
|
|
|
|
public class RobotContainer {
|
|
private Climber climber;
|
|
private Drivetrain drivetrain;
|
|
private Hood hood;
|
|
private Indexer indexer;
|
|
private IntakePivot intakePivot;
|
|
private IntakeRollers intakeRollers;
|
|
private Shooter shooter;
|
|
private Sensors sensors;
|
|
|
|
private CommandXboxController driver;
|
|
private CommandXboxController operator;
|
|
|
|
public RobotContainer() {
|
|
configureBindings();
|
|
|
|
climber = new Climber();
|
|
drivetrain = new Drivetrain();
|
|
hood = new Hood();
|
|
indexer = new Indexer();
|
|
intakePivot = new IntakePivot();
|
|
intakeRollers = new IntakeRollers();
|
|
shooter = new Shooter();
|
|
sensors = new Sensors();
|
|
|
|
driver = new CommandXboxController(0);
|
|
operator = new CommandXboxController(0);
|
|
}
|
|
|
|
private void configureBindings() {
|
|
//default commands//
|
|
drivetrain.setDefaultCommand(
|
|
drivetrain.driveArcade(
|
|
driver::getLeftY, driver::getRightX
|
|
)
|
|
);
|
|
|
|
climber.setDefaultCommand(climber.stopClimber());
|
|
hood.setDefaultCommand(hood.hoodDown());
|
|
indexer.setDefaultCommand(indexer.stopIndexer());
|
|
intakePivot.setDefaultCommand(intakePivot.intakeUp());
|
|
intakeRollers.setDefaultCommand(intakeRollers.stopIntakeRollers());
|
|
shooter.setDefaultCommand(shooter.stopShooter());
|
|
|
|
//controller bindings//
|
|
driver.a().whileTrue(ampUp());
|
|
driver.a().whileFalse(ampScore());
|
|
driver.leftTrigger().whileTrue(smartIntake());
|
|
|
|
operator.povUp().onTrue(hood.hoodUp());
|
|
operator.povDown().whileTrue(climber.runClimber());
|
|
}
|
|
|
|
public Command getAutonomousCommand() {
|
|
return Commands.print("No autonomous command configured");
|
|
}
|
|
|
|
|
|
//methods for smart actions//
|
|
public Command ampUp() {
|
|
if (!sensors.getHoodBeamBreak()) {
|
|
return hood.hoodDown()
|
|
.alongWith(shooter.runShooter(.2), indexer.runIndexer(.6));
|
|
} else {
|
|
return hood.hoodUp()
|
|
.alongWith(shooter.stopShooter(), indexer.runIndexer(.2));
|
|
}
|
|
}
|
|
|
|
public Command ampScore() {
|
|
if (sensors.getHoodBeamBreak()) {
|
|
return hood.hoodUp()
|
|
.alongWith(shooter.runShooter(1));
|
|
} else {
|
|
return hood.hoodDown()
|
|
.alongWith(shooter.stopShooter());
|
|
}
|
|
}
|
|
|
|
public Command smartIntake() {
|
|
if (!sensors.getIndexerBeamBreak()) {
|
|
return intakePivot.intakeDown()
|
|
.alongWith(intakeRollers.runIntakeRollers(1));
|
|
} else {
|
|
return intakePivot.intakeUp()
|
|
.alongWith(intakeRollers.stopIntakeRollers());
|
|
}
|
|
}
|
|
} |