diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..b463252 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Launch Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "frc-2021" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e9e7d63..e297623 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,17 +1,36 @@ package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.core.util.TrajectoryBuilder; +import frc.robot.routines.Autonomous; +import frc.robot.routines.Teleoperated; +import frc.robot.routines.Autonomous.RobotPath; +import frc.robot.subsystems.Buffer; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; public class Robot extends TimedRobot { - private Command autonomousCommand; + public static Drivetrain drivetrain; + public static Intake intake; + public static Buffer buffer; + public static Shooter shooter; - private RobotContainer robotContainer; + public static Autonomous autonomous; + public static TrajectoryBuilder trajectoryBuilder; @Override - public void robotInit() { - this.robotContainer = new RobotContainer(); + public void robotInit() { + drivetrain = new Drivetrain(); + intake = new Intake(); + buffer = new Buffer(); + shooter = new Shooter(); + + + trajectoryBuilder = new TrajectoryBuilder(drivetrain, RobotPath.get()); + + Autonomous.getInstance().configOptions(); } @Override @@ -21,7 +40,7 @@ public void robotPeriodic() { @Override public void disabledInit() { - this.robotContainer.reset(); + drivetrain.reset(); } @Override @@ -29,25 +48,26 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { - this.autonomousCommand = this.robotContainer.getAutonomousCommand(); - - if (this.autonomousCommand != null) { - this.autonomousCommand.schedule(); - } + Autonomous.getInstance().init(); } @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + Autonomous.getInstance().periodic(); + } @Override public void teleopInit() { - if (this.autonomousCommand != null) { - this.autonomousCommand.cancel(); - } + Autonomous.getInstance().cancel(); + Teleoperated.getInstance().init(); } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + //its may causing bugs + Teleoperated.getInstance().periodic(); + + } @Override public void testInit() { diff --git a/src/main/java/frc/robot/routines/Autonomous.java b/src/main/java/frc/robot/routines/Autonomous.java new file mode 100644 index 0000000..5cecb33 --- /dev/null +++ b/src/main/java/frc/robot/routines/Autonomous.java @@ -0,0 +1,84 @@ +package frc.robot.routines; + +import static java.util.Objects.isNull; +import static java.util.Objects.nonNull; + +import java.util.Arrays; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +import frc.robot.Robot; +import frc.robot.commands.autons.GalacticA; + +public class Autonomous implements IRoutines { + private static Autonomous autonomous; + + private SendableChooser chooser = new SendableChooser<>(); + private Command automonousCommand; + + public enum RobotPath { + //add all paths here + GALACTIC_A("galacticA"); + // GALACTIC_B("galacticB"), + // SLALOM("slalom"), + // BOUNCE("bounce"), + // BARREL("barrel"); + + public final String fileName; + private RobotPath(String fileName) { + this.fileName = fileName; + } + + public static String[] get() { + var robotPathValues = RobotPath.values(); + var pathsArray = Arrays + .stream(robotPathValues) + .map(p -> p.fileName) + .toArray(String[]::new); + + return pathsArray; + } + } + + private Autonomous() { } + + public static Autonomous getInstance(){ + if (isNull(autonomous)) autonomous = new Autonomous(); + + return autonomous; + } + + @Override + public void init() { + this.configOptions(); + this.automonousCommand = this.getAutonomousCommand(); + + if (nonNull(this.automonousCommand)) this.automonousCommand.schedule(); + } + + @Override + public void periodic() { } + + @Override + public void cancel() { + if (nonNull(this.automonousCommand)) this.automonousCommand.cancel(); + } + + private Command getAutonomousCommand() { + return this.chooser.getSelected(); + } + + public void configOptions() { + this.chooser.setDefaultOption("Galactic A", new GalacticA(Robot.trajectoryBuilder)); + this.chooser.addOption("Galactic A", new GalacticA(Robot.trajectoryBuilder)); + // waiting for merge autonomous classes + // this.chooser.addOption("Galactic B", new GalacticB(Robot.trajectoryBuilder)); + // this.chooser.addOption("Slalom", new Slalom(Robot.trajectoryBuilder)); + // this.chooser.addOption("Bounce", new Bounce(Robot.trajectoryBuilder)); + // this.chooser.addOption("Barrel", new Barrel(Robot.trajectoryBuilder)); + + SmartDashboard.putData("SELECTED AUTONOMOUS", this.chooser); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/routines/IRoutines.java b/src/main/java/frc/robot/routines/IRoutines.java new file mode 100644 index 0000000..5df82e9 --- /dev/null +++ b/src/main/java/frc/robot/routines/IRoutines.java @@ -0,0 +1,7 @@ +package frc.robot.routines; + +public interface IRoutines { + void init(); + void periodic(); + void cancel(); +} diff --git a/src/main/java/frc/robot/routines/Teleoperated.java b/src/main/java/frc/robot/routines/Teleoperated.java new file mode 100644 index 0000000..a341931 --- /dev/null +++ b/src/main/java/frc/robot/routines/Teleoperated.java @@ -0,0 +1,115 @@ +package frc.robot.routines; + +import static java.util.Objects.isNull; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID.Hand; +import edu.wpi.first.wpilibj.XboxController.Axis; +import edu.wpi.first.wpilibj.XboxController.Button; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.core.util.DoubleButton; +import frc.robot.Robot; +import frc.robot.Constants.OIConstants; +import frc.robot.commands.buffer.Feed; +import frc.robot.commands.buffer.SmartFeed; +import frc.robot.commands.drivetrain.AimTarget; +import frc.robot.commands.drivetrain.ArcadeDrive; +import frc.robot.commands.drivetrain.CurvatureDrive; +import frc.robot.commands.intake.CollectPowerCell; +import frc.robot.commands.intake.ReleasePowerCell; +import frc.robot.commands.shooter.ShootPowerCellAngle; +import frc.robot.commands.shooter.ShootPowerCellDefault; + +public class Teleoperated implements IRoutines { + private static Teleoperated teleoperated; + + private XboxController driver = new XboxController(OIConstants.driverControllerPort); + private XboxController operator = new XboxController(OIConstants.operatorControllerPort); + + private Teleoperated() { } + + public static Teleoperated getInstance(){ + if (isNull(teleoperated)) teleoperated = new Teleoperated(); + + return teleoperated; + } + + @Override + public void init() { } + + @Override + public void periodic() { + this.configureButtonBindings(); + } + + @Override + public void cancel() { } + + private void configureButtonBindings() { + this.commandsIntake(); + this.commandsDrivetrain(); + this.commandsBuffer(); + this.commandsShooter(); + } + + private void commandsIntake() { + var buttonBumperLeft = new JoystickButton(this.operator, Button.kBumperLeft.value); + var axisTriggerLeft = new JoystickButton(this.operator, Axis.kLeftTrigger.value); + + buttonBumperLeft + .whileHeld(new CollectPowerCell(Robot.intake)); + + axisTriggerLeft + .whileHeld(new ReleasePowerCell(Robot.intake)); + } + + private void commandsShooter() { + var buttonBumperRight = new JoystickButton(this.operator, Button.kBumperRight.value); + + Trigger isAtSettedVelocity = new Trigger(() -> Robot.shooter.atSettedVelocity()); + + buttonBumperRight + .whileHeld(new ShootPowerCellDefault(Robot.shooter)) + .and(isAtSettedVelocity).whileActiveContinuous(new Feed(Robot.buffer)); + + var doubleButton = new DoubleButton( + this.operator, + Button.kBumperRight.value, + Button.kA.value + ); + + doubleButton.whileActiveContinuous(new ShootPowerCellAngle(Robot.shooter)); + } + + private void commandsDrivetrain() { + var buttonBumperLeft = new JoystickButton(this.driver, Button.kBumperLeft.value); + + buttonBumperLeft.whileHeld( + new CurvatureDrive( + Robot.drivetrain, + () -> this.driver.getY(Hand.kLeft), + () -> this.driver.getX(Hand.kRight) + ) + ); + + Robot.drivetrain.setDefaultCommand( + new ArcadeDrive( + Robot.drivetrain, + () -> this.driver.getY(Hand.kLeft), + () -> this.driver.getX(Hand.kRight) + ) + ); + + var buttonBumperRight = new JoystickButton(this.driver, Button.kBumperRight.value); + + buttonBumperRight + .whileHeld(new AimTarget(Robot.drivetrain)); + } + + private void commandsBuffer(){ + Robot.buffer.setDefaultCommand( + new SmartFeed(Robot.buffer) + ); + } +} \ No newline at end of file