From f00ab228aa3dead73ff7ff92bf552660fa591691 Mon Sep 17 00:00:00 2001 From: BennyLi Date: Sun, 20 Oct 2024 09:43:13 +0800 Subject: [PATCH 1/2] =?UTF-8?q?settings.gradle=20=E4=BF=AE=E5=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- settings.gradle | 1 + 1 file changed, 1 insertion(+) diff --git a/settings.gradle b/settings.gradle index 2546a13..9bfa9cf 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,3 +1,4 @@ include ':FtcRobotController' include ':TeamCode' include ':roadrunnerCores' +include ':FtcTeamCodeSample' From 08c7823b36c85fb50197277c245610a3858f0d32 Mon Sep 17 00:00:00 2001 From: BennyLi Date: Sun, 20 Oct 2024 13:11:17 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E5=B0=8D=20roadrunner=20=E7=9A=84=E9=8C=AF?= =?UTF-8?q?=E8=AA=A4=E9=80=B2=E8=A1=8C=E4=BF=AE=E5=BE=A9=EF=BC=8C=E5=92=8B?= =?UTF-8?q?=E5=8F=88=E8=AE=8A=E6=88=90=E7=B9=81=E9=AB=94=E5=AD=97=E4=BA=86?= =?UTF-8?q?=EF=BC=9F=EF=BC=9F=EF=BC=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../core/messages/DriveCommandMessage.java | 2 +- .../core/messages/MecanumCommandMessage.java | 2 +- .../MecanumLocalizerInputsMessage.java | 2 +- .../roadrunner/core/messages/PoseMessage.java | 2 +- .../core/messages/TankCommandMessage.java | 2 +- .../messages/TankLocalizerInputsMessage.java | 2 +- .../messages/ThreeDeadWheelInputsMessage.java | 2 +- .../messages/TwoDeadWheelInputsMessage.java | 2 +- .../core/tuning/LocalizationTest.java | 8 +-- .../core/tuning/ManualFeedbackTuner.java | 66 +++++++++++-------- .../roadrunner/core/tuning/SplineTest.java | 6 +- .../roadrunner/core/tuning/TuningOpModes.java | 10 +-- 12 files changed, 59 insertions(+), 47 deletions(-) diff --git a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/DriveCommandMessage.java b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/DriveCommandMessage.java index 4bb4633..9927cb0 100644 --- a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/DriveCommandMessage.java +++ b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/DriveCommandMessage.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.roadrunner.messages; +package org.roadrunner.core.messages; import com.acmerobotics.roadrunner.PoseVelocity2dDual; import com.acmerobotics.roadrunner.Time; diff --git a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/MecanumCommandMessage.java b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/MecanumCommandMessage.java index c8668c2..f500828 100644 --- a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/MecanumCommandMessage.java +++ b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/MecanumCommandMessage.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.roadrunner.messages; +package org.roadrunner.core.messages; public final class MecanumCommandMessage { public long timestamp; diff --git a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/MecanumLocalizerInputsMessage.java b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/MecanumLocalizerInputsMessage.java index ccfe61f..0dc23f7 100644 --- a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/MecanumLocalizerInputsMessage.java +++ b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/MecanumLocalizerInputsMessage.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.roadrunner.messages; +package org.roadrunner.core.messages; import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; diff --git a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/PoseMessage.java b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/PoseMessage.java index aa652f1..aa5dbd3 100644 --- a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/PoseMessage.java +++ b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/PoseMessage.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.roadrunner.messages; +package org.roadrunner.core.messages; import com.acmerobotics.roadrunner.Pose2d; diff --git a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/TankCommandMessage.java b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/TankCommandMessage.java index 20245b1..f09de6f 100644 --- a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/TankCommandMessage.java +++ b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/TankCommandMessage.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.roadrunner.messages; +package org.roadrunner.core.messages; public final class TankCommandMessage { public long timestamp; diff --git a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/TankLocalizerInputsMessage.java b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/TankLocalizerInputsMessage.java index a0d5a51..0687cca 100644 --- a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/TankLocalizerInputsMessage.java +++ b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/TankLocalizerInputsMessage.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.roadrunner.messages; +package org.roadrunner.core.messages; import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; diff --git a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/ThreeDeadWheelInputsMessage.java b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/ThreeDeadWheelInputsMessage.java index f376486..cc03025 100644 --- a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/ThreeDeadWheelInputsMessage.java +++ b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/ThreeDeadWheelInputsMessage.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.roadrunner.messages; +package org.roadrunner.core.messages; import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; diff --git a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/TwoDeadWheelInputsMessage.java b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/TwoDeadWheelInputsMessage.java index 27b3d42..390699a 100644 --- a/roadrunnerCores/src/main/java/org/roadrunner/core/messages/TwoDeadWheelInputsMessage.java +++ b/roadrunnerCores/src/main/java/org/roadrunner/core/messages/TwoDeadWheelInputsMessage.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.roadrunner.messages; +package org.roadrunner.core.messages; import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; diff --git a/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/LocalizationTest.java b/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/LocalizationTest.java index 1c60734..323ac42 100644 --- a/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/LocalizationTest.java +++ b/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/LocalizationTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.roadrunner.tuning; +package org.roadrunner.core.tuning; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -8,9 +8,9 @@ import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.roadrunner.Drawing; -import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive; -import org.firstinspires.ftc.teamcode.roadrunner.TankDrive; +import org.roadrunner.core.Drawing; +import org.roadrunner.core.MecanumDrive; +import org.roadrunner.core.TankDrive; public class LocalizationTest extends LinearOpMode { @Override diff --git a/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/ManualFeedbackTuner.java b/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/ManualFeedbackTuner.java index 64584d6..dc23dda 100644 --- a/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/ManualFeedbackTuner.java +++ b/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/ManualFeedbackTuner.java @@ -1,13 +1,15 @@ -package org.firstinspires.ftc.teamcode.roadrunner.tuning; +package org.roadrunner.core.tuning; + +import androidx.annotation.NonNull; import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive; -import org.firstinspires.ftc.teamcode.roadrunner.TankDrive; -import org.firstinspires.ftc.teamcode.roadrunner.ThreeDeadWheelLocalizer; -import org.firstinspires.ftc.teamcode.roadrunner.TwoDeadWheelLocalizer; +import org.roadrunner.core.MecanumDrive; +import org.roadrunner.core.TankDrive; +import org.roadrunner.core.ThreeDeadWheelLocalizer; +import org.roadrunner.core.TwoDeadWheelLocalizer; public final class ManualFeedbackTuner extends LinearOpMode { public static double DISTANCE = 64; @@ -15,17 +17,7 @@ public final class ManualFeedbackTuner extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); - - if (drive.localizer instanceof TwoDeadWheelLocalizer) { - if (TwoDeadWheelLocalizer.PARAMS.perpXTicks == 0 && TwoDeadWheelLocalizer.PARAMS.parYTicks == 0) { - throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them."); - } - } else if (drive.localizer instanceof ThreeDeadWheelLocalizer) { - if (ThreeDeadWheelLocalizer.PARAMS.perpXTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par0YTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par1YTicks == 1) { - throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them."); - } - } + MecanumDrive drive = getMecanumDrive(); waitForStart(); while (opModeIsActive()) { @@ -36,17 +28,7 @@ public void runOpMode() throws InterruptedException { .build()); } } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) { - TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); - - if (drive.localizer instanceof TwoDeadWheelLocalizer) { - if (TwoDeadWheelLocalizer.PARAMS.perpXTicks == 0 && TwoDeadWheelLocalizer.PARAMS.parYTicks == 0) { - throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them."); - } - } else if (drive.localizer instanceof ThreeDeadWheelLocalizer) { - if (ThreeDeadWheelLocalizer.PARAMS.perpXTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par0YTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par1YTicks == 1) { - throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them."); - } - } + TankDrive drive = getTankDrive(); waitForStart(); while (opModeIsActive()) { @@ -60,4 +42,34 @@ public void runOpMode() throws InterruptedException { throw new RuntimeException(); } } + + private @NonNull MecanumDrive getMecanumDrive() { + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + if (drive.localizer instanceof TwoDeadWheelLocalizer) { + if (TwoDeadWheelLocalizer.PARAMS.perpXTicks == 0 && TwoDeadWheelLocalizer.PARAMS.parYTicks == 0) { + throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them."); + } + } else if (drive.localizer instanceof ThreeDeadWheelLocalizer) { + if (ThreeDeadWheelLocalizer.PARAMS.perpXTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par0YTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par1YTicks == 1) { + throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them."); + } + } + return drive; + } + + private @NonNull TankDrive getTankDrive() { + TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0)); + + if (drive.localizer instanceof TwoDeadWheelLocalizer) { + if (TwoDeadWheelLocalizer.PARAMS.perpXTicks == 0 && TwoDeadWheelLocalizer.PARAMS.parYTicks == 0) { + throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them."); + } + } else if (drive.localizer instanceof ThreeDeadWheelLocalizer) { + if (ThreeDeadWheelLocalizer.PARAMS.perpXTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par0YTicks == 0 && ThreeDeadWheelLocalizer.PARAMS.par1YTicks == 1) { + throw new RuntimeException("Odometry wheel locations not set! Run AngularRampLogger to tune them."); + } + } + return drive; + } } diff --git a/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/SplineTest.java b/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/SplineTest.java index edb8193..0f945b6 100644 --- a/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/SplineTest.java +++ b/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/SplineTest.java @@ -1,12 +1,12 @@ -package org.firstinspires.ftc.teamcode.roadrunner.tuning; +package org.roadrunner.core.tuning; import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive; -import org.firstinspires.ftc.teamcode.roadrunner.TankDrive; +import org.roadrunner.core.MecanumDrive; +import org.roadrunner.core.TankDrive; public final class SplineTest extends LinearOpMode { @Override diff --git a/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/TuningOpModes.java b/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/TuningOpModes.java index 09fc514..0bd2c6c 100644 --- a/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/TuningOpModes.java +++ b/roadrunnerCores/src/main/java/org/roadrunner/core/tuning/TuningOpModes.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.roadrunner.tuning; +package org.roadrunner.core.tuning; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.reflection.ReflectionConfig; @@ -22,10 +22,10 @@ import com.qualcomm.robotcore.eventloop.opmode.OpModeRegistrar; import org.firstinspires.ftc.robotcore.internal.opmode.OpModeMeta; -import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive; -import org.firstinspires.ftc.teamcode.roadrunner.TankDrive; -import org.firstinspires.ftc.teamcode.roadrunner.ThreeDeadWheelLocalizer; -import org.firstinspires.ftc.teamcode.roadrunner.TwoDeadWheelLocalizer; +import org.roadrunner.core.MecanumDrive; +import org.roadrunner.core.TankDrive; +import org.roadrunner.core.ThreeDeadWheelLocalizer; +import org.roadrunner.core.TwoDeadWheelLocalizer; import java.util.ArrayList; import java.util.Arrays;