diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 000000000000..6a6f413e798f --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,87 @@ +name: CI (Build + Lint + Tests) + +on: + pull_request: + paths: + - "TeamCode/**" + - "FtcRobotController/**" + - "libs/**" + - "gradle/**" + - "build.gradle" + - "build.common.gradle" + - "build.dependencies.gradle" + - "settings.gradle" + - "gradle.properties" + - "gradlew" + - "gradlew.bat" + - ".github/workflows/**" + push: + # Keep this narrow so you don't burn minutes on every branch. + # Add/remove branches to match how your team works. + branches: + - "Quali-1" + paths: + - "TeamCode/**" + - "FtcRobotController/**" + - "libs/**" + - "gradle/**" + - "build.gradle" + - "build.common.gradle" + - "build.dependencies.gradle" + - "settings.gradle" + - "gradle.properties" + - "gradlew" + - "gradlew.bat" + - ".github/workflows/**" + +concurrency: + group: ci-${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +permissions: + contents: read + +jobs: + build_lint_test: + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Validate Gradle Wrapper + uses: gradle/wrapper-validation-action@v3 + + - name: Set up JDK 17 + uses: actions/setup-java@v4 + with: + distribution: temurin + java-version: "17" + + - name: Set up Android SDK + uses: android-actions/setup-android@v3 + + - name: Set up Gradle (with caching) + uses: gradle/actions/setup-gradle@v4 + + - name: Make gradlew executable + run: chmod +x ./gradlew + + - name: Build (Debug) + run: ./gradlew --no-daemon --stacktrace assembleDebug + + - name: Android Lint + run: ./gradlew --no-daemon --stacktrace lint + + - name: Unit tests (if present) + run: ./gradlew --no-daemon --stacktrace test + + - name: Upload reports (lint/tests) + if: always() + uses: actions/upload-artifact@v4 + with: + name: reports + if-no-files-found: warn + path: | + **/build/reports/** + **/build/test-results/** diff --git a/.github/workflows/format-java.yml b/.github/workflows/format-java.yml new file mode 100644 index 000000000000..40c3366e83bc --- /dev/null +++ b/.github/workflows/format-java.yml @@ -0,0 +1,48 @@ +name: Format (TeamCode Java) + +on: + pull_request: + paths: + - "TeamCode/**/*.java" + - ".github/workflows/**" + push: + branches: + - "Quali-1" + paths: + - "TeamCode/**/*.java" + - ".github/workflows/**" + +concurrency: + group: format-${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +permissions: + contents: read + +jobs: + google_java_format: + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Set up JDK 21 + uses: actions/setup-java@v4 + with: + distribution: temurin + java-version: "21" + - name: Show java version + run: java -version + # This action can either “check” or “format then show diff”. + # We choose “format then diff” so the PR shows exactly what would change. + - name: Run google-java-format (TeamCode) + uses: axel-op/googlejavaformat-action@v4 + with: + files: "TeamCode/**/*.java" + args: "--replace" + skip-commit: true + + - name: Fail if formatting changed (prints diff) + run: git --no-pager diff --exit-code + diff --git a/.github/workflows/spotbugs-teamcode.yml b/.github/workflows/spotbugs-teamcode.yml new file mode 100644 index 000000000000..679f4f52e465 --- /dev/null +++ b/.github/workflows/spotbugs-teamcode.yml @@ -0,0 +1,96 @@ +name: SpotBugs (TeamCode) + +on: + pull_request: + paths: + - "TeamCode/**" + - "build.gradle" + - "build.common.gradle" + - "build.dependencies.gradle" + - "settings.gradle" + - "gradle.properties" + - "gradle/**" + - "gradlew" + - "gradlew.bat" + - ".github/workflows/spotbugs-teamcode.yml" + push: + branches: ["Quali-1"] + paths: + - "TeamCode/**" + - "build.gradle" + - "build.common.gradle" + - "build.dependencies.gradle" + - "settings.gradle" + - "gradle.properties" + - "gradle/**" + - "gradlew" + - "gradlew.bat" + - ".github/workflows/spotbugs-teamcode.yml" + +concurrency: + group: spotbugs-${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +permissions: + contents: read + +jobs: + spotbugs_teamcode: + runs-on: ubuntu-latest + + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Validate Gradle Wrapper + uses: gradle/wrapper-validation-action@v3 + + - name: Set up JDK 17 + uses: actions/setup-java@v4 + with: + distribution: temurin + java-version: "17" + + # FTC/Android Gradle builds typically need the Android SDK present even for analysis tasks + - name: Set up Android SDK + uses: android-actions/setup-android@v3 + + - name: Set up Gradle (with caching) + uses: gradle/actions/setup-gradle@v4 + + - name: Make gradlew executable + run: chmod +x ./gradlew + + - name: Run SpotBugs on TeamCode + shell: bash + run: | + set -euo pipefail + + echo "Trying SpotBugs tasks for :TeamCode (Android projects differ by plugin/config)..." + + # Try a few common task names. Use the first one that exists and succeeds. + if ./gradlew -q :TeamCode:tasks --all | grep -qE '(^|\s)spotbugsDebug(\s|$)'; then + ./gradlew --no-daemon --stacktrace :TeamCode:spotbugsDebug + elif ./gradlew -q :TeamCode:tasks --all | grep -qE '(^|\s)spotbugsRelease(\s|$)'; then + ./gradlew --no-daemon --stacktrace :TeamCode:spotbugsRelease + elif ./gradlew -q :TeamCode:tasks --all | grep -qE '(^|\s)spotbugsMain(\s|$)'; then + ./gradlew --no-daemon --stacktrace :TeamCode:spotbugsMain + elif ./gradlew -q :TeamCode:tasks --all | grep -qE '(^|\s)spotbugs(\s|$)'; then + ./gradlew --no-daemon --stacktrace :TeamCode:spotbugs + else + echo "::error::No SpotBugs task found for :TeamCode. Make sure the SpotBugs Gradle plugin is applied/configured for TeamCode." + echo "Here are any spotbugs-related tasks Gradle sees:" + ./gradlew -q :TeamCode:tasks --all | grep -i spotbugs || true + exit 1 + fi + + - name: Upload SpotBugs reports + if: always() + uses: actions/upload-artifact@v4 + with: + name: spotbugs-teamcode-reports + if-no-files-found: warn + path: | + TeamCode/build/reports/spotbugs/** + **/build/reports/spotbugs/** + diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java deleted file mode 100644 index 312d7f5110e3..000000000000 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java +++ /dev/null @@ -1,156 +0,0 @@ -/* - * Copyright (c) 2025 DigitalChickenLabs - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.firstinspires.ftc.robotcontroller.external.samples; - -import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.util.ElapsedTime; - -import org.firstinspires.ftc.robotcore.external.Telemetry; - -/* - * This OpMode illustrates how to use DigitalChickenLabs OctoQuad Quad Encoder & Pulse Width I/F Module - * - * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or - * Pulse-Width Absolute Encoder inputs. Relative Quadrature encoders are found on most FTC motors, - * and some stand-alone position sensors like the REV Thru-Bore encoder. Pulse-Width encoders are - * less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width, - * as can several sonar rangefinders such as the MaxBotix MB1000 series. - * - * Note: SDK 11.0+ requires that the OctoQuad is running firmware V3.0 or greater. - * Visit https://github.com/DigitalChickenLabs/OctoQuad/tree/master/firmware for instruction - * on how to upgrade your OctoQuad's firmware. - * - * This basic sample shows how an OctoQuad can be used to read the position of three Odometry pods - * fitted with REV Thru-Bore encoders. For a more advanced example with additional OctoQuad - * capabilities, see the SensorOctoQuadAdv sample. - * - * This OpMode assumes the OctoQuad is attached to an I2C interface named "octoquad" in the robot config. - * - * The code assumes the first three OctoQuad inputs are connected as follows - * - Chan 0: for measuring forward motion on the left side of the robot. - * - Chan 1: for measuring forward motion on the right side of the robot. - * - Chan 2: for measuring Lateral (strafing) motion. - * - * The encoder values may be reset to zero by pressing the X (left most) button on Gamepad 1. - * - * This sample does not show how to interpret these readings, just how to obtain and display them. - * - * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list - * - * See the sensor's product page: https://www.tindie.com/products/35114/ - */ -@Disabled -@TeleOp(name = "OctoQuad Basic", group="OctoQuad") -public class SensorOctoQuad extends LinearOpMode { - - // Identify which encoder OctoQuad inputs are connected to each odometry pod. - private final int ODO_LEFT = 0; // Facing forward direction on left side of robot - private final int ODO_RIGHT = 1; // Facing forward direction on right side or robot - private final int ODO_PERP = 2; // Facing perpendicular direction at the center of the robot - - // Declare the OctoQuad object; - private OctoQuad octoquad; - - private int posLeft; - private int posRight; - private int posPerp; - private int velLeft; - private int velRight; - private int velPerp; - - /** - * This function is executed when this OpMode is selected from the Driver Station. - */ - @Override - public void runOpMode() { - - // Connect to OctoQuad by referring to its name in the Robot Configuration. - octoquad = hardwareMap.get(OctoQuad.class, "octoquad"); - - // Read the Firmware Revision number from the OctoQuad and display it as telemetry. - telemetry.addData("OctoQuad Firmware Version ", octoquad.getFirmwareVersion()); - - // Reverse the count-direction of any encoder that is not what you require. - // e.g. if you push the robot forward and the left encoder counts down, then reverse it. - octoquad.setSingleEncoderDirection(ODO_LEFT, OctoQuad.EncoderDirection.REVERSE); - octoquad.setSingleEncoderDirection(ODO_RIGHT, OctoQuad.EncoderDirection.FORWARD); - octoquad.setSingleEncoderDirection(ODO_PERP, OctoQuad.EncoderDirection.FORWARD); - - // set the interval over which pulses are counted to determine velocity. - octoquad.setAllVelocitySampleIntervals(50); // 50 mSec means 20 velocity updates per second. - - // Save any changes that are made, just in case there is a sensor power glitch. - octoquad.saveParametersToFlash(); - - telemetry.addLine("\nPress START to read encoder values"); - telemetry.update(); - - waitForStart(); - - // Configure the telemetry for optimal display of data. - telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); - telemetry.setMsTransmissionInterval(100); - - // Set all the encoder inputs to zero. - octoquad.resetAllPositions(); - - // Loop while displaying the odometry pod positions. - while (opModeIsActive()) { - telemetry.addData(">", "Press X to Reset Encoders\n"); - - // Check for X button to reset encoders. - if (gamepad1.x) { - // Reset the position of all encoders to zero. - octoquad.resetAllPositions(); - } - - // Read all the encoder data. Load into local members. - readOdometryPods(); - - // Display the values. - telemetry.addData("Left P", "%7d V :%6d CPS ", posLeft, velLeft); - telemetry.addData("Right P", "%7d V :%6d CPS ", posRight, velRight); - telemetry.addData("Perp P", "%7d V :%6d CPS ", posPerp, velPerp); - telemetry.update(); - } - } - - private void readOdometryPods() { - // For best performance, we should only perform ONE transaction with the OctoQuad each cycle. - // This can be achieved in one of two ways: - // 1) by doing a block data read once per control cycle - // or - // 2) by doing individual caching reads, but only reading each encoder value ONCE per cycle. - // - // Since method 2 is simplest, we will use it here. - posLeft = octoquad.readSinglePosition_Caching(ODO_LEFT); - posRight = octoquad.readSinglePosition_Caching(ODO_RIGHT); - posPerp = octoquad.readSinglePosition_Caching(ODO_PERP); - velLeft = octoquad.readSingleVelocity_Caching(ODO_LEFT) * 20; // scale up to cps - velRight = octoquad.readSingleVelocity_Caching(ODO_RIGHT) * 20; // scale up to cps - velPerp = octoquad.readSingleVelocity_Caching(ODO_PERP) * 20; // scale up to cps - } -} diff --git a/TeamCode/.project b/TeamCode/.project new file mode 100644 index 000000000000..81437c6517d8 --- /dev/null +++ b/TeamCode/.project @@ -0,0 +1,28 @@ + + + TeamCode + Project TeamCode created by Buildship. + + + + + org.eclipse.buildship.core.gradleprojectbuilder + + + + + + org.eclipse.buildship.core.gradleprojectnature + + + + 1759929503127 + + 30 + + org.eclipse.core.resources.regexFilterMatcher + node_modules|\.git|__CREATED_BY_JAVA_LANGUAGE_SERVER__ + + + + diff --git a/TeamCode/.settings/org.eclipse.buildship.core.prefs b/TeamCode/.settings/org.eclipse.buildship.core.prefs new file mode 100644 index 000000000000..824d16fef8d7 --- /dev/null +++ b/TeamCode/.settings/org.eclipse.buildship.core.prefs @@ -0,0 +1,13 @@ +arguments=--init-script /home/etangos/.cache/nvim/jdtls/TeamCode/config/org.eclipse.osgi/58/0/.cp/gradle/init/init.gradle +auto.sync=false +build.scans.enabled=false +connection.gradle.distribution=GRADLE_DISTRIBUTION(VERSION(8.9)) +connection.project.dir= +eclipse.preferences.version=1 +gradle.user.home= +java.home=/usr/lib/jvm/java-24-openjdk +jvm.arguments= +offline.mode=false +override.workspace.settings=true +show.console.view=true +show.executions.view=true diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 8ab439e44d9a..75539453d1c5 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -14,6 +14,7 @@ // Include common definitions from above. apply from: '../build.common.gradle' apply from: '../build.dependencies.gradle' +apply plugin: "com.github.spotbugs" android { namespace = 'org.firstinspires.ftc.teamcode' @@ -23,8 +24,8 @@ android { } compileOptions { - sourceCompatibility JavaVersion.VERSION_1_8 - targetCompatibility JavaVersion.VERSION_1_8 + sourceCompatibility JavaVersion.VERSION_16 + targetCompatibility JavaVersion.VERSION_16 } } @@ -48,4 +49,53 @@ dependencies { implementation "com.acmerobotics.roadrunner:core:1.0.1" implementation "com.acmerobotics.roadrunner:actions:1.0.1" implementation "com.acmerobotics.dashboard:dashboard:0.5.1" -} \ No newline at end of file +} + +import com.github.spotbugs.snom.SpotBugsTask + +// Create a stable task name we can call from CI: :TeamCode:spotbugsTeamCodeDebug +afterEvaluate { + def variants = + (project.plugins.hasPlugin("com.android.library") && android.hasProperty("libraryVariants")) ? android.libraryVariants : + (project.plugins.hasPlugin("com.android.application") && android.hasProperty("applicationVariants")) ? android.applicationVariants : + null + + if (variants == null) { + logger.lifecycle("SpotBugs: No Android variants found in :TeamCode (is it an Android module?)") + return + } + + variants.all { variant -> + if (variant.name != "debug") return + + def javaCompile = variant.hasProperty("javaCompileProvider") ? variant.javaCompileProvider.get() : variant.javaCompile + + // AGP versions differ on where compiled classes live: + def classesDir = javaCompile.hasProperty("destinationDirectory") + ? javaCompile.destinationDirectory.asFile.get() + : javaCompile.destinationDir + + tasks.register("spotbugsTeamCodeDebug", SpotBugsTask) { + group = "verification" + description = "Run SpotBugs on TeamCode (debug variant)" + dependsOn javaCompile + + // SpotBugs inputs + classes = files(classesDir) + // TeamCode Java sources (variant source sets) + sourceDirs = files(variant.sourceSets.collectMany { it.java.srcDirs }) + + reports.create("html") { + required = true + outputLocation = file("$buildDir/reports/spotbugs/teamcode-debug.html") + } + reports.create("xml") { + required = true + outputLocation = file("$buildDir/reports/spotbugs/teamcode-debug.xml") + } + } + + // Make `./gradlew check` run it too (optional but handy) + tasks.named("check").configure { dependsOn("spotbugsTeamCodeDebug") } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/AutoPathNavigator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/AutoPathNavigator.java new file mode 100644 index 000000000000..8f5c39996fa3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/AutoPathNavigator.java @@ -0,0 +1,563 @@ +/* + * Autonomous code + * Teleop and action methods + * Uses RoadRunner + * Setup and integration of RR in miscRR/MecanumDrive.java + */ + +package org.firstinspires.ftc.teamcode.auto.roadrunner; + +// Road Runner imports +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.ParallelAction; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.SleepAction; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.acmerobotics.roadrunner.Vector2d; + +// Hardware imports +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +// Other +import org.firstinspires.ftc.teamcode.auto.roadrunner.miscRR.MecanumDrive; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.subsystems.Indexer; + +import java.util.ArrayList; + + +/* --useful info-- + * Naming scheme - far means the side with both goals and near is opposite of it + * + * Robot starts at 1 of 2 locations: + * 1. touching goal or far wall and touching far launch zone (Far opmodes) + * 2. touching near wall and touching near launch zone (Near opmodes) + * And they must be on your alliance's side + * + * tiles are 24 * 24 inches + */ + +// We will make it for all 4 positions, just because allied teams may only have certain auto positions +// If solo full auto is undoable and alliance partner has auto, we shall see how we want to do dividing work with the other team. +// red is right side, blue is left side (perspective of facing obelisk) +// Might/probably will split up this file for easier navigation and organization +public class AutoPathNavigator { + + // Positions numbered from bottom(nearest) rows up, and furthest from your alliance's wall to closest + private static final Vector2d[][] artifact_positions = new Vector2d[3][3]; + + // All of these positions could be redundant except the last of each if indexer works quick enough + public static void runOpModeRedFar(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(180))); + + int x_val_pose_0 = -20; + int x_val_pose_1 = -25; + int x_val_pose_2 = -30; + + int y_val_col_0 = 48; + int y_val_col_1 = 72; + int y_val_col_2 = 96; + + Vector2d shootPos = new Vector2d(-10, 10); + + artifact_positions[2][0] = new Vector2d(x_val_pose_0, y_val_col_2); + artifact_positions[2][1] = new Vector2d(x_val_pose_1, y_val_col_2); + artifact_positions[2][2] = new Vector2d(x_val_pose_2, y_val_col_2); + artifact_positions[1][0] = new Vector2d(x_val_pose_0, y_val_col_1); + artifact_positions[1][1] = new Vector2d(x_val_pose_1, y_val_col_1); + artifact_positions[1][2] = new Vector2d(x_val_pose_2, y_val_col_1); + artifact_positions[0][0] = new Vector2d(x_val_pose_0, y_val_col_0); + artifact_positions[0][1] = new Vector2d(x_val_pose_1, y_val_col_0); + artifact_positions[0][2] = new Vector2d(x_val_pose_2, y_val_col_0); + + Vector2d parkPos = new Vector2d(0,100); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(180))) + .strafeToSplineHeading(new Vector2d(0, y_val_col_2), Math.toRadians(180)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[2][0], Math.toRadians(180)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[2][1], Math.toRadians(180)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[2][2], Math.toRadians(180)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(180)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_1), Math.toRadians(180)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[1][0], Math.toRadians(180)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[1][1], Math.toRadians(180)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[1][2], Math.toRadians(180)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(180)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_0), Math.toRadians(180)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[0][0], Math.toRadians(180)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[0][1], Math.toRadians(180)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(180))) + .strafeToSplineHeading(artifact_positions[0][2], Math.toRadians(180)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(180)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + .strafeToSplineHeading(parkPos, Math.toRadians(180)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeBlueFar(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(0))); + + int x_val_pose_0 = 20; + int x_val_pose_1 = 25; + int x_val_pose_2 = 30; + + int y_val_col_0 = 48; + int y_val_col_1 = 72; + int y_val_col_2 = 96; + + Vector2d shootPos = new Vector2d(10, 10); + + artifact_positions[0][0] = new Vector2d(x_val_pose_0, y_val_col_2); + artifact_positions[0][1] = new Vector2d(x_val_pose_1, y_val_col_2); + artifact_positions[0][2] = new Vector2d(x_val_pose_2, y_val_col_2); + artifact_positions[1][0] = new Vector2d(x_val_pose_0, y_val_col_1); + artifact_positions[1][1] = new Vector2d(x_val_pose_1, y_val_col_1); + artifact_positions[1][2] = new Vector2d(x_val_pose_2, y_val_col_1); + artifact_positions[2][0] = new Vector2d(x_val_pose_0, y_val_col_0); + artifact_positions[2][1] = new Vector2d(x_val_pose_1, y_val_col_0); + artifact_positions[2][2] = new Vector2d(x_val_pose_2, y_val_col_0); + + Vector2d parkPos = new Vector2d(0,100); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(0))) + .strafeToSplineHeading(new Vector2d(0, y_val_col_2), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_1), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_0), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeRedNear(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(0))); + + int x_val_pose_0 = 20; + int x_val_pose_1 = 25; + int x_val_pose_2 = 30; + + int y_val_col_0 = 24; + int y_val_col_1 = 48; + int y_val_col_2 = 72; + + Vector2d shootPos = new Vector2d(0, 0); + + artifact_positions[0][0] = new Vector2d(x_val_pose_0, y_val_col_2); + artifact_positions[0][1] = new Vector2d(x_val_pose_1, y_val_col_2); + artifact_positions[0][2] = new Vector2d(x_val_pose_2, y_val_col_2); + artifact_positions[1][0] = new Vector2d(x_val_pose_0, y_val_col_1); + artifact_positions[1][1] = new Vector2d(x_val_pose_1, y_val_col_1); + artifact_positions[1][2] = new Vector2d(x_val_pose_2, y_val_col_1); + artifact_positions[2][0] = new Vector2d(x_val_pose_0, y_val_col_0); + artifact_positions[2][1] = new Vector2d(x_val_pose_1, y_val_col_0); + artifact_positions[2][2] = new Vector2d(x_val_pose_2, y_val_col_0); + + Vector2d parkPos = new Vector2d(24,24); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(0))) + .strafeToSplineHeading(new Vector2d(0, y_val_col_2), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_1), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_0), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeBlueNear(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(180))); + + int x_val_pose_0 = -20; + int x_val_pose_1 = -25; + int x_val_pose_2 = -30; + + int y_val_col_0 = 24; + int y_val_col_1 = 48; + int y_val_col_2 = 72; + + Vector2d shootPos = new Vector2d(0, 0); + + artifact_positions[0][0] = new Vector2d(x_val_pose_0, y_val_col_2); + artifact_positions[0][1] = new Vector2d(x_val_pose_1, y_val_col_2); + artifact_positions[0][2] = new Vector2d(x_val_pose_2, y_val_col_2); + artifact_positions[1][0] = new Vector2d(x_val_pose_0, y_val_col_1); + artifact_positions[1][1] = new Vector2d(x_val_pose_1, y_val_col_1); + artifact_positions[1][2] = new Vector2d(x_val_pose_2, y_val_col_1); + artifact_positions[2][0] = new Vector2d(x_val_pose_0, y_val_col_0); + artifact_positions[2][1] = new Vector2d(x_val_pose_1, y_val_col_0); + artifact_positions[2][2] = new Vector2d(x_val_pose_2, y_val_col_0); + + Vector2d parkPos = new Vector2d(-24,24); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(180))) + .strafeToSplineHeading(new Vector2d(0, y_val_col_2), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_2, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[2][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_1), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_1, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[1][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + + .strafeToSplineHeading(new Vector2d(0, y_val_col_0), Math.toRadians(0)) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][0], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][1], Math.toRadians(0)) + .build() + )) + .stopAndAdd(new ParallelAction( + botActions.actionIntake(), + mecanumDrive.actionBuilder(new Pose2d(0, y_val_col_0, Math.toRadians(0))) + .strafeToSplineHeading(artifact_positions[0][2], Math.toRadians(0)) + .build() + )) + .waitSeconds(.5) + .strafeToSplineHeading(shootPos, Math.toRadians(0)) + .waitSeconds(.5) + .stopAndAdd(botActions.actionOuttake()) + .waitSeconds(.5) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeRedParkFar(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(180))); + + Vector2d parkPos = new Vector2d(0,100); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(180))) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeBlueParkFar(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(180))); + + Vector2d parkPos = new Vector2d(0,100); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(180))) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeRedParkNear(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(180))); + + Vector2d parkPos = new Vector2d(24,24); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(180))) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } + + public static void runOpModeBlueParkNear(LinearOpMode opMode) { + HardwareMap hardwareMap = opMode.hardwareMap; + Telemetry telemetry = opMode.telemetry; + BotActions botActions = new BotActions(hardwareMap, telemetry); + + MecanumDrive mecanumDrive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, Math.toRadians(180))); + + Vector2d parkPos = new Vector2d(-24,24); + + Action arcStrikeVelocity = mecanumDrive.actionBuilder(new Pose2d(0 , 0 , Math.toRadians(180))) + .strafeToSplineHeading(parkPos, Math.toRadians(0)) + .stopAndAdd(botActions.actionPark()) + .build(); + opMode.waitForStart(); + Actions.runBlocking(arcStrikeVelocity); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/BotActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/BotActions.java new file mode 100644 index 000000000000..a55cfd6fe44b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/BotActions.java @@ -0,0 +1,51 @@ +/* + * Action methods for use in Auto + */ + +package org.firstinspires.ftc.teamcode.auto.roadrunner; + +import androidx.annotation.NonNull; + +import com.acmerobotics.roadrunner.Action; +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.ParallelAction; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.subsystems.Intake; +import org.firstinspires.ftc.teamcode.subsystems.Outtake; +import org.firstinspires.ftc.teamcode.subsystems.Indexer; + + +public class BotActions { + private final Intake intake; + private final Outtake outtake; + private final Indexer indexer; + + public BotActions(@NonNull HardwareMap hardwareMap, @NonNull Telemetry telemetry) { + intake = new Intake(hardwareMap); + + indexer = new Indexer(hardwareMap); + + outtake = new Outtake(hardwareMap); + } + + public Action actionIntake() { + // Intakes and rotates indexer + return new SequentialAction( + + ); + } + + public Action actionOuttake() { + // Rotates Indexer and outtakes all 3 + return new SequentialAction( + ); + } + + public Action actionPark() { + // vert slides + return new ParallelAction( + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueFar.java new file mode 100644 index 000000000000..739c16f36d52 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueFar.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "Blue-Far", group = "Autonomous") +public class BlueFar extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeBlueFar(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueNear.java new file mode 100644 index 000000000000..f3cdc0228295 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueNear.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "Blue-Near", group = "Autonomous") +public class BlueNear extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeBlueNear(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkFar.java new file mode 100644 index 000000000000..96ad6f19f1a7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkFar.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "BluePark-Far", group = "Autonomous") +public class BlueParkFar extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeBlueParkFar(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkNear.java new file mode 100644 index 000000000000..9993c28f1a4f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/BlueParkNear.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "BluePark-Near", group = "Autonomous") +public class BlueParkNear extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeBlueParkNear(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedFar.java new file mode 100644 index 000000000000..2c94a98eeea1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedFar.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "Red-Far", group = "Autonomous") +public class RedFar extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeRedFar(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedNear.java new file mode 100644 index 000000000000..71d19dec0faf --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedNear.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "Red-Close", group = "Autonomous") +public class RedNear extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeRedNear(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkFar.java new file mode 100644 index 000000000000..e259656f8aea --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkFar.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "RedPark-Far", group = "Autonomous") +public class RedParkFar extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeRedParkFar(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkNear.java new file mode 100644 index 000000000000..fe43569d0449 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/roadrunner/opmodes/RedParkNear.java @@ -0,0 +1,14 @@ +package org.firstinspires.ftc.teamcode.auto.roadrunner.opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.auto.roadrunner.AutoPathNavigator; + +@Autonomous(name = "RedPark-Near", group = "Autonomous") +public class RedParkNear extends LinearOpMode { + @Override + public void runOpMode() { + AutoPathNavigator.runOpModeRedParkNear(this); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BetterCRControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BetterCRControl.java new file mode 100644 index 000000000000..f4c23233bb94 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BetterCRControl.java @@ -0,0 +1,80 @@ +package org.firstinspires.ftc.teamcode.subsystems; +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.util.ElapsedTime; + +@Config +public class BetterCRControl { + private final CRServo crServo; + private final AnalogInput encoder; + + public static double kp = 0.45; + public static double ki = 0.0; + public static double kd = 0.04; + + public static double deadband = 1.5; // degrees + public static double minPower = 0.08; + public static double holdPower = 0.05; + + private double integral = 0.0; + private double lastError = 0.0; + private double filteredVoltage = 0; + + private ElapsedTime timer = new ElapsedTime(); + + public BetterCRControl(CRServo servo, AnalogInput encoder) { + this.crServo = servo; + this.encoder = encoder; + timer.reset(); + } + + private double getFilteredVoltage() { + filteredVoltage = 0.6 * filteredVoltage + 0.4 * encoder.getVoltage(); + return filteredVoltage; + } + + private double voltageToAngle(double v) { + return (v / 3.2) * 360.0; + } + + private double wrapDegrees(double angle) { + angle %= 360; + if (angle > 180) angle -= 360; + if (angle < -180) angle += 360; + return angle; + } + + public void moveToAngle(double target) { + double current = voltageToAngle(getFilteredVoltage()); + double error = wrapDegrees(target - current); + + // Hold state behavior + if (Math.abs(error) < deadband) { + crServo.setPower(holdPower * Math.signum(lastError)); + lastError = error; + return; + } + + double dt = timer.seconds(); + timer.reset(); + if (dt < 0.0001) dt = 0.0001; + + integral += error * dt; + integral = Math.max(-2, Math.min(2, integral)); + + double derivative = (error - lastError) / dt; + + double output = kp * error + ki * integral + kd * derivative; + + // Minimum power to break stiction + if (Math.abs(output) < minPower) + output = minPower * Math.signum(output); + + output = Math.max(-1.0, Math.min(1.0, output)); + + crServo.setPower(output); + + lastError = error; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java new file mode 100644 index 000000000000..cf95713edb48 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Bot.java @@ -0,0 +1,150 @@ +package org.firstinspires.ftc.teamcode.teleop; + +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.subsystems.Actuator; +import org.firstinspires.ftc.teamcode.subsystems.Indexer; +import org.firstinspires.ftc.teamcode.subsystems.Intake; +import org.firstinspires.ftc.teamcode.subsystems.Movement; +import org.firstinspires.ftc.teamcode.subsystems.Outtake; + +public class Bot { + private final Intake intake; + private final Indexer indexer; + private final Actuator actuator; + private final Outtake outtake; + private final Movement movement; + + private final GamepadEx g1; + private final GamepadEx g2; + private final Telemetry telemetry; + + private boolean fieldCentric = false; + + public enum FSM { + Intake, + QuickOuttake, + SortOuttake, + Endgame + } + + public FSM state; + + private static final double TRIGGER_DEADZONE = 0.05; + private boolean quickSpinRequested = false; + + public Bot(HardwareMap hardwareMap, Telemetry tele, Gamepad gamepad1, Gamepad gamepad2) { + intake = new Intake(hardwareMap); + indexer = new Indexer(hardwareMap); + actuator = new Actuator(hardwareMap); + outtake = new Outtake(hardwareMap, Outtake.Mode.RPM); + movement = new Movement(hardwareMap); + g1 = new GamepadEx(gamepad1); + g2 = new GamepadEx(gamepad2); + telemetry = tele; + state = FSM.Intake; + } + + private void enterState(FSM newState) { + if (state != newState) { + state = newState; + quickSpinRequested = false; + } + } + + public void teleopInit() { + indexer.startIntake(); + enterState(FSM.Intake); + } + + public void teleopTick() { + g1.readButtons(); + g2.readButtons(); + + handleMovement(); + + if (g1.wasJustPressed(GamepadKeys.Button.LEFT_STICK_BUTTON)) { + fieldCentric = !fieldCentric; + } + + switch (state) { + case Intake: + handleIntakeState(); + break; + case QuickOuttake: + handleQuickOuttakeState(); + break; + case SortOuttake: + handleSortOuttakeState(); + break; + case Endgame: + handleEndgameState(); + break; + } + + // Telem (most sigma ever) + telemetry.addData("Field Centric:", fieldCentric); + telemetry.addData("Indexer States:", "Current State: %s , Next State: %s",indexer.getState(), indexer.nextState()); + telemetry.addData("Indexer Voltages:", "Target: %.3f , Actual: %.3f" , indexer.getTargetVoltage(), indexer.getVoltageAnalog()); + telemetry.addData("Actuator up?: ", actuator.isActivated()); + telemetry.update(); + } + + private void handleMovement() { + double lx = g1.getLeftX(); + double ly = g1.getLeftY(); + double rx = g1.getRightX(); + + if (fieldCentric) movement.teleopTickFieldCentric(lx, ly, rx, 0, true); + else movement.teleopTick(lx, ly, rx, 0); + } + + private void handleIntakeState() { + double leftTrigger = g2.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER); + if (leftTrigger > TRIGGER_DEADZONE) intake.run(); + else intake.stop(); + + if (g2.wasJustPressed(GamepadKeys.Button.A)) enterState(FSM.QuickOuttake); + if (g2.wasJustPressed(GamepadKeys.Button.B)) enterState(FSM.SortOuttake); + if (g2.wasJustPressed(GamepadKeys.Button.Y)) enterState(FSM.Endgame); + } + + private void handleQuickOuttakeState() { + if (g2.wasJustPressed(GamepadKeys.Button.X)) { + quickSpinRequested = !quickSpinRequested; + if (quickSpinRequested) { + // TODO: start shooter quickly (e.g. outtake.startHighSpeed()) + } else { + // TODO: outtake.stop() + } + } + + if (quickSpinRequested) { + // TODO: do ts ig + // Example: call indexer.advanceOne() with your own timing logic + } + + if (g2.wasJustPressed(GamepadKeys.Button.A)) enterState(FSM.Intake); + } + + private void handleSortOuttakeState() { + // TODO: implement sorted shoot sequence: + // 1) spin shooter to RPM + // 2) wait for stable RPM (ideally ts is in a roadronere action) + // 3) feed one ball at a time with small delays + if (g2.wasJustPressed(GamepadKeys.Button.A)) { + // cancel sort and return to intake + enterState(FSM.Intake); + } + } + + private void handleEndgameState() { + // TODO: activate actuator to open slides or perform endgame actions + // Example: actuator.extendSlides(); + if (g2.wasJustPressed(GamepadKeys.Button.A)) enterState(FSM.Intake); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java new file mode 100644 index 000000000000..d20597c34449 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/ContinuousValueTester.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.teamcode.testing; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.AnalogInput; + + +import org.firstinspires.ftc.teamcode.subsystems.CRServoPositionControl; + +@TeleOp(name = "ContinuousServoTester") + +public class ContinuousValueTester extends LinearOpMode { + AnalogInput indexerAnalog; + + @Override + public void runOpMode() throws InterruptedException { + AnalogInput signal = hardwareMap.get(AnalogInput.class, "indexAnalog"); + CRServo indexerServo = hardwareMap.get(CRServo.class, "index"); + CRServoPositionControl crServo = new CRServoPositionControl(indexerServo, indexerAnalog); + waitForStart(); + while (opModeIsActive()) + { + indexerServo.setPower(0.1); + telemetry.addData("voltage", signal.getVoltage()); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java new file mode 100644 index 000000000000..095c650fb1e7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/SpindexerTester.java @@ -0,0 +1,94 @@ +package org.firstinspires.ftc.teamcode.testing; + +import androidx.annotation.NonNull; + +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.arcrobotics.ftclib.gamepad.GamepadKeys; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.subsystems.Indexer; +import org.firstinspires.ftc.teamcode.subsystems.Intake; + +@TeleOp(name = "SpindexerTest", group = "Teleop") +public class SpindexerTester extends LinearOpMode { + + Indexer indexer; + Intake intake; + GamepadEx gp2; + + @Override + public void runOpMode() { + indexer = new Indexer(hardwareMap); + intake = new Intake(hardwareMap); + gp2 = new GamepadEx(gamepad2); + + waitForStart(); + + indexer.startIntake(); + + while (opModeIsActive()) { + gp2.readButtons(); + + telemetry.addData("CurrentState: ", indexer.getState()); + telemetry.addData("NextState: ", indexer.nextState()); + + if (gp2.wasJustPressed(GamepadKeys.Button.DPAD_UP)) { + if (!indexer.isBusy()) { + indexer.moveTo(indexer.nextState()); + } + } + + if (gp2.wasJustPressed(GamepadKeys.Button.A)) { + if (!indexer.isBusy()) { + indexer.setIntaking(true); + } + } + + if (gp2.wasJustPressed(GamepadKeys.Button.B)) { + if (!indexer.isBusy()) { + indexer.setIntaking(false); + } + } + + if (gp2.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.01) { + intake.run(); + } else { + intake.stop(); + } + + // Call update each loop to continuously control the servo position + indexer.update(); + telemetry.addData("Indexer Voltage: ", indexer.getVoltageAnalog()); + + // Update color scanning timing and sensor reading + //indexer.updateColorScanning(); + + telemetry.update(); + } + } + + @NonNull + private static String getMainColor(int r, int g, int b) { + double total = r + g + b; + double rRatio = r / total; + double gRatio = g / total; + double bRatio = b / total; + + if (rRatio > 0.45 && gRatio < 0.35 && bRatio < 0.35) { + return "Red"; + } else if (rRatio > 0.45 && gRatio > 0.25 && bRatio < 0.20) { + return "Orange"; + } else if (rRatio > 0.38 && gRatio > 0.38 && bRatio < 0.25) { + return "Yellow"; + } else if (gRatio > 0.45 && rRatio < 0.35 && bRatio < 0.35) { + return "Green"; + } else if (bRatio > 0.45 && rRatio < 0.35 && gRatio < 0.35) { + return "Blue"; + } else if (rRatio > 0.35 && bRatio > 0.35 && gRatio < 0.30) { + return "Purple"; + } else { + return "Unclear"; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java new file mode 100644 index 000000000000..c320d0b25bdd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/testing/VoltageTester.java @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode.testing; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.subsystems.Indexer; + +@TeleOp(name = "Voltage Tester") +public class VoltageTester extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + Indexer indexer = new Indexer(hardwareMap); + waitForStart(); + while (opModeIsActive()) + { + telemetry.addData("voltage", indexer.getVoltageAnalog()); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml index 22ae7a86ba33..eb71d94f7d3b 100644 --- a/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml +++ b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml @@ -147,14 +147,14 @@ - + diff --git a/build.gradle b/build.gradle index 6dd7fd3483ce..d979eaef0b24 100644 --- a/build.gradle +++ b/build.gradle @@ -11,10 +11,14 @@ buildscript { } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:8.13.0' + classpath 'com.android.tools.build:gradle:8.8.0' } } +plugins { + id "com.github.spotbugs" version "6.4.4" apply false +} + // This is now required because aapt2 has to be downloaded from the // google() repository beginning with version 3.2 of the Android Gradle Plugin allprojects { @@ -24,6 +28,7 @@ allprojects { } } + repositories { mavenCentral() }