Hoops bot progress

Hoops/src/main/java/org/firstinspires/ftc/hoops/Robot.java

@@ -4,13 +4,17 @@
 import com.technototes.library.util.Alliance;
 import org.firstinspires.ftc.hoops.helpers.StartingPosition;
 import org.firstinspires.ftc.hoops.subsystems.DrivebaseSubsystem;
+import org.firstinspires.ftc.hoops.subsystems.IntakeSubsystem;
+import org.firstinspires.ftc.hoops.subsystems.LauncherSubsystem;
 
 public class Robot implements Loggable {
 
     public StartingPosition position;
     public Alliance alliance;
     public double initialVoltage;
     public DrivebaseSubsystem drivebaseSubsystem;
+    public LauncherSubsystem launcherSubsystem;
+    public IntakeSubsystem intakeSubsystem;
 
     public Robot(Hardware hw) {
         this.initialVoltage = hw.voltage();
@@ -23,5 +27,11 @@ public Robot(Hardware hw) {
                 hw.imu
             );
         }
+        if (Setup.Connected.LAUNCHER) {
+            this.launcherSubsystem = new LauncherSubsystem(hw);
+        }
+        if (Setup.Connected.INTAKE) {
+            this.intakeSubsystem = new IntakeSubsystem(hw);
+        }
     }
 }

Hoops/src/main/java/org/firstinspires/ftc/hoops/Setup.java

