From 9106511f2f972ad885c1d5e408859f4e9f96b853 Mon Sep 17 00:00:00 2001 From: Carlos Rivas Date: Sun, 10 Nov 2024 18:40:09 -0800 Subject: [PATCH] Commit competition code --- .../ftc/teamcode/CometBotAuto.java | 2 +- .../ftc/teamcode/CometBotDrive.java | 22 +-- .../runmodes/CometBotTeleopCompetition.java | 185 ++++++++++++++++++ 3 files changed, 195 insertions(+), 14 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java index 1ad67fd..6e5c723 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotAuto.java @@ -7,7 +7,7 @@ import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.runmodes.Auto; -@Autonomous(name = "CometBot Auto", group = "Debug") +@Autonomous(name = "CometBot Auto", group = "Competition") public class CometBotAuto extends OpMode { public Auto auto; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDrive.java index 4411c43..d634583 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDrive.java @@ -1,31 +1,27 @@ package org.firstinspires.ftc.teamcode; -import static org.firstinspires.ftc.teamcode.util.action.FieldConstants.blueBucketStartPose; - import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; -import org.firstinspires.ftc.teamcode.runmodes.Teleop; +import org.firstinspires.ftc.teamcode.runmodes.CometBotTeleopCompetition; +import org.firstinspires.ftc.teamcode.runmodes.DevTeleopRunModeCompetition; -@TeleOp(name="ComeBot Drive", group="Debug") +@TeleOp(name = "ComeBot Drive", group = "Competition") @Disabled public class CometBotDrive extends OpMode { - private Teleop teleop; + public CometBotTeleopCompetition runMode; @Override public void init() { - teleop = new Teleop(hardwareMap, - telemetry, - new Follower(hardwareMap), - gamepad1); - teleop.start(); + this.runMode = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2); + this.runMode.init(); } @Override public void loop() { - teleop.update(); + this.runMode.update(); + telemetry.update(); } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java new file mode 100644 index 0000000..a80893d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/CometBotTeleopCompetition.java @@ -0,0 +1,185 @@ +package org.firstinspires.ftc.teamcode.runmodes; + +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.SleepAction; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.qualcomm.robotcore.hardware.Gamepad; +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.ArmActionsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.ClawActionsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.LiftActionsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem; +import org.firstinspires.ftc.teamcode.subsystem.WristActionsSubsystem; + +public class CometBotTeleopCompetition { + + /* + Subsystems + */ + private MotorsSubsystem motors; + public ClawActionsSubsystem claw; + public ArmActionsSubsystem arm; + public WristActionsSubsystem wrist; + public LiftActionsSubsystem lift; + + /* + 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 Follower follower; + + public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) { + this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55); + this.claw = new ClawActionsSubsystem(hardwareMap); + this.arm = new ArmActionsSubsystem(hardwareMap); + this.wrist = new WristActionsSubsystem(hardwareMap); + this.lift = new LiftActionsSubsystem(hardwareMap); + this.GP1 = gp1; + this.GP2 = gp2; + 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 void init() { + this.motors.init(); + this.claw.init(); + this.arm.init(); + this.wrist.init(); + this.lift.init(); + this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); + follower.setMaxPower(.75); + follower.startTeleopDrive(); + } + + public void update() { + this.previousGP1.copy(currentGP1); + this.currentGP1.copy(this.GP1); + this.previousGP2.copy(currentGP2); + this.currentGP2.copy(this.GP2); + + this.toHighBucketScore(); + this.toLowBucketScore(); + this.toArmParkPosition(); + this.toArmParkThenSwitchBetweenSubmarineAndFloorPosition(); + this.clawControl(); + +// this.motors.calculateTrajectory(this.GP1); + follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x); + follower.update(); + this.telemetry.addData("Field State", this.fieldStates.getFieldLocation()); + this.telemetry.addData("Claw State", this.claw.getState()); + this.telemetry.addData("Claw Position", this.claw.getPosition()); + this.telemetry.addData("Wrist State", this.wrist.getState()); + this.telemetry.addData("Arm State", this.arm.getState()); + this.telemetry.addData("Lift State", this.lift.getState()); + this.telemetry.addData("Lift Position", this.lift.getPosition()); + } + + /* + Controller: 1 + Button: A + Action: On button press, Arm hovers the floor with wrist parallel to arm + */ + public void toHighBucketScore() { + if (this.currentGP1.triangle && !this.previousGP1.triangle) { + fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); + Actions.runBlocking(new SequentialAction( + this.wrist.toFloorPosition(), + this.arm.toParkPosition(), + this.lift.toHighBucketPosition(), + new SleepAction(.5), + this.arm.toBucketPosition(), + new SleepAction(.5), + this.wrist.toBucketPosition(), + new SleepAction(.5), + this.claw.openClaw(), + new SleepAction(.5), + this.wrist.toFloorPosition(), + new SleepAction(.5), + this.arm.toParkPosition(), + this.lift.toFloorPosition() + )); + fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); + } + } + + public void toLowBucketScore() { + if (this.currentGP1.circle && !this.previousGP1.circle) { + fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); + Actions.runBlocking(new SequentialAction( + this.wrist.toFloorPosition(), + this.arm.toParkPosition(), + this.lift.toLowBucketPosition(), + new SleepAction(.5), + this.arm.toBucketPosition(), + new SleepAction(.5), + this.wrist.toBucketPosition(), + new SleepAction(.5), + this.claw.openClaw(), + new SleepAction(.5), + this.wrist.toFloorPosition(), + new SleepAction(.5), + this.arm.toParkPosition(), + this.lift.toFloorPosition() + )); + fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); + } + } + + public void clawControl() { + if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) { + this.claw.switchState(); + } + } + + public void toArmParkPosition() { + if (this.currentGP1.square && !this.previousGP1.square) { + Actions.runBlocking(this.arm.toParkPosition()); + } + } + + public void toArmParkThenSwitchBetweenSubmarineAndFloorPosition() { + if (this.currentGP1.cross && !previousGP1.cross) { + if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) { + Actions.runBlocking( + new SequentialAction( + this.arm.toSubmarinePosition(), + this.wrist.toFloorPosition() + ) + ); + } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE) { + Actions.runBlocking( + new SequentialAction( + this.arm.toFloorPosition(), + this.wrist.toFloorPosition() + ) + ); + } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.FLOOR) { + Actions.runBlocking( + new SequentialAction( + this.arm.toSubmarinePosition(), + this.wrist.toFloorPosition() + ) + ); + } + } + } + +}