diff --git a/Hoops/src/main/java/org/firstinspires/ftc/hoops/Robot.java b/Hoops/src/main/java/org/firstinspires/ftc/hoops/Robot.java index 1037448d..3207009d 100644 --- a/Hoops/src/main/java/org/firstinspires/ftc/hoops/Robot.java +++ b/Hoops/src/main/java/org/firstinspires/ftc/hoops/Robot.java @@ -4,6 +4,8 @@ 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 { @@ -11,6 +13,8 @@ public class Robot implements Loggable { 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); + } } } diff --git a/Hoops/src/main/java/org/firstinspires/ftc/hoops/Setup.java b/Hoops/src/main/java/org/firstinspires/ftc/hoops/Setup.java index 5aa49a8c..3725f060 100644 --- a/Hoops/src/main/java/org/firstinspires/ftc/hoops/Setup.java +++ b/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 diff --git a/Hoops/src/main/java/org/firstinspires/ftc/hoops/commands/IntakeAndLaunchCommand.java b/Hoops/src/main/java/org/firstinspires/ftc/hoops/commands/IntakeAndLaunchCommand.java new file mode 100644 index 00000000..dae4b477 --- /dev/null +++ b/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) + ); + } +} diff --git a/Hoops/src/main/java/org/firstinspires/ftc/hoops/commands/IntakeCommands.java b/Hoops/src/main/java/org/firstinspires/ftc/hoops/commands/IntakeCommands.java new file mode 100644 index 00000000..111ab23a --- /dev/null +++ b/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); + } +} diff --git a/Hoops/src/main/java/org/firstinspires/ftc/hoops/commands/LaunchCommands.java b/Hoops/src/main/java/org/firstinspires/ftc/hoops/commands/LaunchCommands.java new file mode 100644 index 00000000..096380c4 --- /dev/null +++ b/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); + } +} diff --git a/Hoops/src/main/java/org/firstinspires/ftc/hoops/controllers/AllinOneController.java b/Hoops/src/main/java/org/firstinspires/ftc/hoops/controllers/AllinOneController.java new file mode 100644 index 00000000..5e251af3 --- /dev/null +++ b/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)); + } +} diff --git a/Hoops/src/main/java/org/firstinspires/ftc/hoops/controllers/OperatorController.java b/Hoops/src/main/java/org/firstinspires/ftc/hoops/controllers/OperatorController.java new file mode 100644 index 00000000..92b81ebe --- /dev/null +++ b/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)); + } +} diff --git a/Hoops/src/main/java/org/firstinspires/ftc/hoops/opmodes/tele/AllInOneTele.java b/Hoops/src/main/java/org/firstinspires/ftc/hoops/opmodes/tele/AllInOneTele.java new file mode 100644 index 00000000..8bac0dd1 --- /dev/null +++ b/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 + ); + } + } +} diff --git a/Hoops/src/main/java/org/firstinspires/ftc/hoops/opmodes/tele/LaunchTest.java b/Hoops/src/main/java/org/firstinspires/ftc/hoops/opmodes/tele/LaunchTest.java new file mode 100644 index 00000000..903ffae8 --- /dev/null +++ b/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); + } +} diff --git a/Hoops/src/main/java/org/firstinspires/ftc/hoops/opmodes/tele/TwoControllers.java b/Hoops/src/main/java/org/firstinspires/ftc/hoops/opmodes/tele/TwoControllers.java new file mode 100644 index 00000000..6221a182 --- /dev/null +++ b/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 + ); + } + } +} diff --git a/Hoops/src/main/java/org/firstinspires/ftc/hoops/subsystems/IntakeSubsystem.java b/Hoops/src/main/java/org/firstinspires/ftc/hoops/subsystems/IntakeSubsystem.java index faf31d26..67fcd811 100644 --- a/Hoops/src/main/java/org/firstinspires/ftc/hoops/subsystems/IntakeSubsystem.java +++ b/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); } diff --git a/Hoops/src/main/java/org/firstinspires/ftc/hoops/subsystems/LauncherSubsystem.java b/Hoops/src/main/java/org/firstinspires/ftc/hoops/subsystems/LauncherSubsystem.java index 033312bf..70d097d4 100644 --- a/Hoops/src/main/java/org/firstinspires/ftc/hoops/subsystems/LauncherSubsystem.java +++ b/Hoops/src/main/java/org/firstinspires/ftc/hoops/subsystems/LauncherSubsystem.java @@ -10,9 +10,9 @@ @Config public class LauncherSubsystem { - public static double MAX_MOTOR_VELOCITY = 1.0; + public static double MAX_MOTOR_VELOCITY = 0.25; // 0.5 1.0 - public static double MIN_MOTOR_VELOCITY = 0.25; + public static double MIN_MOTOR_VELOCITY = 0.075; // 0.15 0.25 boolean hasHardware; EncodedMotor top; @@ -24,8 +24,8 @@ public LauncherSubsystem(Hardware h) { hasHardware = true; top = h.topLaunch; bottom = h.bottomLaunch; - bottom.setDirection(DcMotorSimple.Direction.REVERSE); - top.setDirection(DcMotorSimple.Direction.FORWARD); + bottom.setDirection(DcMotorSimple.Direction.FORWARD); + top.setDirection(DcMotorSimple.Direction.REVERSE); bottom.coast(); top.coast(); } else { @@ -46,8 +46,8 @@ public void Launch(double angleInDegrees) { top.setVelocity(MIN_MOTOR_VELOCITY); bottom.setVelocity(MAX_MOTOR_VELOCITY); } else { - top.setVelocity(MAX_MOTOR_VELOCITY); - bottom.setVelocity(MAX_MOTOR_VELOCITY); + top.setVelocity(MAX_MOTOR_VELOCITY); + bottom.setVelocity(MAX_MOTOR_VELOCITY); } } }