@@ -7,10 +7,10 @@ public class Setup {
     @Config
     public static class Connected {
 
-        public static boolean DRIVEBASE = true;
-        public static boolean EXTERNAL_IMU = true;
+        public static boolean DRIVEBASE = false;
+        public static boolean EXTERNAL_IMU = false;
         public static boolean LAUNCHER = true;
-        public static boolean INTAKE = true;
+        public static boolean INTAKE = false;
     }
 
     @Config

Hoops/src/main/java/org/firstinspires/ftc/hoops/commands/IntakeAndLaunchCommand.java

@@ -0,0 +1,25 @@
+package org.firstinspires.ftc.hoops.commands;
+
+import com.technototes.library.command.Command;
+import com.technototes.library.command.ParallelCommandGroup;
+import com.technototes.library.command.SequentialCommandGroup;
+import com.technototes.library.command.WaitCommand;
+import org.firstinspires.ftc.hoops.Robot;
+import org.firstinspires.ftc.hoops.subsystems.IntakeSubsystem;
+
+public class IntakeAndLaunchCommand {
+
+    public static ParallelCommandGroup IntakeAndLaunch(Robot r) {
+        return new ParallelCommandGroup(
+            IntakeCommands.intakeCommand(r),
+            LaunchCommands.launchCommand(r)
+        );
+    }
+
+    public static ParallelCommandGroup IntakeAndLaunchStop(Robot r) {
+        return new ParallelCommandGroup(
+            IntakeCommands.stopIntakeCommand(r),
+            LaunchCommands.stopLaunchCommand(r)
+        );
+    }
+}

Hoops/src/main/java/org/firstinspires/ftc/hoops/commands/IntakeCommands.java

@@ -0,0 +1,16 @@
+package org.firstinspires.ftc.hoops.commands;
+
+import com.technototes.library.command.Command;
+
+import org.firstinspires.ftc.hoops.Robot;
+
+public class IntakeCommands {
+
+    public static Command intakeCommand(Robot r) {
+        return Command.create(r.intakeSubsystem::Intake);
+    }
+
+    public static Command stopIntakeCommand(Robot r) {
+        return Command.create(r.intakeSubsystem::Stop);
+    }
+}

Hoops/src/main/java/org/firstinspires/ftc/hoops/commands/LaunchCommands.java

@@ -0,0 +1,17 @@
+package org.firstinspires.ftc.hoops.commands;
+
+import com.technototes.library.command.Command;
+import com.technototes.library.command.MethodCommand;
+import org.firstinspires.ftc.hoops.Robot;
+import org.firstinspires.ftc.hoops.subsystems.LauncherSubsystem;
+
+public class LaunchCommands {
+
+    public static Command launchCommand(Robot r) {
+        return Command.create(r.launcherSubsystem::Launch, 45.0);
+    }
+
+    public static Command stopLaunchCommand(Robot r) {
+        return Command.create(r.launcherSubsystem::Stop);
+    }
+}

Hoops/src/main/java/org/firstinspires/ftc/hoops/controllers/AllinOneController.java

@@ -0,0 +1,85 @@
+package org.firstinspires.ftc.hoops.controllers;
+
+import com.technototes.library.command.CommandScheduler;
+import com.technototes.library.control.CommandAxis;
+import com.technototes.library.control.CommandButton;
+import com.technototes.library.control.CommandGamepad;
+import com.technototes.library.control.Stick;
+import org.firstinspires.ftc.hoops.Robot;
+import org.firstinspires.ftc.hoops.Setup;
+import org.firstinspires.ftc.hoops.commands.EZCmd;
+import org.firstinspires.ftc.hoops.commands.IntakeAndLaunchCommand;
+import org.firstinspires.ftc.hoops.commands.IntakeCommands;
+import org.firstinspires.ftc.hoops.commands.JoystickDriveCommand;
+import org.firstinspires.ftc.hoops.commands.LaunchCommands;
+
+public class AllinOneController {
+
+    public Robot robot;
+    public CommandGamepad gamepad;
+
+    public Stick driveLeftStick, driveRightStick;
+    public CommandButton resetGyroButton, turboButton, snailButton;
+    public CommandButton override;
+    public CommandAxis driveStraighten;
+    public CommandButton launch;
+    public CommandButton intake;
+    public CommandButton intakeandlaunch;
+
+    public AllinOneController(CommandGamepad g, Robot r) {
+        this.robot = r;
+        gamepad = g;
+
+        AssignNamedControllerButton();
+        if (Setup.Connected.DRIVEBASE) {
+            bindDriveControls();
+        }
+        if (Setup.Connected.LAUNCHER) {
+            bindLaunchControls();
+        }
+        if (Setup.Connected.INTAKE) {
+            bindIntakeControls();
+        }
+    }
+
+    private void AssignNamedControllerButton() {
+        resetGyroButton = gamepad.ps_options;
+        driveLeftStick = gamepad.leftStick;
+        driveRightStick = gamepad.rightStick;
+        driveStraighten = gamepad.rightTrigger;
+        turboButton = gamepad.leftBumper;
+        snailButton = gamepad.rightBumper;
+        launch = gamepad.ps_square;
+        intake = gamepad.ps_circle;
+        intakeandlaunch = gamepad.ps_triangle;
+    }
+
+    public void bindDriveControls() {
+        CommandScheduler.scheduleJoystick(
+            new JoystickDriveCommand(
+                robot.drivebaseSubsystem,
+                driveLeftStick,
+                driveRightStick,
+                driveStraighten
+            )
+        );
+        turboButton.whenPressed(EZCmd.Drive.TurboMode(robot.drivebaseSubsystem));
+        turboButton.whenReleased(EZCmd.Drive.NormalMode(robot.drivebaseSubsystem));
+        snailButton.whenPressed(EZCmd.Drive.SnailMode(robot.drivebaseSubsystem));
+        snailButton.whenReleased(EZCmd.Drive.NormalMode(robot.drivebaseSubsystem));
+
+        resetGyroButton.whenPressed(EZCmd.Drive.ResetGyro(robot.drivebaseSubsystem));
+    }
+
+    public void bindLaunchControls() {
+        launch.whenPressed(LaunchCommands.launchCommand(robot));
+        launch.whenReleased(LaunchCommands.stopLaunchCommand(robot));
+        intakeandlaunch.whenPressed(IntakeAndLaunchCommand.IntakeAndLaunch(robot));
+        intakeandlaunch.whenReleased(IntakeAndLaunchCommand.IntakeAndLaunchStop(robot));
+    }
+
+    public void bindIntakeControls() {
+        intake.whenPressed(IntakeCommands.intakeCommand(robot));
+        intake.whenReleased(IntakeCommands.stopIntakeCommand(robot));
+    }
+}

Hoops/src/main/java/org/firstinspires/ftc/hoops/controllers/OperatorController.java

@@ -0,0 +1,55 @@
+package org.firstinspires.ftc.hoops.controllers;
+
+import com.technototes.library.command.CommandScheduler;
+import com.technototes.library.control.CommandAxis;
+import com.technototes.library.control.CommandButton;
+import com.technototes.library.control.CommandGamepad;
+import com.technototes.library.control.Stick;
+import org.firstinspires.ftc.hoops.Robot;
+import org.firstinspires.ftc.hoops.Setup;
+import org.firstinspires.ftc.hoops.commands.EZCmd;
+import org.firstinspires.ftc.hoops.commands.IntakeAndLaunchCommand;
+import org.firstinspires.ftc.hoops.commands.IntakeCommands;
+import org.firstinspires.ftc.hoops.commands.JoystickDriveCommand;
+import org.firstinspires.ftc.hoops.commands.LaunchCommands;
+
+public class OperatorController {
+
+    public Robot robot;
+    public CommandGamepad gamepad;
+
+    public CommandButton launch;
+    public CommandButton intake;
+    public CommandButton intakeandlaunch;
+
+    public OperatorController(CommandGamepad g, Robot r) {
+        this.robot = r;
+        gamepad = g;
+
+        AssignNamedControllerButton();
+        if (Setup.Connected.LAUNCHER) {
+            bindLaunchControls();
+        }
+        if (Setup.Connected.INTAKE) {
+            bindIntakeControls();
+        }
+    }
+
+    private void AssignNamedControllerButton() {
+        launch = gamepad.ps_square;
+        intake = gamepad.ps_circle;
+        intakeandlaunch = gamepad.ps_triangle;
+    }
+
+    public void bindLaunchControls() {
+        launch.whenPressed(LaunchCommands.launchCommand(robot));
+        launch.whenReleased(LaunchCommands.stopLaunchCommand(robot));
+        intakeandlaunch.whenPressed(IntakeAndLaunchCommand.IntakeAndLaunch(robot));
+        intakeandlaunch.whenReleased(IntakeAndLaunchCommand.IntakeAndLaunchStop(robot));
+    }
+
+    public void bindIntakeControls() {
+        intake.whenPressed(IntakeCommands.intakeCommand(robot));
+        intake.whenReleased(IntakeCommands.stopIntakeCommand(robot));
+    }
+}

Hoops/src/main/java/org/firstinspires/ftc/hoops/opmodes/tele/AllInOneTele.java

@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.hoops.opmodes.tele;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.technototes.library.command.CommandScheduler;
+import com.technototes.library.structure.CommandOpMode;
+
+import org.firstinspires.ftc.hoops.Hardware;
+import org.firstinspires.ftc.hoops.Robot;
+import org.firstinspires.ftc.hoops.Setup;
+import org.firstinspires.ftc.hoops.commands.EZCmd;
+
+@TeleOp(name = "AllInOneTele")
+@SuppressWarnings("unused")
+public class AllInOneTele extends CommandOpMode {
+
+    public Robot robot;
+    public org.firstinspires.ftc.hoops.controllers.AllinOneController Allcontrols;
+
+    public Hardware hardware;
+
+    @Override
+    public void uponInit() {
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        hardware = new Hardware(hardwareMap);
+        robot = new Robot(hardware);
+        if (Setup.Connected.DRIVEBASE) {
+            Allcontrols = new org.firstinspires.ftc.hoops.controllers.AllinOneController(
+                driverGamepad,
+                robot
+            );
+
+            CommandScheduler.scheduleForState(
+                EZCmd.Drive.ResetGyro(robot.drivebaseSubsystem),
+                OpModeState.INIT
+            );
+        }
+    }
+}

Hoops/src/main/java/org/firstinspires/ftc/hoops/opmodes/tele/LaunchTest.java

@@ -0,0 +1,30 @@
+package org.firstinspires.ftc.hoops.opmodes.tele;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.technototes.library.logger.Loggable;
+import com.technototes.library.structure.CommandOpMode;
+import org.firstinspires.ftc.hoops.Hardware;
+import org.firstinspires.ftc.hoops.Robot;
+import org.firstinspires.ftc.hoops.controllers.AllinOneController;
+import org.firstinspires.ftc.hoops.controllers.DriverController;
+import org.firstinspires.ftc.hoops.controllers.OperatorController;
+
+@TeleOp(name = "LaunchTest")
+@SuppressWarnings("unused")
+public class LaunchTest extends CommandOpMode implements Loggable {
+
+    public Hardware hardware;
+    public Robot robot;
+
+    public AllinOneController operator;
+
+    @Override
+    public void uponInit() {
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        hardware = new Hardware(hardwareMap);
+        robot = new Robot(hardware);
+        operator = new AllinOneController(driverGamepad, robot);
+    }
+}

Hoops/src/main/java/org/firstinspires/ftc/hoops/opmodes/tele/TwoControllers.java

@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.hoops.opmodes.tele;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.technototes.library.command.CommandScheduler;
+import com.technototes.library.structure.CommandOpMode;
+
+import org.firstinspires.ftc.hoops.Hardware;
+import org.firstinspires.ftc.hoops.Robot;
+import org.firstinspires.ftc.hoops.Setup;
+import org.firstinspires.ftc.hoops.commands.EZCmd;
+
+@TeleOp(name = "TwoControllers")
+@SuppressWarnings("unused")
+public class TwoControllers extends CommandOpMode {
+
+    public Robot robot;
+    public org.firstinspires.ftc.hoops.controllers.DriverController controlsDriver;
+    public org.firstinspires.ftc.hoops.controllers.OperatorController controlsOperator;
+    public Hardware hardware;
+
+    @Override
+    public void uponInit() {
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        hardware = new Hardware(hardwareMap);
+        robot = new Robot(hardware);
+        controlsOperator = new org.firstinspires.ftc.hoops.controllers.OperatorController(
+            codriverGamepad,
+            robot
+        );
+        if (Setup.Connected.DRIVEBASE) {
+            controlsDriver = new org.firstinspires.ftc.hoops.controllers.DriverController(
+                driverGamepad,
+                robot
+            );
+
+            CommandScheduler.scheduleForState(
+                EZCmd.Drive.ResetGyro(robot.drivebaseSubsystem),
+                OpModeState.INIT
+            );
+        }
+    }
+}

Hoops/src/main/java/org/firstinspires/ftc/hoops/subsystems/IntakeSubsystem.java

@@ -30,7 +30,7 @@ public IntakeSubsystem(Hardware h) {
         }
     }
 
-    public void Intake(double angleInDegrees) {
+    public void Intake() {
         setSlurp(THE_VELOCITY);
     }