diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java index 0f95a8a..51270ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java @@ -5,20 +5,20 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.cometbots.CometBotAutoDevelopment; -@TeleOp(name = "CometBot Auto", group = "Development") +@TeleOp(name = "CometBot Auto v2", group = "Development") public class CometBotDevAuto extends OpMode { public CometBotAutoDevelopment runMode; @Override public void init() { - this.runMode = new CometBotAutoDevelopment(hardwareMap, telemetry, gamepad1, gamepad2); - this.runMode.init(); + runMode = new CometBotAutoDevelopment(hardwareMap, gamepad1, gamepad2); + runMode.init(); } @Override public void loop() { - this.runMode.update(); + runMode.update(); telemetry.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java index 43954eb..a0e30d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java @@ -15,6 +15,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.states.FieldStates; +import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem; import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem; public class CometBotAutoDevelopment { @@ -22,121 +23,64 @@ public class CometBotAutoDevelopment { /* Subsystems */ - private MotorsSubsystem motors; + private DualMotorSliderSubsystem dualSlides; /* Controllers */ - public Gamepad GP1; - public Gamepad GP2; - public Gamepad currentGP1; - public Gamepad previousGP1; - public Gamepad currentGP2; - public Gamepad previousGP2; - private Telemetry telemetry; - - public FieldStates fieldStates; - private boolean centricity = false; + public Gamepad gamepad1; + public Gamepad gamepad2; + public Gamepad currentGamepad1; + public Gamepad currentGamepad2; + public Gamepad previousGamepad1; + public Gamepad previousGamepad2; private Follower follower; - private HardwareMap hardwareMap; - public CometBotAutoDevelopment(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) { - this.motors = new MotorsSubsystem(hardwareMap, telemetry); - this.GP1 = gp1; - this.GP2 = gp2; - this.hardwareMap = hardwareMap; - this.telemetry = telemetry; - this.currentGP1 = new Gamepad(); - this.currentGP2 = new Gamepad(); - this.previousGP1 = new Gamepad(); - this.previousGP2 = new Gamepad(); - this.fieldStates = new FieldStates(); - this.follower = new Follower(hardwareMap); + public CometBotAutoDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) { + dualSlides = new DualMotorSliderSubsystem(hardwareMap); + this.gamepad1 = gamepad1; + this.gamepad2 = gamepad2; + currentGamepad1 = new Gamepad(); + currentGamepad2 = new Gamepad(); + previousGamepad1 = new Gamepad(); + previousGamepad2 = new Gamepad(); + follower = new Follower(hardwareMap); } - - public class ZeroOutPower implements Action { - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - follower = new Follower(hardwareMap); - follower.setMaxPower(0); - System.out.println("Running ZeroOutPower"); - return follower.isBusy(); - } - } - - public class ReturnToMaxPower implements Action { - - @Override - public boolean run(@NonNull TelemetryPacket telemetryPacket) { - follower = new Follower(hardwareMap); - follower.setMaxPower(MAX_POWER); - follower.startTeleopDrive(); - System.out.println("Running ReturnToMaxPower"); - return follower.isBusy(); - } - } - - public Action zeroOutPower() { - return new ZeroOutPower(); - } - - public Action returnToMaxPower() { - return new ReturnToMaxPower(); - } - + public void init() { - this.motors.init(); - this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); + dualSlides.init(); follower.setMaxPower(MAX_POWER); follower.startTeleopDrive(); } public void update() { - this.previousGP1.copy(currentGP1); - this.currentGP1.copy(this.GP1); - this.previousGP2.copy(currentGP2); - this.currentGP2.copy(this.GP2); + previousGamepad1.copy(currentGamepad1); + currentGamepad1.copy(gamepad1); + previousGamepad2.copy(currentGamepad2); + currentGamepad2.copy(gamepad2); - this.toFixMotorBlockingIssueFirstMethod(); - this.toFixMotorBlockingIssueSecondMethod(); - this.changeCentricity(); + /* + Check if dpad_up/down is being pressed for slides + */ + dualSlides.update(); + dualSlidesToLowBucketPosition(); + dualSlidesToHighBucketPosition(); - follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, centricity); + follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x); follower.update(); - this.telemetry.addData("Field State", this.fieldStates.getFieldLocation()); } - public void changeCentricity() { - if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) { - this.centricity = !centricity; - this.follower.breakFollowing(); - this.follower.startTeleopDrive(); + private void dualSlidesToHighBucketPosition() { + if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) { + dualSlides.toHighBucketPosition(); } } - public void toFixMotorBlockingIssueFirstMethod() { - if (this.currentGP1.cross && !this.previousGP1.cross) { - fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); - Actions.runBlocking(new SequentialAction( - this.zeroOutPower(), - new SleepAction(3), - this.returnToMaxPower() - )); - fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); + private void dualSlidesToLowBucketPosition() { + if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) { + dualSlides.toLowBucketPosition(); } } - public void toFixMotorBlockingIssueSecondMethod() { - if (this.currentGP1.circle && !this.previousGP1.circle) { - this.follower.breakFollowing(); - fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); - Actions.runBlocking(new SequentialAction( - new SleepAction(3) - )); - fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); - this.follower.startTeleopDrive(); - } - } }