Skip to content

Commit

Permalink
different driver opmodes
Browse files Browse the repository at this point in the history
  • Loading branch information
Bi1ku committed Dec 9, 2024
1 parent dadbe17 commit 54d08f9
Show file tree
Hide file tree
Showing 4 changed files with 195 additions and 33 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -34,23 +34,24 @@

@Config
public class CommandRobot {
public Command ready, accepting, highBasket, highRung, lowBasket, lowRung, liftIncrement, liftDecrement, open, close, slamdown, specimen;
private TeleOpMode mode;

private final MultipleTelemetry telemetry;
private final Lift lift;
private final Extendo extendo;
private final Claw claw;
private final OpModeCore opMode;
public Command ready, accepting, highBasket, highRung, lowBasket, lowRung, liftIncrement, liftDecrement, open, close, slamdown, specimen;

private Drivetrain drivetrain;

private GamepadEx gamepad1;
private GamepadEx gamepad2;
private final OpModeCore opMode;

public static int CLAW_DEPLOY_DELAY = 450;
public static int LIFT_DELAY = 250;

private final boolean isExtended = false;

// TELEOP
public CommandRobot(HardwareMap hwMap, MultipleTelemetry telemetry, Gamepad gamepad1, Gamepad gamepad2, OpModeCore opMode) {
public CommandRobot(HardwareMap hwMap, MultipleTelemetry telemetry, Gamepad gamepad1, Gamepad gamepad2, OpModeCore opMode, TeleOpMode mode) {
this.telemetry = telemetry;

this.drivetrain = new Drivetrain(hwMap, telemetry, new Pose2d(0, 0, 0));
Expand All @@ -62,12 +63,12 @@ public CommandRobot(HardwareMap hwMap, MultipleTelemetry telemetry, Gamepad game
this.gamepad2 = new GamepadEx(gamepad2);

this.opMode = opMode;
this.mode = mode;

this.configureCommands();
this.configureControls();
}

// AUTON
public CommandRobot(HardwareMap hwMap, Pose2d startPose, MultipleTelemetry telemetry, OpModeCore opMode) {
this.telemetry = telemetry;

Expand All @@ -85,9 +86,8 @@ public MecanumDrive getDrive() {
}

public void startThreads() {
this.drivetrain.startThread(this.gamepad1, this.opMode);
this.drivetrain.startThread(this.mode == TeleOpMode.OWEN || this.mode == TeleOpMode.RYAN ? this.gamepad1 : this.gamepad2, this.opMode);
this.lift.startThread(this.opMode);
// TODO: Add in more threads if needed
}

public void configureCommands() {
Expand Down Expand Up @@ -144,7 +144,6 @@ public void configureCommands() {
new ClawOpen(this.telemetry, this.claw)
);


this.liftIncrement = new LiftIncrement(this.telemetry, this.lift);

this.liftDecrement = new LiftDecrement(this.telemetry, this.lift);
Expand All @@ -154,27 +153,95 @@ public void configureCommands() {
this.close = new ClawClose(this.telemetry, this.claw);
}

// TODO: Configure controls for gamepad (talk with drive team)
public void configureControls() {
this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
.whenPressed(this.accepting);
// this.gamepad2.getGamepadButton(GamepadKeys.Button.Y)
// .whenPressed(this.highBasket);
this.gamepad1.getGamepadButton(GamepadKeys.Button.A)
.whenPressed(this.ready);
this.gamepad2.getGamepadButton(GamepadKeys.Button.A)
.whenPressed(this.lowBasket);
this.gamepad2.getGamepadButton(GamepadKeys.Button.DPAD_UP)
.whenPressed(this.liftIncrement);
this.gamepad2.getGamepadButton(GamepadKeys.Button.DPAD_DOWN)
.whenPressed(this.liftDecrement);
this.gamepad2.getGamepadButton(GamepadKeys.Button.X)
.whenPressed(this.lowRung);
this.gamepad2.getGamepadButton(GamepadKeys.Button.B)
.whenPressed(this.highRung);
this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
.whenPressed(this.open);
this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
.whenPressed(this.close);
switch (this.mode) {
/* ------------------------------------- */
/* --------------- OWEN ---------------- */
/* ------------------------------------- */

case OWEN:
this.gamepad1.getGamepadButton(GamepadKeys.Button.A)
.whenPressed(this.ready);
this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
.whenPressed(this.accepting);
this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_UP)
.whenPressed(this.liftIncrement);
this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_DOWN)
.whenPressed(this.liftDecrement);
this.gamepad1.getGamepadButton(GamepadKeys.Button.X)
.whenPressed(this.lowRung);
this.gamepad1.getGamepadButton(GamepadKeys.Button.Y)
.whenPressed(this.highRung);
this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_RIGHT)
.whenPressed(this.highBasket);
this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_LEFT)
.whenPressed(this.lowBasket);
this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
.whenPressed(this.open);
this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
.whenPressed(this.close);
break;

/* ------------------------------------- */
/* --------------- RYAN ---------------- */
/* ------------------------------------- */

case RYAN:
this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
.whenPressed(this.accepting);
this.gamepad1.getGamepadButton(GamepadKeys.Button.Y)
.whenPressed(this.highBasket);
this.gamepad1.getGamepadButton(GamepadKeys.Button.A)
.whenPressed(this.ready);
this.gamepad1.getGamepadButton(GamepadKeys.Button.X)
.whenPressed(this.lowBasket);
this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_UP)
.whenPressed(this.liftIncrement);
this.gamepad1.getGamepadButton(GamepadKeys.Button.DPAD_DOWN)
.whenPressed(this.liftDecrement);
this.gamepad1.getGamepadButton(GamepadKeys.Button.X)
.whenPressed(this.lowRung);
this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
.whenPressed(this.highRung);
this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
.whenPressed(this.open);
this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
.whenPressed(this.close);
break;

/* ------------------------------------- */
/* ------------ RYAN & KELLY ----------- */
/* ------------------------------------- */

case RYAN_KELLY:
this.gamepad1.getGamepadButton(GamepadKeys.Button.B)
.whenPressed(this.accepting);
this.gamepad2.getGamepadButton(GamepadKeys.Button.Y)
.whenPressed(this.highBasket);
this.gamepad1.getGamepadButton(GamepadKeys.Button.A)
.whenPressed(this.ready);
this.gamepad2.getGamepadButton(GamepadKeys.Button.A)
.whenPressed(this.lowBasket);
this.gamepad2.getGamepadButton(GamepadKeys.Button.DPAD_UP)
.whenPressed(this.liftIncrement);
this.gamepad2.getGamepadButton(GamepadKeys.Button.DPAD_DOWN)
.whenPressed(this.liftDecrement);
this.gamepad2.getGamepadButton(GamepadKeys.Button.X)
.whenPressed(this.lowRung);
this.gamepad2.getGamepadButton(GamepadKeys.Button.B)
.whenPressed(this.highRung);
this.gamepad1.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER)
.whenPressed(this.open);
this.gamepad1.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER)
.whenPressed(this.close);

