New routines

.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
     }
   ]
 }

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,33 +40,34 @@ public void robotPeriodic() {
 
   @Override
   public void disabledInit() {
-    this.robotContainer.reset();
+    drivetrain.reset();
   }
 
   @Override
   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() {

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<Command> 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);
+  }
+}

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();
+}

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)
+    );
+  }
+}