From a362d2e0047dc8ff668652cc7caf9819578bc1a5 Mon Sep 17 00:00:00 2001 From: Carlos Date: Tue, 12 Nov 2024 23:16:23 -0800 Subject: [PATCH 1/3] Massive upgrade and shift of files --- TeamCode/src/main/java/nbRedAutoV1/Java.java | 8 -- .../ftc/teamcode/CometBotDevAuto.java | 24 ++++ .../ftc/teamcode/PedroConstants.java | 11 +- .../ftc/teamcode/cometbots/BluenbAutov1.java | 4 - .../cometbots/CometBotAutoDevelopment.java | 122 ++++++++++++++++++ .../cometbots/{ => projects}/AsherPathV1.java | 2 +- .../cometbots/{ => projects}/AutoExample.java | 2 +- .../{ => projects}/AutoExampleFour.java | 2 +- .../AutoExampleSeason2025V1.java | 2 +- .../{ => projects}/AutoExampleThree.java | 2 +- .../{ => projects}/AutoExampleTwo.java | 2 +- .../{ => cometbots/projects}/AutoTest.java | 2 +- .../BasicOmniOpMode_Linear.java | 2 +- .../{ => cometbots/projects}/BlueAuto.java | 2 +- .../{ => projects}/BlueBasketAuto.java | 2 +- .../cometbots/{ => projects}/BluebAutoV1.java | 2 +- .../cometbots/projects/BluenbAutov1.java | 4 + .../cometbots/projects}/GeneratedPath.java | 2 + .../{ => cometbots/projects}/RedAuto.java | 2 +- .../projects}/SensorBNO055IMU.java | 3 +- .../projects}/SensorBNO055IMUCalibration.java | 3 +- .../{ => projects}/SensorIMUOrthogonal.java | 2 +- .../cometbots/projects}/bBlueAutoV1.java | 5 +- .../cometbots/projects}/bRedAutoV1.java | 2 +- .../ftc/teamcode/states/FieldStates.java | 18 +++ .../teamcode/subsystem/MotorsSubsystem.java | 105 +++++++++++++++ build.dependencies.gradle | 5 +- 27 files changed, 306 insertions(+), 36 deletions(-) delete mode 100644 TeamCode/src/main/java/nbRedAutoV1/Java.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluenbAutov1.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/{ => projects}/AsherPathV1.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/{ => projects}/AutoExample.java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/{ => projects}/AutoExampleFour.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/{ => projects}/AutoExampleSeason2025V1.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/{ => projects}/AutoExampleThree.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/{ => projects}/AutoExampleTwo.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => cometbots/projects}/AutoTest.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/{ => projects}/BasicOmniOpMode_Linear.java (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => cometbots/projects}/BlueAuto.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/{ => projects}/BlueBasketAuto.java (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/{ => projects}/BluebAutoV1.java (99%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluenbAutov1.java rename TeamCode/src/main/java/{ => org/firstinspires/ftc/teamcode/cometbots/projects}/GeneratedPath.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => cometbots/projects}/RedAuto.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => cometbots/projects}/SensorBNO055IMU.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => cometbots/projects}/SensorBNO055IMUCalibration.java (99%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/{ => projects}/SensorIMUOrthogonal.java (99%) rename TeamCode/src/main/java/{ => org/firstinspires/ftc/teamcode/cometbots/projects}/bBlueAutoV1.java (98%) rename TeamCode/src/main/java/{nbRedAutoV1 => org/firstinspires/ftc/teamcode/cometbots/projects}/bRedAutoV1.java (99%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java diff --git a/TeamCode/src/main/java/nbRedAutoV1/Java.java b/TeamCode/src/main/java/nbRedAutoV1/Java.java deleted file mode 100644 index c8df1e0..0000000 --- a/TeamCode/src/main/java/nbRedAutoV1/Java.java +++ /dev/null @@ -1,8 +0,0 @@ -package nbRedAutoV1; - -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; - -public class Java { -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java new file mode 100644 index 0000000..0f95a8a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.cometbots.CometBotAutoDevelopment; + +@TeleOp(name = "CometBot Auto", 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(); + } + + @Override + public void loop() { + this.runMode.update(); + telemetry.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java index 88bfac0..06f9aa7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -13,9 +13,9 @@ public class PedroConstants { // Robot motor configurations public static final String FRONT_LEFT_MOTOR = "front-left"; - public static final String BACK_LEFT_MOTOR = "back-left"; - public static final String FRONT_RIGHT_MOTOR = "front-right"; - public static final String BACK_RIGHT_MOTOR = "back-right"; + public static final String BACK_LEFT_MOTOR = "back-left"; + public static final String FRONT_RIGHT_MOTOR = "front-right"; + public static final String BACK_RIGHT_MOTOR = "back-right"; // Robot motor direction public static final Direction FRONT_LEFT_MOTOR_DIRECTION = Direction.REVERSE; @@ -23,6 +23,11 @@ public class PedroConstants { public static final Direction FRONT_RIGHT_MOTOR_DIRECTION = Direction.FORWARD; public static final Direction BACK_RIGHT_MOTOR_DIRECTION = Direction.FORWARD; + /* + Motor Max Power + */ + public static final double MAX_POWER = .75; + // Robot IMU configuration public static final String IMU = "imu"; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluenbAutov1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluenbAutov1.java deleted file mode 100644 index 039e835..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluenbAutov1.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.cometbots; - -public class BluenbAutov1 { -} 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 new file mode 100644 index 0000000..ddfaabe --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java @@ -0,0 +1,122 @@ +package org.firstinspires.ftc.teamcode.cometbots; + +import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +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.MotorsSubsystem; + +public class CometBotAutoDevelopment { + + /* + Subsystems + */ + private MotorsSubsystem motors; + + /* + 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; + private HardwareMap hardwareMap; + private double currentPower = MAX_POWER; + + 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 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); + 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); + + this.toFixMotorBlockingIssuer(); + + follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, false); + follower.update(); + this.telemetry.addData("Field State", this.fieldStates.getFieldLocation()); + this.telemetry.addData("Current Power", currentPower); + } + + public void toFixMotorBlockingIssuer() { + if (this.currentGP1.cross && !this.previousGP1.cross) { + fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); + Actions.runBlocking(new SequentialAction( + new SleepAction(2.5), + this.zeroOutPower(), + new SleepAction(2.5), + this.returnToMaxPower(), + new SleepAction(2.5) + )); + fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AsherPathV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AsherPathV1.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AsherPathV1.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AsherPathV1.java index 060548e..eadafc9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AsherPathV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AsherPathV1.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.projects; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExample.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExample.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExample.java index a944187..763aa0e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExample.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.projects; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleFour.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleFour.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleFour.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleFour.java index 64e96f1..296d7c3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleFour.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleFour.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.projects; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleSeason2025V1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleSeason2025V1.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleSeason2025V1.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleSeason2025V1.java index 632804e..a1f6bfc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleSeason2025V1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleSeason2025V1.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.projects; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleThree.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleThree.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleThree.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleThree.java index 60fb0ec..033783e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleThree.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleThree.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.projects; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleTwo.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleTwo.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleTwo.java index 0784e81..038328b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/AutoExampleTwo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoExampleTwo.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.projects; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoTest.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoTest.java index 7f5fea7..37d0916 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/AutoTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.cometbots.projects; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BasicOmniOpMode_Linear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BasicOmniOpMode_Linear.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BasicOmniOpMode_Linear.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BasicOmniOpMode_Linear.java index c726774..285bf5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BasicOmniOpMode_Linear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BasicOmniOpMode_Linear.java @@ -27,7 +27,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.projects; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueAuto.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueAuto.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueAuto.java index 1af2c8e..d9b1c47 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueAuto.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.cometbots.projects; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueBasketAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueBasketAuto.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueBasketAuto.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueBasketAuto.java index 9a7e7a3..782c451 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BlueBasketAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BlueBasketAuto.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.projects; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.acmerobotics.dashboard.FtcDashboard; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluebAutoV1.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluebAutoV1.java index 778c4c8..2275b5d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/BluebAutoV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluebAutoV1.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.projects; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluenbAutov1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluenbAutov1.java new file mode 100644 index 0000000..e5581b8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/BluenbAutov1.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.cometbots.projects; + +public class BluenbAutov1 { +} diff --git a/TeamCode/src/main/java/GeneratedPath.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/GeneratedPath.java similarity index 98% rename from TeamCode/src/main/java/GeneratedPath.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/GeneratedPath.java index 301a70a..dcc3596 100644 --- a/TeamCode/src/main/java/GeneratedPath.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/GeneratedPath.java @@ -1,3 +1,5 @@ +package org.firstinspires.ftc.teamcode.cometbots.projects; + import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/RedAuto.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedAuto.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/RedAuto.java index ef7ea4f..3c9e1c6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/RedAuto.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.cometbots.projects; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMU.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMU.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMU.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMU.java index cc786f3..259989e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMU.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMU.java @@ -27,11 +27,10 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.cometbots.projects; import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMUCalibration.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMUCalibration.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMUCalibration.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMUCalibration.java index 5d79cc6..4e1cef5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMUCalibration.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorBNO055IMUCalibration.java @@ -27,10 +27,9 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.cometbots.projects; import com.qualcomm.hardware.bosch.BNO055IMU; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.ReadWriteFile; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/SensorIMUOrthogonal.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorIMUOrthogonal.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/SensorIMUOrthogonal.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorIMUOrthogonal.java index 6473172..908fdcc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/SensorIMUOrthogonal.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/SensorIMUOrthogonal.java @@ -27,7 +27,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -package org.firstinspires.ftc.teamcode.cometbots; +package org.firstinspires.ftc.teamcode.cometbots.projects; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER; import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_ENCODER_DIRECTION; diff --git a/TeamCode/src/main/java/bBlueAutoV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bBlueAutoV1.java similarity index 98% rename from TeamCode/src/main/java/bBlueAutoV1.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bBlueAutoV1.java index 48f57c4..eb85bcf 100644 --- a/TeamCode/src/main/java/bBlueAutoV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bBlueAutoV1.java @@ -1,15 +1,16 @@ +package org.firstinspires.ftc.teamcode.cometbots.projects; + import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; -import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; -@Autonomous(name = "bBlueAutoV1", group = "V1") +@Autonomous(name = "org.firstinspires.ftc.teamcode.cometbots.projects.bBlueAutoV1", group = "V1") public class bBlueAutoV1 extends OpMode { public Telemetry telemetry; diff --git a/TeamCode/src/main/java/nbRedAutoV1/bRedAutoV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bRedAutoV1.java similarity index 99% rename from TeamCode/src/main/java/nbRedAutoV1/bRedAutoV1.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bRedAutoV1.java index 949d68c..d785128 100644 --- a/TeamCode/src/main/java/nbRedAutoV1/bRedAutoV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/projects/bRedAutoV1.java @@ -1,4 +1,4 @@ -package nbRedAutoV1; +package org.firstinspires.ftc.teamcode.cometbots.projects; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java new file mode 100644 index 0000000..20d6666 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/states/FieldStates.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.teamcode.states; + +public class FieldStates { + + public enum FieldLocation { + BUCKET, SUBMARINE, FLOATING, TRAVELING + } + + private FieldLocation fieldLocation; + + public FieldLocation getFieldLocation() { + return fieldLocation; + } + + public void setFieldLocation(FieldLocation fieldLocation) { + this.fieldLocation = fieldLocation; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java new file mode 100644 index 0000000..77067bb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/MotorsSubsystem.java @@ -0,0 +1,105 @@ +package org.firstinspires.ftc.teamcode.subsystem; + +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_LEFT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.BACK_RIGHT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR; +import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_RIGHT_MOTOR_DIRECTION; +import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Action; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class MotorsSubsystem { + + public HardwareMap hardwareMap; + public Telemetry telemetry; + + public DcMotor frontLeftMotor; + public DcMotor backLeftMotor; + public DcMotor frontRightMotor; + public DcMotor backRightMotor; + + public enum TravelState { + STOPPED, MOVING + } + + public TravelState travelState; + + public double power; + + public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) { + this.hardwareMap = hardwareMap; + this.telemetry = telemetry; + this.power = MAX_POWER; + } + + public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry, double power) { + this.hardwareMap = hardwareMap; + this.telemetry = telemetry; + this.power = power; + } + + public void init() { + frontLeftMotor = hardwareMap.get(DcMotor.class, FRONT_LEFT_MOTOR); + backLeftMotor = hardwareMap.get(DcMotor.class, BACK_LEFT_MOTOR); + frontRightMotor = hardwareMap.get(DcMotor.class, FRONT_RIGHT_MOTOR); + backRightMotor = hardwareMap.get(DcMotor.class, BACK_RIGHT_MOTOR); + + frontLeftMotor.setDirection(FRONT_LEFT_MOTOR_DIRECTION); + backLeftMotor.setDirection(BACK_LEFT_MOTOR_DIRECTION); + frontRightMotor.setDirection(FRONT_RIGHT_MOTOR_DIRECTION); + backRightMotor.setDirection(BACK_RIGHT_MOTOR_DIRECTION); + + frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + this.setState(TravelState.STOPPED); + } + + public void setFrontLeftMotorPower(double power) { + frontLeftMotor.setPower(power); + } + + public void setBackLeftMotorPower(double power) { + backLeftMotor.setPower(power); + } + + public void setFrontRightMotorPower(double power) { + frontRightMotor.setPower(power); + } + + public void setBackRightMotorPower(double power) { + backRightMotor.setPower(power); + } + + + public void setState(TravelState travelState) { + this.travelState = travelState; + } + + public TravelState getState() { + return this.travelState; + } + + public void setPower(DcMotor motor, double power) { + motor.setPower(power); + if (power < 0.05) { + this.setState(TravelState.MOVING); + } else { + this.setState(TravelState.STOPPED); + } + } +} diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 4061db7..08b6a3b 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -16,6 +16,9 @@ dependencies { implementation 'org.firstinspires.ftc:Vision:10.1.0' implementation 'androidx.appcompat:appcompat:1.2.0' - implementation 'com.acmerobotics.dashboard:dashboard:0.4.5' + implementation "com.acmerobotics.roadrunner:ftc:0.1.14" + implementation "com.acmerobotics.roadrunner:core:1.0.0" + implementation "com.acmerobotics.roadrunner:actions:1.0.0" + implementation "com.acmerobotics.dashboard:dashboard:0.4.16" } From 94144780b8e1cb194fcb6b260872b9e85963dece Mon Sep 17 00:00:00 2001 From: Carlos Date: Wed, 13 Nov 2024 09:13:24 -0800 Subject: [PATCH 2/3] Tentative fix for robot/runBlocking problem as per issue #2 --- .../cometbots/CometBotAutoDevelopment.java | 25 +++++++++++++------ 1 file changed, 17 insertions(+), 8 deletions(-) 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 ddfaabe..9ffee06 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 @@ -38,7 +38,6 @@ public class CometBotAutoDevelopment { private Follower follower; private HardwareMap hardwareMap; - private double currentPower = MAX_POWER; public CometBotAutoDevelopment(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) { this.motors = new MotorsSubsystem(hardwareMap, telemetry); @@ -98,25 +97,35 @@ public class CometBotAutoDevelopment { this.previousGP2.copy(currentGP2); this.currentGP2.copy(this.GP2); - this.toFixMotorBlockingIssuer(); + this.toFixMotorBlockingIssueFirstMethod(); + this.toFixMotorBlockingIssueSecondMethod(); follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, false); follower.update(); this.telemetry.addData("Field State", this.fieldStates.getFieldLocation()); - this.telemetry.addData("Current Power", currentPower); } - public void toFixMotorBlockingIssuer() { + public void toFixMotorBlockingIssueFirstMethod() { if (this.currentGP1.cross && !this.previousGP1.cross) { fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET); Actions.runBlocking(new SequentialAction( - new SleepAction(2.5), this.zeroOutPower(), - new SleepAction(2.5), - this.returnToMaxPower(), - new SleepAction(2.5) + new SleepAction(3), + this.returnToMaxPower() )); fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING); } } + + 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(); + } + } } From dc71eb43179f237584f3a12d04e91f10d7895913 Mon Sep 17 00:00:00 2001 From: Carlos Date: Wed, 13 Nov 2024 15:09:59 -0800 Subject: [PATCH 3/3] Feature where driver can override centricity (robot vs field). --- .../teamcode/cometbots/CometBotAutoDevelopment.java | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) 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 9ffee06..43954eb 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 @@ -34,7 +34,9 @@ public class CometBotAutoDevelopment { public Gamepad currentGP2; public Gamepad previousGP2; private Telemetry telemetry; + public FieldStates fieldStates; + private boolean centricity = false; private Follower follower; private HardwareMap hardwareMap; @@ -99,12 +101,21 @@ public class CometBotAutoDevelopment { this.toFixMotorBlockingIssueFirstMethod(); this.toFixMotorBlockingIssueSecondMethod(); + this.changeCentricity(); - follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, false); + follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x, centricity); 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(); + } + } + public void toFixMotorBlockingIssueFirstMethod() { if (this.currentGP1.cross && !this.previousGP1.cross) { fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);