break;
}
}

public enum TeleOpMode {
OWEN,
RYAN,
RYAN_KELLY
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
import org.firstinspires.ftc.teamcode.CommandRobot;
import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;

@TeleOp(name = "Main", group = "TeleOp")
@TeleOp(name = "Ryan & Kelly's Main")
public class Main extends OpModeCore {
private CommandRobot robot;
private MultipleTelemetry multipleTelemetry;
Expand All @@ -21,7 +21,8 @@ public void initialize() {
this.multipleTelemetry,
super.gamepad1,
super.gamepad2,
this
this,
CommandRobot.TeleOpMode.RYAN_KELLY
);
}

Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
package org.firstinspires.ftc.teamcode.opmodes.teleop;

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.command.CommandScheduler;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import org.firstinspires.ftc.teamcode.CommandRobot;
import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;

@TeleOp(name = "Owen's Main")
public class OwenMain extends OpModeCore {
private CommandRobot robot;
private MultipleTelemetry multipleTelemetry;

public void initialize() {
this.multipleTelemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());

this.robot = new CommandRobot(
super.hardwareMap,
this.multipleTelemetry,
super.gamepad1,
super.gamepad2,
this,
CommandRobot.TeleOpMode.OWEN
);
}

@Override
public void runOpMode() {
CommandScheduler.getInstance().enable();

this.initialize();
super.waitForStart();

this.robot.startThreads();
while (!isStopRequested() && opModeIsActive()) {
super.resetCycle();
CommandScheduler.getInstance().run();

super.logCycles();
super.telemetry.update();
}

super.end();
}
}
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
package org.firstinspires.ftc.teamcode.opmodes.teleop;

import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.arcrobotics.ftclib.command.CommandScheduler;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import org.firstinspires.ftc.teamcode.CommandRobot;
import org.firstinspires.ftc.teamcode.utils.commands.OpModeCore;

@TeleOp(name = "Ryan's Main")
public class RyanMain extends OpModeCore {
private CommandRobot robot;
private MultipleTelemetry multipleTelemetry;

public void initialize() {
this.multipleTelemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());

this.robot = new CommandRobot(
super.hardwareMap,
this.multipleTelemetry,
super.gamepad1,
super.gamepad2,
this,
CommandRobot.TeleOpMode.RYAN
);
}

@Override
public void runOpMode() {
CommandScheduler.getInstance().enable();

this.initialize();
super.waitForStart();

this.robot.startThreads();
while (!isStopRequested() && opModeIsActive()) {
super.resetCycle();
CommandScheduler.getInstance().run();

super.logCycles();
super.telemetry.update();
}

super.end();
}
}

0 comments on commit 54d08f9

Please sign in to comment.