From ff203e59c6b5529d1754b90b45d813a89775a51e Mon Sep 17 00:00:00 2001 From: LeCh2td <85344760+lechocolatchaud@users.noreply.github.com> Date: Mon, 9 Sep 2024 23:18:40 +0800 Subject: [PATCH 01/93] Merge branch 'FIRST-Tech-Challenge:master' into master --- .../samples/BasicOmniOpMode_Linear.java | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index 1d14dfb76035..ac2367240a5a 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -36,26 +36,26 @@ import com.qualcomm.robotcore.util.ElapsedTime; /* - * This file contains an example of a Linear "OpMode". - * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * This file contains an example of a Linear线性的 "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop(teleoperation遥控的) period of an FTC match. * The names of OpModes appear on the menu of the FTC Driver Station. * When a selection is made from the menu, the corresponding OpMode is executed. * - * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This particular OpMode illustrates driving a 4-motor Omni-Directional全向的 (or Holonomic完整的) robot. * This code will work with either a Mecanum-Drive or an X-Drive train. * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html - * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * Note that a Mecanum drive must display an X roller-pattern when viewed from above.注意，麦克纳姆轮正确安装时，从上往下看起来应该像X * - * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * Also note that it is critical to set the correct rotation旋转 direction for each motor. See details below. * - * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. - * Each motion axis is controlled by one Joystick axis. + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously同时的. + * Each motion axis轴 is controlled by one Joystick axis操纵杆. * * 1) Axial: Driving forward and backward Left-joystick Forward/Backward * 2) Lateral: Strafing right and left Left-joystick Right and Left - * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter相反的 clockwise Right-joystick Right and Left * - * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * This code is written assuming that the right-side motors need to be reversed反转 for the robot to drive forward. * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip * the direction of all 4 motors (see code below). * @@ -67,7 +67,7 @@ @Disabled public class BasicOmniOpMode_Linear extends LinearOpMode { - // Declare OpMode members for each of the 4 motors. + // Declare声明 OpMode members for each of the 4 motors. private ElapsedTime runtime = new ElapsedTime(); private DcMotor leftFrontDrive = null; private DcMotor leftBackDrive = null; From b38b73f30714750e4ab639337a084dd5b3f1d6a5 Mon Sep 17 00:00:00 2001 From: leoli Date: Mon, 9 Sep 2024 23:26:01 +0800 Subject: [PATCH 02/93] Merge branch 'FIRST-Tech-Challenge:master' into master --- .../external/samples/BasicOmniOpMode_Linear.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index ac2367240a5a..02ed3660c1cf 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -97,7 +97,7 @@ public void runOpMode() { leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); leftBackDrive.setDirection(DcMotor.Direction.REVERSE); rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); - rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD);r // Wait for the game to start (driver presses START) telemetry.addData("Status", "Initialized"); From 843b16859184cb418c65842b79f2cb2d5bba704c Mon Sep 17 00:00:00 2001 From: leoli Date: Mon, 9 Sep 2024 23:26:54 +0800 Subject: [PATCH 03/93] Merge branch 'FIRST-Tech-Challenge:master' into master --- .../external/samples/BasicOmniOpMode_Linear.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index 02ed3660c1cf..ac2367240a5a 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -97,7 +97,7 @@ public void runOpMode() { leftFrontDrive.setDirection(DcMotor.Direction.REVERSE); leftBackDrive.setDirection(DcMotor.Direction.REVERSE); rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); - rightBackDrive.setDirection(DcMotor.Direction.FORWARD);r + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); // Wait for the game to start (driver presses START) telemetry.addData("Status", "Initialized"); From 0e39465e7dcb53bfe42b0e3061716d9d50eb0fc2 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 9 Sep 2024 23:35:05 +0800 Subject: [PATCH 04/93] Merge branch 'FIRST-Tech-Challenge:master' into master --- .../external/samples/BasicOmniOpMode_Linear.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index ac2367240a5a..355cbbe656fe 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -100,7 +100,7 @@ public void runOpMode() { rightBackDrive.setDirection(DcMotor.Direction.FORWARD); // Wait for the game to start (driver presses START) - telemetry.addData("Status", "Initialized"); + telemetry.addData("Status", "Initialized");1 telemetry.update(); waitForStart(); From a3571a45a7b6bba29804964fecea8bd4bdf76dbd Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 9 Sep 2024 23:35:47 +0800 Subject: [PATCH 05/93] Merge branch 'FIRST-Tech-Challenge:master' into master --- .../external/samples/BasicOmniOpMode_Linear.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index 355cbbe656fe..ac2367240a5a 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -100,7 +100,7 @@ public void runOpMode() { rightBackDrive.setDirection(DcMotor.Direction.FORWARD); // Wait for the game to start (driver presses START) - telemetry.addData("Status", "Initialized");1 + telemetry.addData("Status", "Initialized"); telemetry.update(); waitForStart(); From b245e54bcc583c47a8003217a6f9ad8831ed1274 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 9 Sep 2024 23:37:55 +0800 Subject: [PATCH 06/93] Merge branch 'FIRST-Tech-Challenge:master' into master --- .../external/samples/BasicOmniOpMode_Linear.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index ac2367240a5a..355cbbe656fe 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -100,7 +100,7 @@ public void runOpMode() { rightBackDrive.setDirection(DcMotor.Direction.FORWARD); // Wait for the game to start (driver presses START) - telemetry.addData("Status", "Initialized"); + telemetry.addData("Status", "Initialized");1 telemetry.update(); waitForStart(); From 97110fcd90f1183c387fdcf018851386ed53e4fa Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 9 Sep 2024 23:41:13 +0800 Subject: [PATCH 07/93] Merge branch 'FIRST-Tech-Challenge:master' into master --- .../external/samples/BasicOmniOpMode_Linear.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index 355cbbe656fe..ac2367240a5a 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -100,7 +100,7 @@ public void runOpMode() { rightBackDrive.setDirection(DcMotor.Direction.FORWARD); // Wait for the game to start (driver presses START) - telemetry.addData("Status", "Initialized");1 + telemetry.addData("Status", "Initialized"); telemetry.update(); waitForStart(); From 3e70e46cc8cc711c2fcf8f8bfd688d38c6045112 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 9 Sep 2024 23:44:14 +0800 Subject: [PATCH 08/93] Merge branch 'FIRST-Tech-Challenge:master' into master --- .../external/samples/BasicOmniOpMode_Linear.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index ac2367240a5a..355cbbe656fe 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -100,7 +100,7 @@ public void runOpMode() { rightBackDrive.setDirection(DcMotor.Direction.FORWARD); // Wait for the game to start (driver presses START) - telemetry.addData("Status", "Initialized"); + telemetry.addData("Status", "Initialized");1 telemetry.update(); waitForStart(); From 018781aca3c6c36ee5fb839303453304c5eeab63 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 9 Sep 2024 23:44:45 +0800 Subject: [PATCH 09/93] Merge branch 'FIRST-Tech-Challenge:master' into master --- .../external/samples/BasicOmniOpMode_Linear.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java index 355cbbe656fe..ac2367240a5a 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -100,7 +100,7 @@ public void runOpMode() { rightBackDrive.setDirection(DcMotor.Direction.FORWARD); // Wait for the game to start (driver presses START) - telemetry.addData("Status", "Initialized");1 + telemetry.addData("Status", "Initialized"); telemetry.update(); waitForStart(); From 3de097a09ccf7f7f78677a0348759b6138e5d96e Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Tue, 10 Sep 2024 22:08:55 +0800 Subject: [PATCH 10/93] Merge branch 'FIRST-Tech-Challenge:master' into master --- .../java/org/firstinspires/ftc/teamcode/readme.md | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md index 4d1da42de0c0..22c8475426d8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md @@ -3,7 +3,7 @@ Welcome! This module, TeamCode, is the place where you will write/paste the code for your team's -robot controller App. This module is currently empty (a clean slate) but the +robot controller App. This module is currently empty (a clean slate板) but the process for adding OpModes is straightforward. ## Creating your own OpModes @@ -19,7 +19,7 @@ Expand the following tree elements: ### Naming of Samples To gain a better understanding of how the samples are organized, and how to interpret the -naming system, it will help to understand the conventions that were used during their creation. +naming system, it will help to understand the conventions公约 that were used during their creation. These conventions are described (in detail) in the sample_conventions.md file in this folder. @@ -29,20 +29,29 @@ The prefix of the name will be one of the following: Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure of a particular style of OpMode. These are bare bones examples. + 这是一个功能最少的 OpMode，用于说明特定 OpMode 样式的骨架/结构。这些是基本示例。 Sensor: This is a Sample OpMode that shows how to use a specific sensor. It is not intended to drive a functioning robot, it is simply showing the minimal code required to read and display the sensor values. + 这是一个示例 OpMode，展示了如何使用特定的传感器。 + 它并非用于驱动功能齐全的机器人，而只是展示读取和显示传感器值所需的最少代码。 Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. It may be used to provide a common baseline driving OpMode, or to demonstrate how a particular sensor or concept can be used to navigate. + 这是一个示例 OpMode，假设一个简单的双电机（差速）驱动基础。 + 它可用于提供通用基线驱动 OpMode，或演示如何使用特定传感器或概念进行导航。 Concept: This is a sample OpMode that illustrates performing a specific function or concept. These may be complex, but their operation should be explained clearly in the comments, or the comments should reference an external doc, guide or tutorial. Each OpMode should try to only demonstrate a single concept so they are easy to locate based on their name. These OpModes may not produce a drivable robot. + 这是演示如何执行特定功能或概念的示例 OpMode。 + 这些可能很复杂，但应在注释中清楚地解释其操作， + 或者注释应引用外部文档、指南或教程。 + 每个 OpMode 都应尝试仅演示一个概念，以便根据其名称轻松找到它们。这些 OpMode 可能无法生成可驾驶的机器人。 After the prefix, other conventions will apply: From 234fd46ad64a16ebb648481388e51d84a5d11372 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sun, 15 Sep 2024 22:45:57 +0800 Subject: [PATCH 11/93] Add new ManualOpMode structure --- .../ftc/teamcode/ManualOpMode.java | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java new file mode 100644 index 000000000000..57b73ee3ca85 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +@TeleOp +public class ManualOpMode extends OpMode +{ + + + + @Override + public void init() { + + } + + @Override + public void loop() { + + } +} From d4df22b2caf6cc1fe799197460360983f709b77a Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sun, 15 Sep 2024 23:21:16 +0800 Subject: [PATCH 12/93] Add new ManualOpMode structure --- .../ftc/teamcode/ManualOpMode.java | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 57b73ee3ca85..5f74d31bd527 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -1,21 +1,26 @@ package org.firstinspires.ftc.teamcode; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; @TeleOp -public class ManualOpMode extends OpMode +public class ManualOpMode extends LinearOpMode { - + DcMotor motor; @Override - public void init() { - - } + public void runOpMode() throws InterruptedException + { - @Override - public void loop() { + motor = hardwareMap.dcMotor.get("LeftRightmotor"); + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + waitForStart(); + motor.setPower(1); + motor.setTargetPosition(1000); } -} +} \ No newline at end of file From 70758d1d2e48308af425ad3b5712cd44239a390f Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 16 Sep 2024 22:44:34 +0800 Subject: [PATCH 13/93] Add new ManualOpMode structure --- .../ftc/teamcode/AUTOOpModeTest.java | 25 +++++++++++++++++++ .../ftc/teamcode/ManualOpMode.java | 19 ++++++-------- build.dependencies.gradle | 2 +- 3 files changed, 33 insertions(+), 13 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AUTOOpModeTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AUTOOpModeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AUTOOpModeTest.java new file mode 100644 index 000000000000..c4fe82c6689d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AUTOOpModeTest.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +@Autonomous +public class AUTOOpModeTest extends LinearOpMode +{ + + DcMotor motor; + + @Override + public void runOpMode() throws InterruptedException + { + + motor = hardwareMap.dcMotor.get("LeftRightmotor"); + motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + waitForStart(); + motor.setPower(1); + motor.setTargetPosition(1000); + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 5f74d31bd527..ed05e4252c7d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -1,26 +1,21 @@ package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; -@TeleOp public class ManualOpMode extends LinearOpMode { DcMotor motor; - @Override - public void runOpMode() throws InterruptedException - { + public void runOpMode() throws InterruptedException { - motor = hardwareMap.dcMotor.get("LeftRightmotor"); - motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motor.setMode(DcMotor.RunMode.RUN_TO_POSITION); waitForStart(); - motor.setPower(1); - motor.setTargetPosition(1000); + float x = gamepad1.left_stick_x; + if ( gamepad1.left_stick_x > 0) { + + motor.setPower(x); + } } -} \ No newline at end of file +} diff --git a/build.dependencies.gradle b/build.dependencies.gradle index c82423d50db3..c714d2ff5f7f 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -12,6 +12,6 @@ dependencies { implementation 'org.firstinspires.ftc:Hardware:10.0.0' implementation 'org.firstinspires.ftc:FtcCommon:10.0.0' implementation 'org.firstinspires.ftc:Vision:10.0.0' - implementation 'androidx.appcompat:appcompat:1.2.0' + implementation 'androidx.appcompat:appcompat:1.7.0' } From e8a15164fa6a4dda4de4780fa0d36c9858f5d03e Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Tue, 17 Sep 2024 23:03:12 +0800 Subject: [PATCH 14/93] Rename ManualOpMode.java --- .../ftc/teamcode/{ManualOpMode.java => ManualOpModeTest.java} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ManualOpMode.java => ManualOpModeTest.java} (88%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpModeTest.java similarity index 88% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpModeTest.java index ed05e4252c7d..9b187e0f497a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpModeTest.java @@ -3,7 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; -public class ManualOpMode extends LinearOpMode +public class ManualOpModeTest extends LinearOpMode { DcMotor motor; From 0f997ee949f0e5353032d8e0a34aa26b4b0d75bd Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 22 Nov 2024 16:38:03 +0800 Subject: [PATCH 15/93] Feat: unfinished ManualOpMode --- .../ftc/teamcode/ManualOpMode.java | 46 ++++++ .../ftc/teamcode/TaskOpMode.java | 110 --------------- .../ftc/teamcode/hardware/RobotChassis.java | 133 ++++++++++++++++++ .../ftc/teamcode/hardware/RobotTop.java | 59 ++++++++ .../ftc/teamcode/hardware/TimedServo.java | 126 ----------------- .../ftc/teamcode/task/OneTimeTask.java | 14 -- .../ftc/teamcode/task/SleepTask.java | 19 --- .../firstinspires/ftc/teamcode/task/Task.java | 35 ----- build.gradle | 10 +- 9 files changed, 243 insertions(+), 309 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/OneTimeTask.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java new file mode 100644 index 000000000000..d894f1e7731c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.hardware.RobotChassis; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + +@TeleOp +public class ManualOpMode extends LinearOpMode { + RobotChassis robotChassis; + RobotTop robotTop; + + @Override + public void runOpMode() { + robotChassis = new RobotChassis(this); + robotTop = new RobotTop(this); + + int liftPosition; + LiftState liftState = LiftState.BOTTOM; + waitForStart(); + while (opModeIsActive()){ + robotChassis.driveRobot(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + liftPosition = robotTop.getLiftPosition(); + if(liftState == LiftState.BOTTOM){ + if(gamepad1.y && liftPosition <= 1100){ + robotTop.setLeftPower(0.7); + } else if (liftPosition >= 1100) { + robotTop.setLeftPower(0); + liftState = LiftState.TOP; + } + } else if (liftState == LiftState.TOP) { + if(gamepad1.y && liftPosition >= 100){ + robotTop.setLeftPower(-0.5); + } else if (liftPosition <= 100) { + robotTop.setLeftPower(0); + liftState = LiftState.BOTTOM; + } + } + sleep(10); + } + } +} +enum LiftState{ + BOTTOM,TOP +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java deleted file mode 100644 index 47ec4f901bdd..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ /dev/null @@ -1,110 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import android.util.ArrayMap; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.task.Task; - -import java.util.List; -import java.util.Map; -import java.util.stream.Collectors; - -public abstract class TaskOpMode extends LinearOpMode { - - protected Map tasks; - protected Map taskLinks; - - public TaskOpMode() { - tasks = new ArrayMap<>(); - taskLinks = new ArrayMap<>(); - } - - public abstract void linearInit(); - - public void linearStart() { - } - - public abstract void linearLoop(); - - public void linearStop() { - } - - @Override - public void runOpMode() { - linearInit(); - waitForStart(); - linearStart(); - while (opModeIsActive()) { - for (Map.Entry taskEntry : tasks.entrySet()) { - int taskId = taskEntry.getKey(); - Task task = taskEntry.getValue(); - if (!task.hasNext()) { - task.finish(); - tasks.remove(taskId, task); - taskLinks.remove(taskId); - taskLinks.entrySet().stream() - .filter(e -> e.getValue() == taskId) - .forEach(e -> { - Task nextTask = tasks.get(e.getKey()); - if (nextTask != null) nextTask.init(); - }); - continue; - } - Integer linkedTaskId = taskLinks.get(taskId); - if (tasks.containsKey(linkedTaskId)) continue; - task.iterate(); - } - linearLoop(); - sleep(Task.TICK_MS); - } - linearStop(); - } - - public int registerTask(Task task) { - task.init(); - return registerTask(task, -1); - } - - /** - * Register a task in the task map. All the tasks in the map will be iterated in - * the main loop when it's the right time to do. - * - * @param task the task that need registering. - * @param linkedTaskId the taskId of the linked task. The task being registered will - * be run after the linked task is finished. If nothing provided, - * the registered task will start to run immediately. - * @return the taskId of the registered task. - */ - public int registerTask(Task task, int linkedTaskId) { - int taskId = findMinFreeTaskId(); - tasks.put(taskId, task); - taskLinks.put(taskId, linkedTaskId); - return taskId; - } - - /** - * Cancel the task and all the linked-after tasks if necessary. - * - * @param taskId the taskId of the task needs canceling. - */ - public void cancelTask(int taskId) { - Task task = tasks.get(taskId); - if (task == null) return; - task.cancel(); - tasks.remove(taskId, task); - taskLinks.remove(taskId); - taskLinks.entrySet().stream() - .filter(e -> e.getValue() == taskId) - .forEach(e -> cancelTask(e.getKey())); - } - - protected int findMinFreeTaskId() { - List taskIdList = tasks.keySet().stream().sorted().collect(Collectors.toList()); - int maxTaskId = taskIdList.get(taskIdList.size() - 1); - for (int i = 0; i <= maxTaskId; i++) { - if (taskIdList.contains(i)) return 1; - } - return maxTaskId + 1; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java new file mode 100644 index 000000000000..caeb9089440a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java @@ -0,0 +1,133 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +@SuppressWarnings(value = "unused") +public class RobotChassis { + OpMode opMode; + HardwareMap hardwareMap; + Telemetry telemetry; + private DcMotor leftFrontDrive; + private DcMotor leftBackDrive; + private DcMotor rightFrontDrive; + private DcMotor rightBackDrive; + public RobotChassis(LinearOpMode opMode) { + this.opMode = opMode; + hardwareMap = opMode.hardwareMap; + telemetry = opMode.telemetry; + this.initMovement(); + } + public void initMovement() { + // init motors and servos + leftFrontDrive = hardwareMap.get(DcMotor.class, "FL"); + leftBackDrive = hardwareMap.get(DcMotor.class, "BL"); + rightFrontDrive = hardwareMap.get(DcMotor.class, "FR"); + rightBackDrive = hardwareMap.get(DcMotor.class, "BR"); + + leftFrontDrive.setDirection(DcMotor.Direction.FORWARD); + leftBackDrive.setDirection(DcMotor.Direction.FORWARD); + rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); + rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + + leftFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + leftBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + /** + * Calculates the motor powers required to achieve the requested + * robot motions: Drive (Axial motion) and Turn (Yaw motion). + * Then sends these power levels to the motors. + * + * @param axial Forward/Rev driving power (-1.0 to 1.0) +ve is forward + * @param lateral Right/Left driving power (-1.0 to 1.0) +ve is forward + * @param yaw Right/Left turning power (-1.0 to 1.0) +ve is clockwise + */ + public void driveRobot(double axial, double lateral, double yaw) { + double y = axial; // Remember, Y stick value is reversed + double x = -lateral * 1.1; // Counteract imperfect strafing + double rx = -yaw; + + final double LEFT_REDUCTION = 0.96; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator * LEFT_REDUCTION; + double backLeftPower = (y - x + rx) / denominator * LEFT_REDUCTION; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + leftFrontDrive.setPower(frontLeftPower); + leftBackDrive.setPower(backLeftPower); + rightFrontDrive.setPower(frontRightPower); + rightBackDrive.setPower(backRightPower); + } + + /** + * Set the power of every motor. + * + * @param leftFrontPower Forward/Rev driving power (-1.0 to 1.0) +ve is forward + * @param rightFrontPower Forward/Rev driving power (-1.0 to 1.0) +ve is forward + * @param leftBackPower Forward/Rev driving power (-1.0 to 1.0) +ve is forward + * @param rightBackPower Forward/Rev driving power (-1.0 to 1.0) +ve is forward + * @return The {@link RobotChassis} instance. + */ + public RobotChassis setDrivePower(double leftFrontPower, double rightFrontPower, double leftBackPower, double rightBackPower) { + leftFrontDrive.setPower(leftFrontPower); + rightFrontDrive.setPower(rightFrontPower); + leftBackDrive.setPower(leftBackPower); + rightBackDrive.setPower(rightBackPower); + return this; + } + + /** + * Stop All drive motors. + */ + public void stopMotor() { + setDrivePower(0, 0, 0, 0); + } + + /** + * Set drive wheels run mode for all drive motors. + * + * @param mode The desired run mode. + */ + public void setRunMode(DcMotor.RunMode mode) { + leftFrontDrive.setMode(mode); + rightFrontDrive.setMode(mode); + leftBackDrive.setMode(mode); + rightBackDrive.setMode(mode); + } + + /** + * Set zero power behavior for all drive motors. + * + * @param behavior The desired zero power behaviour. + */ + public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior behavior) { + leftFrontDrive.setZeroPowerBehavior(behavior); + rightFrontDrive.setZeroPowerBehavior(behavior); + leftBackDrive.setZeroPowerBehavior(behavior); + rightBackDrive.setZeroPowerBehavior(behavior); + } + + /** + * Set target position for all drive motors for moving forward. + * + * @param moveCounts The desired increment/decrement. + */ + public void setTargetPosition(int moveCounts) { + // Set Target FIRST, then turn on RUN_TO_POSITION + leftFrontDrive.setTargetPosition(leftFrontDrive.getCurrentPosition() + moveCounts); + rightFrontDrive.setTargetPosition(rightFrontDrive.getCurrentPosition() + moveCounts); + leftBackDrive.setTargetPosition(leftBackDrive.getCurrentPosition() + moveCounts); + rightBackDrive.setTargetPosition(rightBackDrive.getCurrentPosition() + moveCounts); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java new file mode 100644 index 000000000000..12fc7483cbe1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java @@ -0,0 +1,59 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class RobotTop { + OpMode opMode; + HardwareMap hardwareMap; + Telemetry telemetry; + private DcMotor leftLiftMotor; + private DcMotor rightLiftMotor; + private Servo armStretchServo; + + public RobotTop(LinearOpMode opMode) { + this.opMode = opMode; + hardwareMap = opMode.hardwareMap; + telemetry = opMode.telemetry; + this.init(); + } + + public void init() { + leftLiftMotor = hardwareMap.get(DcMotor.class, "leftLift"); + rightLiftMotor = hardwareMap.get(DcMotor.class, "rightLift"); + armStretchServo = hardwareMap.get(Servo.class, "armStretch"); + + leftLiftMotor.setDirection(DcMotor.Direction.FORWARD); + rightLiftMotor.setDirection(DcMotor.Direction.REVERSE); + + leftLiftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightLiftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + leftLiftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightLiftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + public void setLeftPower(double power) { + leftLiftMotor.setPower(power); + rightLiftMotor.setPower(power); + } + + public int getLiftPosition() { + return leftLiftMotor.getCurrentPosition(); + } + + public void setArmStretchPosition(double position){ + armStretchServo.setPosition(position); + } + + public double getArmStretchPosition() { + return armStretchServo.getPosition(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java deleted file mode 100644 index f5f6a641ecc7..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java +++ /dev/null @@ -1,126 +0,0 @@ -package org.firstinspires.ftc.teamcode.hardware; - -import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.hardware.ServoController; - -import org.firstinspires.ftc.teamcode.task.Task; - -public class TimedServo { - protected Servo servo; - - public TimedServo(Servo servo) { - this.servo = servo; - } - - /** - * Sets the logical direction in which this servo operates. - * - * @param direction the direction to set for this servo - * @see #getDirection() - * @see Servo.Direction - */ - public void setDirection(Servo.Direction direction) { - servo.setDirection(direction); - } - - /** - * Returns the current logical direction in which this servo is set as operating. - * - * @return the current logical direction in which this servo is set as operating. - * @see #setDirection(Servo.Direction) - */ - public Servo.Direction getDirection() { - return servo.getDirection(); - } - - /** - * Sets the current position of the servo, expressed as a fraction of its available - * range. If PWM power is enabled for the servo, the servo will attempt to move to - * the indicated position. - * - * @param position the position to which the servo should move, a value in the range [0.0, 1.0] - * @see ServoController#pwmEnable() - * @see #getPosition() - */ - public void setPosition(double position) { - servo.setPosition(position); - } - - /** - * Returns the position to which the servo was last commanded to move. Note that this method - * does NOT read a position from the servo through any electrical means, as no such electrical - * mechanism is, generally, available. - * - * @return the position to which the servo was last commanded to move, or Double.NaN - * if no such position is known - * @see #setPosition(double) - * @see Double#NaN - * @see Double#isNaN() - */ - public double getPosition() { - return servo.getPosition(); - } - - /** - * Scales the available movement range of the servo to be a subset of its maximum range. Subsequent - * positioning calls will operate within that subset range. This is useful if your servo has - * only a limited useful range of movement due to the physical hardware that it is manipulating - * (as is often the case) but you don't want to have to manually scale and adjust the input - * to {@link #setPosition(double) setPosition()} each time. - * - *

For example, if scaleRange(0.2, 0.8) is set; then servo positions will be - * scaled to fit in that range:
- * setPosition(0.0) scales to 0.2
- * setPosition(1.0) scales to 0.8
- * setPosition(0.5) scales to 0.5
- * setPosition(0.25) scales to 0.35
- * setPosition(0.75) scales to 0.65
- *

- * - *

Note the parameters passed here are relative to the underlying full range of motion of - * the servo, not its currently scaled range, if any. Thus, scaleRange(0.0, 1.0) will reset - * the servo to its full range of movement.

- * - * @param min the lower limit of the servo movement range, a value in the interval [0.0, 1.0] - * @param max the upper limit of the servo movement range, a value in the interval [0.0, 1.0] - * @see #setPosition(double) - */ - public void scaleRange(double min, double max) { - servo.scaleRange(min, max); - } - - /** - * Sets the current position of the servo in a fixed time, expressed as a fraction of - * its available range. If PWM power is enabled for the servo, the servo will attempt - * to move to the indicated position. - * - * @param position the position to which the servo should move, a value in the range [0.0, 1.0] - * @param timeMs the time that the servo will take to move. - * @return a Task object - */ - public Task setPosition(double position, long timeMs) { - final double delta = position - servo.getPosition(); - final long iterationCount = timeMs / Task.TICK_MS; - final double perDelta = delta / iterationCount; - return new Task() { - long remainingIterations = iterationCount; - final double targetPosition = position; - - @Override - public void iterate() { - servo.setPosition(servo.getPosition() + perDelta); - remainingIterations--; - } - - @Override - public void finish() { - servo.setPosition(targetPosition); - } - - @Override - public boolean hasNext() { - return remainingIterations > 0; - } - }; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/OneTimeTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/OneTimeTask.java deleted file mode 100644 index 8a1fd39bb8da..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/OneTimeTask.java +++ /dev/null @@ -1,14 +0,0 @@ -package org.firstinspires.ftc.teamcode.task; - -public interface OneTimeTask extends Task { - - void init(); - - @Override - default void iterate() {} - - @Override - default boolean hasNext() { - return false; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java deleted file mode 100644 index 87ed2726201d..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java +++ /dev/null @@ -1,19 +0,0 @@ -package org.firstinspires.ftc.teamcode.task; - -public class SleepTask implements Task { - protected int remainingSleepTicks; - - public SleepTask(int sleepTicks) { - remainingSleepTicks = sleepTicks; - } - - @Override - public void iterate() { - remainingSleepTicks--; - } - - @Override - public boolean hasNext() { - return remainingSleepTicks > 0; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java deleted file mode 100644 index 6776ae6a1b33..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java +++ /dev/null @@ -1,35 +0,0 @@ -package org.firstinspires.ftc.teamcode.task; - -public interface Task { - int TICK_MS = 50; - - /** - * This method is called in the main loop of OpMode - * It should contain the code of EACH TICK, instead of a loop in it! - */ - void iterate(); - - /** - * To judge whether the task needs to continue. - * @return Return true when it need to continue, otherwise false. - */ - boolean hasNext(); - - /** - * This method is called once when the task starts looping in the TaskOpMode. - */ - default void init() { - } - - /** - * The method is called when the task is finished. - */ - default void finish() { - } - - /** - * The method is called when the task is canceled. - */ - default void cancel() { - } -} diff --git a/build.gradle b/build.gradle index c2fef66cf5ff..4303020f15eb 100644 --- a/build.gradle +++ b/build.gradle @@ -6,8 +6,8 @@ buildscript { repositories { - maven { url "https://maven.aliyun.com/repository/central" } - maven { url "https://maven.aliyun.com/repository/google" } +// maven { url "https://maven.aliyun.com/repository/central" } +// maven { url "https://maven.aliyun.com/repository/google" } mavenCentral() google() } @@ -21,14 +21,14 @@ buildscript { // google() repository beginning with version 3.2 of the Android Gradle Plugin allprojects { repositories { - maven { url "https://maven.aliyun.com/repository/central" } - maven { url "https://maven.aliyun.com/repository/google" } +// maven { url "https://maven.aliyun.com/repository/central" } +// maven { url "https://maven.aliyun.com/repository/google" } mavenCentral() google() } } repositories { - maven { url "https://maven.aliyun.com/repository/central" } +// maven { url "https://maven.aliyun.com/repository/central" } mavenCentral() } From 28b48cf8a6c9098d9f6b6f1210b48e455d4bfb7f Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 22 Nov 2024 17:52:53 +0800 Subject: [PATCH 16/93] unfinished --- .../ftc/teamcode/hardware/RobotAuto.java | 84 +++++++++++++++++++ .../ftc/teamcode/hardware/RobotChassis.java | 31 ++++--- .../ftc/teamcode/hardware/RobotTop.java | 1 + 3 files changed, 103 insertions(+), 13 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java new file mode 100644 index 000000000000..b2940ccb7d99 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java @@ -0,0 +1,84 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +@SuppressWarnings(value = "unused") +public class RobotAuto { + LinearOpMode opMode; + HardwareMap hardwareMap; + Telemetry telemetry; + RobotChassis robotChassis; + RobotTop robotTop; + + IMU imu; + public RobotAuto(LinearOpMode opMode) { + this.opMode = opMode; + hardwareMap = opMode.hardwareMap; + telemetry = opMode.telemetry; + this.robotChassis = new RobotChassis(opMode); + this.robotTop = new RobotTop(opMode); + } + + final double COUNTS_PER_INCH = 0; + final double P_DRIVE_GAIN = 0; + + public RobotAuto driveStraight(double maxDriveSpeed, + double distance, + double heading) { + + // Ensure that the OpMode is still active + if (opMode.opModeIsActive()) { + double turnSpeed; + + // Determine new target position, and pass to motor controller + int moveCounts = (int) (distance * COUNTS_PER_INCH); + robotChassis.setTargetPosition(moveCounts); + + robotChassis.setRunMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set the required driving speed (must be positive for RUN_TO_POSITION) + // Start driving straight, and then enter the control loop + maxDriveSpeed = Math.abs(maxDriveSpeed); + robotChassis.driveRobot(maxDriveSpeed, 0, 0); + + // keep looping while we are still active, and BOTH motors are running. + while (opMode.opModeIsActive() && robotChassis.isAllBusy()) { + + // Determine required steering to keep on heading + turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN); + + // if driving in reverse, the motor correction also needs to be reversed + if (distance < 0) + turnSpeed *= -1.0; + + // Apply the turning correction to the current driving speed. + robotChassis.driveRobot(maxDriveSpeed, 0, -turnSpeed); + // telemetry.addData("x","%4.2f, %4.2f, %4.2f, %4.2f, %4d",maxDriveSpeed,distance,heading,turnSpeed,moveCounts); + telemetry.update(); + } + + // Stop all motion & Turn off RUN_TO_POSITION + robotChassis.stopMotor(); + robotChassis.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + return this; + } +// public double getSteeringCorrection(double desiredHeading, double proportionalGain) { +// double targetHeading = desiredHeading; // Save for telemetry +// +// // Determine the heading current error +// headingError = targetHeading - getHeading(); +// +// // Normalize the error to be within +/- 180 degrees +// while (headingError > 180) headingError -= 360; +// while (headingError <= -180) headingError += 360; +// +// // Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0 +// return Range.clip(headingError * proportionalGain, -1, 1); +// } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java index caeb9089440a..c49a48c53265 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.hardware; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -9,7 +8,7 @@ @SuppressWarnings(value = "unused") public class RobotChassis { - OpMode opMode; + LinearOpMode opMode; HardwareMap hardwareMap; Telemetry telemetry; private DcMotor leftFrontDrive; @@ -39,6 +38,14 @@ public void initMovement() { rightFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); } + + public boolean isAllBusy() { + return leftFrontDrive.isBusy() && + leftBackDrive.isBusy() && + rightFrontDrive.isBusy() && + rightBackDrive.isBusy(); + } + /** * Calculates the motor powers required to achieve the requested * robot motions: Drive (Axial motion) and Turn (Yaw motion). @@ -49,20 +56,19 @@ public void initMovement() { * @param yaw Right/Left turning power (-1.0 to 1.0) +ve is clockwise */ public void driveRobot(double axial, double lateral, double yaw) { - double y = axial; // Remember, Y stick value is reversed - double x = -lateral * 1.1; // Counteract imperfect strafing - double rx = -yaw; + lateral = -lateral * 1.1; // Counteract imperfect strafing + yaw = -yaw; final double LEFT_REDUCTION = 0.96; // Denominator is the largest motor power (absolute value) or 1 // This ensures all the powers maintain the same ratio, // but only if at least one is out of the range [-1, 1] - double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); - double frontLeftPower = (y + x + rx) / denominator * LEFT_REDUCTION; - double backLeftPower = (y - x + rx) / denominator * LEFT_REDUCTION; - double frontRightPower = (y - x - rx) / denominator; - double backRightPower = (y + x - rx) / denominator; + double denominator = Math.max(Math.abs(axial) + Math.abs(lateral) + Math.abs(yaw), 1); + double frontLeftPower = (axial + lateral + yaw) / denominator * LEFT_REDUCTION; + double backLeftPower = (axial - lateral + yaw) / denominator * LEFT_REDUCTION; + double frontRightPower = (axial - lateral - yaw) / denominator; + double backRightPower = (axial + lateral - yaw) / denominator; leftFrontDrive.setPower(frontLeftPower); leftBackDrive.setPower(backLeftPower); @@ -77,14 +83,12 @@ public void driveRobot(double axial, double lateral, double yaw) { * @param rightFrontPower Forward/Rev driving power (-1.0 to 1.0) +ve is forward * @param leftBackPower Forward/Rev driving power (-1.0 to 1.0) +ve is forward * @param rightBackPower Forward/Rev driving power (-1.0 to 1.0) +ve is forward - * @return The {@link RobotChassis} instance. */ - public RobotChassis setDrivePower(double leftFrontPower, double rightFrontPower, double leftBackPower, double rightBackPower) { + public void setDrivePower(double leftFrontPower, double rightFrontPower, double leftBackPower, double rightBackPower) { leftFrontDrive.setPower(leftFrontPower); rightFrontDrive.setPower(rightFrontPower); leftBackDrive.setPower(leftBackPower); rightBackDrive.setPower(rightBackPower); - return this; } /** @@ -130,4 +134,5 @@ public void setTargetPosition(int moveCounts) { leftBackDrive.setTargetPosition(leftBackDrive.getCurrentPosition() + moveCounts); rightBackDrive.setTargetPosition(rightBackDrive.getCurrentPosition() + moveCounts); } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java index 12fc7483cbe1..17f1a8c0987f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java @@ -8,6 +8,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; +@SuppressWarnings(value = "unused") public class RobotTop { OpMode opMode; HardwareMap hardwareMap; From fab5a359730aa932e8bcb9df53a54fada93b7873 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Fri, 22 Nov 2024 18:09:39 +0800 Subject: [PATCH 17/93] Feat:Some fix,small progress. --- .../ftc/teamcode/hardware/RobotAuto.java | 53 ++++++++++++++----- 1 file changed, 40 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java index b2940ccb7d99..4970e2f9f2fc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java @@ -6,6 +6,9 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import com.qualcomm.robotcore.util.Range; @SuppressWarnings(value = "unused") public class RobotAuto { @@ -68,17 +71,41 @@ public RobotAuto driveStraight(double maxDriveSpeed, } return this; } -// public double getSteeringCorrection(double desiredHeading, double proportionalGain) { -// double targetHeading = desiredHeading; // Save for telemetry -// -// // Determine the heading current error -// headingError = targetHeading - getHeading(); -// -// // Normalize the error to be within +/- 180 degrees -// while (headingError > 180) headingError -= 360; -// while (headingError <= -180) headingError += 360; -// -// // Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0 -// return Range.clip(headingError * proportionalGain, -1, 1); -// } + public double getSteeringCorrection(double desiredHeading, double proportionalGain) { + double targetHeading = desiredHeading; // Save for telemetry + + // Determine the heading current error + double headingError = targetHeading - getHeading(); + + // Normalize the error to be within +/- 180 degrees + while (headingError > 180) headingError -= 360; + while (headingError <= -180) headingError += 360; + + // Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0 + return Range.clip(headingError * proportionalGain, -1, 1); + } + /** + * Read the robot heading directly from the IMU. + * + * @return The heading of the robot in degrees. + */ + public double getHeading() { + return getHeading(AngleUnit.DEGREES); + } + + /** + * read the Robot heading directly from the IMU + * + * @param unit The desired angle unit (degrees or radians) + * @return The heading of the robot in desired units. + */ + public double getHeading(AngleUnit unit) { + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + telemetry.addData("Yaw/Pitch/Roll", orientation.toString()); + telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); + telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); + telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); + return orientation.getYaw(unit); + } + } From 382c9b929d030963ce6730ae12b8ab9c6a36fe48 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 22 Nov 2024 20:20:40 +0800 Subject: [PATCH 18/93] unfinished --- .../firstinspires/ftc/teamcode/ManualOpMode.java | 15 +++++++++++---- .../ftc/teamcode/hardware/RobotTop.java | 2 +- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index d894f1e7731c..5af69501f122 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -25,18 +25,25 @@ public void runOpMode() { if(liftState == LiftState.BOTTOM){ if(gamepad1.y && liftPosition <= 1100){ robotTop.setLeftPower(0.7); - } else if (liftPosition >= 1100) { + }else if (liftPosition >= 1250) { robotTop.setLeftPower(0); liftState = LiftState.TOP; + } else if (liftPosition >= 1100) { + robotTop.setLeftPower(0.3); } } else if (liftState == LiftState.TOP) { - if(gamepad1.y && liftPosition >= 100){ - robotTop.setLeftPower(-0.5); - } else if (liftPosition <= 100) { + if(gamepad1.y && liftPosition >= 200){ + robotTop.setLeftPower(-0.6); + } else if (liftPosition <= 50) { robotTop.setLeftPower(0); liftState = LiftState.BOTTOM; + }else if (liftPosition <= 200) { + robotTop.setLeftPower(-0.2); } } + telemetry.addData("pos", liftPosition); + telemetry.addData("state", liftState); + telemetry.update(); sleep(10); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java index 17f1a8c0987f..5b4589f64a10 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java @@ -27,7 +27,7 @@ public RobotTop(LinearOpMode opMode) { public void init() { leftLiftMotor = hardwareMap.get(DcMotor.class, "leftLift"); rightLiftMotor = hardwareMap.get(DcMotor.class, "rightLift"); - armStretchServo = hardwareMap.get(Servo.class, "armStretch"); + //armStretchServo = hardwareMap.get(Servo.class, "armStretch"); leftLiftMotor.setDirection(DcMotor.Direction.FORWARD); rightLiftMotor.setDirection(DcMotor.Direction.REVERSE); From 2535541cb015fbd81e87847050e8d0bba99f53e8 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 22 Nov 2024 20:20:40 +0800 Subject: [PATCH 19/93] unfinished --- .../ftc/teamcode/hardware/RobotVision.java | 86 +++++++++++++++++++ 1 file changed, 86 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java new file mode 100644 index 000000000000..e265445bb21e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java @@ -0,0 +1,86 @@ +//这是第一版视觉识别,可以识别摄像头中心的颜色 +//robotVision.getDetectedColor()返回的是一个字符串然后这个字符串是三个类型Unknown,Red，Blue + + +package org.firstinspires.ftc.teamcode.hardware; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvPipeline; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; + + +public class RobotVision { + private OpenCvCamera webcam; + private String detectedColor = "Unknown"; + + public void initialize(HardwareMap hardwareMap) { + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam = OpenCvCameraFactory + .getInstance() + .createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + webcam.setPipeline(new CenterColorPipeline()); + + webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + // 处理摄像头打开错误 + } + }); + } + + public String getDetectedColor() { + return detectedColor; + } + + private class CenterColorPipeline extends OpenCvPipeline { + Mat hsv = new Mat(); + Mat maskRed = new Mat(); + Mat maskBlue = new Mat(); + Scalar lowerRed = new Scalar(0, 100, 100); + Scalar upperRed = new Scalar(10, 255, 255); + Scalar lowerBlue = new Scalar(110, 100, 100); + Scalar upperBlue = new Scalar(130, 255, 255); + + @Override + public Mat processFrame(Mat input) { + Imgproc.cvtColor(input, hsv, Imgproc.COLOR_RGB2HSV); + + // 获取图像中心区域 + Rect centerRect = new Rect(input.width() / 2 - 20, input.height() / 2 - 20, 40, 40); + Mat centerMat = hsv.submat(centerRect); + + // 检测红色和蓝色 + Core.inRange(centerMat, lowerRed, upperRed, maskRed); + Core.inRange(centerMat, lowerBlue, upperBlue, maskBlue); + + double redValue = Core.mean(maskRed).val[0]; + double blueValue = Core.mean(maskBlue).val[0]; + + if (redValue > blueValue) { + detectedColor = "Red"; + } else if (blueValue > redValue) { + detectedColor = "Blue"; + } else { + detectedColor = "Unknown"; + } + + return input; + } + } +} + From 0798c82b6969b994ce8c71d635a9c5f67b95a4df Mon Sep 17 00:00:00 2001 From: Ehicy Date: Sun, 24 Nov 2024 20:00:56 +0800 Subject: [PATCH 20/93] =?UTF-8?q?=E8=BF=99=E6=98=AF=E7=AC=AC=E4=B8=80?= =?UTF-8?q?=E7=89=88=E8=A7=86=E8=A7=89=E8=AF=86=E5=88=AB,=E5=8F=AF?= =?UTF-8?q?=E4=BB=A5=E8=AF=86=E5=88=AB=E6=91=84=E5=83=8F=E5=A4=B4=E4=B8=AD?= =?UTF-8?q?=E5=BF=83=E7=9A=84=E9=A2=9C=E8=89=B2=E3=80=82=20robotVision.get?= =?UTF-8?q?DetectedColor()=E8=BF=94=E5=9B=9E=E7=9A=84=E6=98=AF=E4=B8=80?= =?UTF-8?q?=E4=B8=AA=E5=AD=97=E7=AC=A6=E4=B8=B2=E7=84=B6=E5=90=8E=E8=BF=99?= =?UTF-8?q?=E4=B8=AA=E5=AD=97=E7=AC=A6=E4=B8=B2=E6=98=AF=E4=B8=89=E4=B8=AA?= =?UTF-8?q?=E7=B1=BB=E5=9E=8BUnknown,Red=EF=BC=8CBlue=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../org/firstinspires/ftc/teamcode/hardware/RobotVision.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java index e265445bb21e..8af1f1383326 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java @@ -1,6 +1,6 @@ //这是第一版视觉识别,可以识别摄像头中心的颜色 //robotVision.getDetectedColor()返回的是一个字符串然后这个字符串是三个类型Unknown,Red，Blue - +//是这样的 package org.firstinspires.ftc.teamcode.hardware; From e937287fe3ddf63096bb634a424d9b45766bd8a3 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Sun, 24 Nov 2024 20:03:43 +0800 Subject: [PATCH 21/93] =?UTF-8?q?=E8=BF=99=E6=98=AF=E7=AC=AC=E4=B8=80?= =?UTF-8?q?=E7=89=88=E8=A7=86=E8=A7=89=E8=AF=86=E5=88=AB,=E5=8F=AF?= =?UTF-8?q?=E4=BB=A5=E8=AF=86=E5=88=AB=E6=91=84=E5=83=8F=E5=A4=B4=E4=B8=AD?= =?UTF-8?q?=E5=BF=83=E7=9A=84=E9=A2=9C=E8=89=B2=E3=80=82=20robotVision.get?= =?UTF-8?q?DetectedColor()=E8=BF=94=E5=9B=9E=E7=9A=84=E6=98=AF=E4=B8=80?= =?UTF-8?q?=E4=B8=AA=E5=AD=97=E7=AC=A6=E4=B8=B2=E7=84=B6=E5=90=8E=E8=BF=99?= =?UTF-8?q?=E4=B8=AA=E5=AD=97=E7=AC=A6=E4=B8=B2=E6=98=AF=E4=B8=89=E4=B8=AA?= =?UTF-8?q?=E7=B1=BB=E5=9E=8BUnknown,Red=EF=BC=8CBlue=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../org/firstinspires/ftc/teamcode/hardware/RobotVision.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java index 8af1f1383326..e265445bb21e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java @@ -1,6 +1,6 @@ //这是第一版视觉识别,可以识别摄像头中心的颜色 //robotVision.getDetectedColor()返回的是一个字符串然后这个字符串是三个类型Unknown,Red，Blue -//是这样的 + package org.firstinspires.ftc.teamcode.hardware; From 590a57e9f68bf34dd3226873eeb502413819f4e4 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Sun, 24 Nov 2024 20:07:24 +0800 Subject: [PATCH 22/93] =?UTF-8?q?=E8=BF=99=E6=98=AF=E7=AC=AC=E4=B8=80?= =?UTF-8?q?=E7=89=88=E8=A7=86=E8=A7=89=E8=AF=86=E5=88=AB,=E5=8F=AF?= =?UTF-8?q?=E4=BB=A5=E8=AF=86=E5=88=AB=E6=91=84=E5=83=8F=E5=A4=B4=E4=B8=AD?= =?UTF-8?q?=E5=BF=83=E7=9A=84=E9=A2=9C=E8=89=B2=E3=80=82=20robotVision.get?= =?UTF-8?q?DetectedColor()=E8=BF=94=E5=9B=9E=E7=9A=84=E6=98=AF=E4=B8=80?= =?UTF-8?q?=E4=B8=AA=E5=AD=97=E7=AC=A6=E4=B8=B2=E7=84=B6=E5=90=8E=E8=BF=99?= =?UTF-8?q?=E4=B8=AA=E5=AD=97=E7=AC=A6=E4=B8=B2=E6=98=AF=E4=B8=89=E4=B8=AA?= =?UTF-8?q?=E7=B1=BB=E5=9E=8BUnknown,Red=EF=BC=8CBlue=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../org/firstinspires/ftc/teamcode/hardware/RobotVision.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java index e265445bb21e..15656bdc6dd3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java @@ -1,6 +1,6 @@ //这是第一版视觉识别,可以识别摄像头中心的颜色 //robotVision.getDetectedColor()返回的是一个字符串然后这个字符串是三个类型Unknown,Red，Blue - +// package org.firstinspires.ftc.teamcode.hardware; From 84bed6c4d4cd1f0a6a93d414c91b821ec49480d9 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Mon, 25 Nov 2024 13:32:18 +0800 Subject: [PATCH 23/93] =?UTF-8?q?=E8=BF=99=E6=98=AF=E7=AC=AC=E4=BA=8C?= =?UTF-8?q?=E7=89=88=E8=A7=86=E8=A7=89=E8=AF=86=E5=88=AB,=E5=8F=AF?= =?UTF-8?q?=E4=BB=A5=E8=AF=86=E5=88=AB=E6=91=84=E5=83=8F=E5=A4=B4=E4=B8=AD?= =?UTF-8?q?=E5=BF=83=E7=9A=84=E9=A2=9C=E8=89=B2=20robotVision.getDetectedC?= =?UTF-8?q?olor()=E8=BF=94=E5=9B=9E=E7=9A=84=E6=98=AF=E4=B8=80=E4=B8=AA?= =?UTF-8?q?=E5=AD=97=E7=AC=A6=E4=B8=B2=E7=84=B6=E5=90=8E=E8=BF=99=E4=B8=AA?= =?UTF-8?q?=E5=AD=97=E7=AC=A6=E4=B8=B2=E6=98=AF=E5=9B=9B=E4=B8=AA=E7=B1=BB?= =?UTF-8?q?=E5=9E=8BUnknown,Red=EF=BC=8CBlue=EF=BC=8CYellow?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ftc/teamcode/hardware/RobotVision.java | 22 +++++++++++-------- build.dependencies.gradle | 3 ++- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java index 15656bdc6dd3..283bab7f45de 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java @@ -1,13 +1,12 @@ -//这是第一版视觉识别,可以识别摄像头中心的颜色 -//robotVision.getDetectedColor()返回的是一个字符串然后这个字符串是三个类型Unknown,Red，Blue -// +//这是第二版视觉识别,可以识别摄像头中心的颜色 +//robotVision.getDetectedColor()返回的是一个字符串然后这个字符串是四个类型Unknown,Red，Blue，Yellow + package org.firstinspires.ftc.teamcode.hardware; import com.qualcomm.robotcore.hardware.HardwareMap; import org.opencv.core.Core; import org.opencv.core.Mat; -import org.opencv.core.Point; import org.opencv.core.Rect; import org.opencv.core.Scalar; import org.opencv.imgproc.Imgproc; @@ -17,7 +16,6 @@ import org.openftc.easyopencv.OpenCvPipeline; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; - public class RobotVision { private OpenCvCamera webcam; private String detectedColor = "Unknown"; @@ -51,10 +49,13 @@ private class CenterColorPipeline extends OpenCvPipeline { Mat hsv = new Mat(); Mat maskRed = new Mat(); Mat maskBlue = new Mat(); + Mat maskYellow = new Mat(); // 增加黄色检测的mask Scalar lowerRed = new Scalar(0, 100, 100); Scalar upperRed = new Scalar(10, 255, 255); Scalar lowerBlue = new Scalar(110, 100, 100); Scalar upperBlue = new Scalar(130, 255, 255); + Scalar lowerYellow = new Scalar(20, 100, 100); // 黄色的HSV范围 + Scalar upperYellow = new Scalar(30, 255, 255); @Override public Mat processFrame(Mat input) { @@ -64,17 +65,21 @@ public Mat processFrame(Mat input) { Rect centerRect = new Rect(input.width() / 2 - 20, input.height() / 2 - 20, 40, 40); Mat centerMat = hsv.submat(centerRect); - // 检测红色和蓝色 + // 检测红色、蓝色和黄色 Core.inRange(centerMat, lowerRed, upperRed, maskRed); Core.inRange(centerMat, lowerBlue, upperBlue, maskBlue); + Core.inRange(centerMat, lowerYellow, upperYellow, maskYellow); double redValue = Core.mean(maskRed).val[0]; double blueValue = Core.mean(maskBlue).val[0]; + double yellowValue = Core.mean(maskYellow).val[0]; - if (redValue > blueValue) { + if (redValue > blueValue && redValue > yellowValue) { detectedColor = "Red"; - } else if (blueValue > redValue) { + } else if (blueValue > redValue && blueValue > yellowValue) { detectedColor = "Blue"; + } else if (yellowValue > redValue && yellowValue > blueValue) { + detectedColor = "Yellow"; } else { detectedColor = "Unknown"; } @@ -83,4 +88,3 @@ public Mat processFrame(Mat input) { } } } - diff --git a/build.dependencies.gradle b/build.dependencies.gradle index a2b4ea1130fc..ad4b55d5c9d5 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -12,6 +12,7 @@ dependencies { implementation 'org.firstinspires.ftc:Hardware:10.1.0' implementation 'org.firstinspires.ftc:FtcCommon:10.1.0' implementation 'org.firstinspires.ftc:Vision:10.1.0' - implementation 'androidx.appcompat:appcompat:1.2.0' + implementation 'androidx.appcompat:appcompat:1.7.0' + implementation 'org.openftc:easyopencv:1.7.3' } From 74ab281291240d81d3f7d3a3d68e186878239766 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Mon, 25 Nov 2024 21:12:40 +0800 Subject: [PATCH 24/93] 1111 --- .../org/firstinspires/ftc/teamcode/TEST2.java | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java new file mode 100644 index 000000000000..021e54ffab40 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.hardware.RobotVision; + +@TeleOp + +public class TEST2 extends LinearOpMode { + private RobotVision robotVision; + + @Override + public void runOpMode() { + robotVision = new RobotVision(); + + robotVision.initialize(hardwareMap); // 初始化摄像头 + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + // 显示颜色检测结果 + telemetry.addData("Center Color", robotVision.getDetectedColor()); + telemetry.update(); + + sleep(100); + } + } +} + + From 2ff6a2c3c2cec88c585d0f2dc5568aba717b3bba Mon Sep 17 00:00:00 2001 From: Ehicy Date: Mon, 25 Nov 2024 21:13:24 +0800 Subject: [PATCH 25/93] wtf --- .../src/main/java/org/firstinspires/ftc/teamcode/TEST2.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java index 021e54ffab40..f235c65895b5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java @@ -1,5 +1,5 @@ package org.firstinspires.ftc.teamcode; - +// import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.hardware.RobotVision; From 1d0d8e71317da314e52e35501db9d69d4f3c4518 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Tue, 26 Nov 2024 19:57:59 +0800 Subject: [PATCH 26/93] Feat:Add some exegeses --- .../src/main/java/org/firstinspires/ftc/teamcode/TEST2.java | 3 ++- .../org/firstinspires/ftc/teamcode/hardware/RobotAuto.java | 1 - .../org/firstinspires/ftc/teamcode/hardware/RobotChassis.java | 1 + .../java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java | 1 + 4 files changed, 4 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java index f235c65895b5..903b96831e4c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java @@ -1,5 +1,6 @@ +//测试摄像头颜色识别功能 package org.firstinspires.ftc.teamcode; -// + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.hardware.RobotVision; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java index 4970e2f9f2fc..2cc095daa296 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java @@ -92,7 +92,6 @@ public double getSteeringCorrection(double desiredHeading, double proportionalGa public double getHeading() { return getHeading(AngleUnit.DEGREES); } - /** * read the Robot heading directly from the IMU * diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java index c49a48c53265..c4766bac9339 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java @@ -1,3 +1,4 @@ +//底盘代码 package org.firstinspires.ftc.teamcode.hardware; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java index 5b4589f64a10..fc06aa9074b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java @@ -1,3 +1,4 @@ +//底盘上部代码 package org.firstinspires.ftc.teamcode.hardware; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; From 192cd210b8873f7c446c4cd9c37508f82b4d7e2c Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Tue, 26 Nov 2024 20:46:48 +0800 Subject: [PATCH 27/93] Feat:Add some armServos in RobotTop.java --- .../ftc/teamcode/hardware/RobotTop.java | 43 +++++++++++++++++-- 1 file changed, 39 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java index fc06aa9074b4..10c56aff61d2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java @@ -17,6 +17,10 @@ public class RobotTop { private DcMotor leftLiftMotor; private DcMotor rightLiftMotor; private Servo armStretchServo; + private Servo armTurnServo; + private Servo armSpinXServo; + private Servo armSpinYServo; + private Servo armGrabServo; public RobotTop(LinearOpMode opMode) { this.opMode = opMode; @@ -28,7 +32,12 @@ public RobotTop(LinearOpMode opMode) { public void init() { leftLiftMotor = hardwareMap.get(DcMotor.class, "leftLift"); rightLiftMotor = hardwareMap.get(DcMotor.class, "rightLift"); - //armStretchServo = hardwareMap.get(Servo.class, "armStretch"); + armStretchServo = hardwareMap.get(Servo.class, "Stretch"); + armTurnServo = hardwareMap.get(Servo.class, "Turn"); + armSpinXServo = hardwareMap.get(Servo.class,"SpinX"); + armSpinYServo = hardwareMap.get(Servo.class,"SpinY"); + armGrabServo = hardwareMap.get(Servo.class,"Grab"); + leftLiftMotor.setDirection(DcMotor.Direction.FORWARD); rightLiftMotor.setDirection(DcMotor.Direction.REVERSE); @@ -46,7 +55,6 @@ public void setLeftPower(double power) { leftLiftMotor.setPower(power); rightLiftMotor.setPower(power); } - public int getLiftPosition() { return leftLiftMotor.getCurrentPosition(); } @@ -54,8 +62,35 @@ public int getLiftPosition() { public void setArmStretchPosition(double position){ armStretchServo.setPosition(position); } - - public double getArmStretchPosition() { + public double getarmStretchPosition() { return armStretchServo.getPosition(); } + + public void setArmTurnPosition(double position){ + armTurnServo.setPosition(position); + } + public double getArmTurnPosition() { + return armTurnServo.getPosition(); + } + + public void setArmSpinXServoPosition(double position){ + armTurnServo.setPosition(position); + } + public double getArmSpinXServoPosition() { + return armTurnServo.getPosition(); + } + + public void setArmSpinYServoPosition(double position){ + armTurnServo.setPosition(position); + } + public double getArmSpinYServoPosition() { + return armTurnServo.getPosition(); + } + + public void setArmGrabServoPosition(double position){ + armTurnServo.setPosition(position); + } + public double getArmGrabServoPosition() { + return armTurnServo.getPosition(); + } } From d5cdeb8283f051912a551f46bac70d668be2439c Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Wed, 27 Nov 2024 21:00:41 +0800 Subject: [PATCH 28/93] Feat:Add roadrunner --- TeamCode/build.gradle | 4 + .../ftc/teamcode/Tests/ServoTests.java | 38 + .../ftc/teamcode/drive/DriveConstants.java | 94 +++ .../teamcode/drive/SampleMecanumDrive.java | 311 ++++++++ .../ftc/teamcode/drive/SampleTankDrive.java | 305 ++++++++ .../drive/StandardTrackingWheelLocalizer.java | 99 +++ .../opmode/AutomaticFeedforwardTuner.java | 221 ++++++ .../teamcode/drive/opmode/BackAndForth.java | 52 ++ .../drive/opmode/DriveVelocityPIDTuner.java | 171 +++++ .../drive/opmode/FollowerPIDTuner.java | 55 ++ .../drive/opmode/LocalizationTest.java | 45 ++ .../drive/opmode/ManualFeedforwardTuner.java | 154 ++++ .../drive/opmode/MaxAngularVeloTuner.java | 73 ++ .../drive/opmode/MaxVelocityTuner.java | 84 +++ .../drive/opmode/MotorDirectionDebugger.java | 93 +++ .../ftc/teamcode/drive/opmode/SplineTest.java | 38 + .../ftc/teamcode/drive/opmode/StrafeTest.java | 46 ++ .../teamcode/drive/opmode/StraightTest.java | 46 ++ .../drive/opmode/TrackWidthTuner.java | 88 +++ .../TrackingWheelForwardOffsetTuner.java | 104 +++ .../TrackingWheelLateralDistanceTuner.java | 130 ++++ .../ftc/teamcode/drive/opmode/TurnTest.java | 27 + .../EmptySequenceException.java | 4 + .../TrajectorySequence.java | 44 ++ .../TrajectorySequenceBuilder.java | 679 ++++++++++++++++++ .../TrajectorySequenceRunner.java | 306 ++++++++ .../sequencesegment/SequenceSegment.java | 41 ++ .../sequencesegment/TrajectorySegment.java | 20 + .../sequencesegment/TurnSegment.java | 36 + .../sequencesegment/WaitSegment.java | 12 + .../util/AssetsTrajectoryManager.java | 70 ++ .../ftc/teamcode/util/AxesSigns.java | 45 ++ .../ftc/teamcode/util/AxisDirection.java | 8 + .../ftc/teamcode/util/DashboardUtil.java | 54 ++ .../ftc/teamcode/util/Encoder.java | 125 ++++ .../ftc/teamcode/util/LogFiles.java | 273 +++++++ .../ftc/teamcode/util/LoggingUtil.java | 60 ++ .../ftc/teamcode/util/LynxModuleUtil.java | 124 ++++ .../ftc/teamcode/util/RegressionUtil.java | 156 ++++ build.dependencies.gradle | 4 + 40 files changed, 4339 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/ServoTests.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 878287a7c712..4684f01ec788 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -25,4 +25,8 @@ android { dependencies { implementation project(':FtcRobotController') + implementation 'org.apache.commons:commons-math3:3.6.1' + implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' + implementation 'com.acmerobotics.roadrunner:core:0.5.7' + implementation 'com.acmerobotics.roadrunner:quickstart:0.5.7' } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/ServoTests.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/ServoTests.java new file mode 100644 index 000000000000..cb6c757d81df --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/ServoTests.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.Tests; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; + +public class ServoTests extends LinearOpMode { + + private Servo servo1; + private double position = 0.5; + private final double step = 0.01; + + @Override + public void runOpMode() { + // 初始化舵机 + servo1 = hardwareMap.get(Servo.class, "servo1"); + + waitForStart(); + + while (opModeIsActive()) { + // 控制舵机位置 + if (gamepad1.a && position + step <= 1.0) { + + position += step; + servo1.setPosition(position); + sleep(50); + } else if (gamepad1.b && position - step >= 0) { + + position -= step; + servo1.setPosition(position); + sleep(50); + } + + // 添加其他舵机控制逻辑 + telemetry.addData("Servo Position", servo1.getPosition()); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java new file mode 100644 index 000000000000..4681679a2103 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java @@ -0,0 +1,94 @@ +package org.firstinspires.ftc.teamcode.drive; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; + +/* + * Constants shared between multiple drive types. + * + * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final + * fields may also be edited through the dashboard (connect to the robot's WiFi network and + * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you + * adjust them in the dashboard; **config variable changes don't persist between app restarts**. + * + * These are not the only parameters; some are located in the localizer classes, drive base classes, + * and op modes themselves. + */ +@Config +public class DriveConstants { + + /* + * These are motor constants that should be listed online for your motors. + */ + public static final double TICKS_PER_REV = 1; + public static final double MAX_RPM = 1; + + /* + * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. + * Set this flag to false if drive encoders are not present and an alternative localization + * method is in use (e.g., tracking wheels). + * + * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients + * from DriveVelocityPIDTuner. + */ + public static final boolean RUN_USING_ENCODER = false; + public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0, + getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV)); + + /* + * These are physical constants that can be determined from your robot (including the track + * width; it will be tune empirically later although a rough estimate is important). Users are + * free to chose whichever linear distance unit they would like so long as it is consistently + * used. The default values were selected with inches in mind. Road runner uses radians for + * angular distances although most angular parameters are wrapped in Math.toRadians() for + * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. + */ + public static double WHEEL_RADIUS = 2; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed + public static double TRACK_WIDTH = 1; // in + + /* + * These are the feedforward parameters used to model the drive motor behavior. If you are using + * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive + * motor encoders or have elected not to use them for velocity control, these values should be + * empirically tuned. + */ + public static double kV = 1.0 / rpmToVelocity(MAX_RPM); + public static double kA = 0; + public static double kStatic = 0; + + /* + * These values are used to generate the trajectories for you robot. To ensure proper operation, + * the constraints should never exceed ~80% of the robot's actual capabilities. While Road + * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start + * small and gradually increase them later after everything is working. All distance units are + * inches. + */ + public static double MAX_VEL = 30; + public static double MAX_ACCEL = 30; + public static double MAX_ANG_VEL = Math.toRadians(60); + public static double MAX_ANG_ACCEL = Math.toRadians(60); + + /* + * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. + */ + public static RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR = + RevHubOrientationOnRobot.LogoFacingDirection.UP; + public static RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR = + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + public static double rpmToVelocity(double rpm) { + return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0; + } + + public static double getMotorVelocityF(double ticksPerSecond) { + // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx + return 32767 / ticksPerSecond; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java new file mode 100644 index 000000000000..fcd18aed3cc9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java @@ -0,0 +1,311 @@ +package org.firstinspires.ftc.teamcode.drive; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.drive.DriveSignal; +import com.acmerobotics.roadrunner.drive.MecanumDrive; +import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower; +import com.acmerobotics.roadrunner.followers.TrajectoryFollower; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner; +import org.firstinspires.ftc.teamcode.util.LynxModuleUtil; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; + +/* + * Simple mecanum drive hardware implementation for REV hardware. + */ +@Config +public class SampleMecanumDrive extends MecanumDrive { + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); + + public static double LATERAL_MULTIPLIER = 1; + + public static double VX_WEIGHT = 1; + public static double VY_WEIGHT = 1; + public static double OMEGA_WEIGHT = 1; + + private TrajectorySequenceRunner trajectorySequenceRunner; + + private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH); + private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL); + + private TrajectoryFollower follower; + + private DcMotorEx leftFront, leftRear, rightRear, rightFront; + private List motors; + + private IMU imu; + private VoltageSensor batteryVoltageSensor; + + private List lastEncPositions = new ArrayList<>(); + private List lastEncVels = new ArrayList<>(); + + public SampleMecanumDrive(HardwareMap hardwareMap) { + super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER); + + follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID, + new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5); + + LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + // TODO: adjust the names of the following hardware devices to match your configuration + imu = hardwareMap.get(IMU.class, "imu"); + IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR)); + imu.initialize(parameters); + + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + + motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + if (RUN_USING_ENCODER) { + setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) { + setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + } + + // TODO: reverse any motors using DcMotor.setDirection() + + List lastTrackingEncPositions = new ArrayList<>(); + List lastTrackingEncVels = new ArrayList<>(); + + // TODO: if desired, use setLocalizer() to change the localization method + // setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels)); + + trajectorySequenceRunner = new TrajectorySequenceRunner( + follower, HEADING_PID, batteryVoltageSensor, + lastEncPositions, lastEncVels, lastTrackingEncPositions, lastTrackingEncVels + ); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { + return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) { + return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) { + return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT); + } + + public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) { + return new TrajectorySequenceBuilder( + startPose, + VEL_CONSTRAINT, ACCEL_CONSTRAINT, + MAX_ANG_VEL, MAX_ANG_ACCEL + ); + } + + public void turnAsync(double angle) { + trajectorySequenceRunner.followTrajectorySequenceAsync( + trajectorySequenceBuilder(getPoseEstimate()) + .turn(angle) + .build() + ); + } + + public void turn(double angle) { + turnAsync(angle); + waitForIdle(); + } + + public void followTrajectoryAsync(Trajectory trajectory) { + trajectorySequenceRunner.followTrajectorySequenceAsync( + trajectorySequenceBuilder(trajectory.start()) + .addTrajectory(trajectory) + .build() + ); + } + + public void followTrajectory(Trajectory trajectory) { + followTrajectoryAsync(trajectory); + waitForIdle(); + } + + public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) { + trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence); + } + + public void followTrajectorySequence(TrajectorySequence trajectorySequence) { + followTrajectorySequenceAsync(trajectorySequence); + waitForIdle(); + } + + public Pose2d getLastError() { + return trajectorySequenceRunner.getLastPoseError(); + } + + public void update() { + updatePoseEstimate(); + DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity()); + if (signal != null) setDriveSignal(signal); + } + + public void waitForIdle() { + while (!Thread.currentThread().isInterrupted() && isBusy()) + update(); + } + + public boolean isBusy() { + return trajectorySequenceRunner.isBusy(); + } + + public void setMode(DcMotor.RunMode runMode) { + for (DcMotorEx motor : motors) { + motor.setMode(runMode); + } + } + + public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(zeroPowerBehavior); + } + } + + public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) { + PIDFCoefficients compensatedCoefficients = new PIDFCoefficients( + coefficients.p, coefficients.i, coefficients.d, + coefficients.f * 12 / batteryVoltageSensor.getVoltage() + ); + + for (DcMotorEx motor : motors) { + motor.setPIDFCoefficients(runMode, compensatedCoefficients); + } + } + + public void setWeightedDrivePower(Pose2d drivePower) { + Pose2d vel = drivePower; + + if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY()) + + Math.abs(drivePower.getHeading()) > 1) { + // re-normalize the powers according to the weights + double denom = VX_WEIGHT * Math.abs(drivePower.getX()) + + VY_WEIGHT * Math.abs(drivePower.getY()) + + OMEGA_WEIGHT * Math.abs(drivePower.getHeading()); + + vel = new Pose2d( + VX_WEIGHT * drivePower.getX(), + VY_WEIGHT * drivePower.getY(), + OMEGA_WEIGHT * drivePower.getHeading() + ).div(denom); + } + + setDrivePower(vel); + } + + @NonNull + @Override + public List getWheelPositions() { + lastEncPositions.clear(); + + List wheelPositions = new ArrayList<>(); + for (DcMotorEx motor : motors) { + int position = motor.getCurrentPosition(); + lastEncPositions.add(position); + wheelPositions.add(encoderTicksToInches(position)); + } + return wheelPositions; + } + + @Override + public List getWheelVelocities() { + lastEncVels.clear(); + + List wheelVelocities = new ArrayList<>(); + for (DcMotorEx motor : motors) { + int vel = (int) motor.getVelocity(); + lastEncVels.add(vel); + wheelVelocities.add(encoderTicksToInches(vel)); + } + return wheelVelocities; + } + + @Override + public void setMotorPowers(double v, double v1, double v2, double v3) { + leftFront.setPower(v); + leftRear.setPower(v1); + rightRear.setPower(v2); + rightFront.setPower(v3); + } + + @Override + public double getRawExternalHeading() { + return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + } + + @Override + public Double getExternalHeadingVelocity() { + return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate; + } + + public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) { + return new MinVelocityConstraint(Arrays.asList( + new AngularVelocityConstraint(maxAngularVel), + new MecanumVelocityConstraint(maxVel, trackWidth) + )); + } + + public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) { + return new ProfileAccelerationConstraint(maxAccel); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java new file mode 100644 index 000000000000..55aeb3a52163 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java @@ -0,0 +1,305 @@ +package org.firstinspires.ftc.teamcode.drive; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.drive.DriveSignal; +import com.acmerobotics.roadrunner.drive.TankDrive; +import com.acmerobotics.roadrunner.followers.TankPIDVAFollower; +import com.acmerobotics.roadrunner.followers.TrajectoryFollower; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TankVelocityConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner; +import org.firstinspires.ftc.teamcode.util.LynxModuleUtil; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; + +/* + * Simple tank drive hardware implementation for REV hardware. + */ +@Config +public class SampleTankDrive extends TankDrive { + public static PIDCoefficients AXIAL_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients CROSS_TRACK_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); + + public static double VX_WEIGHT = 1; + public static double OMEGA_WEIGHT = 1; + + private TrajectorySequenceRunner trajectorySequenceRunner; + + private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH); + private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(MAX_ACCEL); + + private TrajectoryFollower follower; + + private List motors, leftMotors, rightMotors; + private IMU imu; + + private VoltageSensor batteryVoltageSensor; + + public SampleTankDrive(HardwareMap hardwareMap) { + super(kV, kA, kStatic, TRACK_WIDTH); + + follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_PID, + new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5); + + LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + // TODO: adjust the names of the following hardware devices to match your configuration + imu = hardwareMap.get(IMU.class, "imu"); + IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( + DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR)); + imu.initialize(parameters); + + // add/remove motors depending on your robot (e.g., 6WD) + DcMotorEx leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + DcMotorEx leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + DcMotorEx rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + DcMotorEx rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + + motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); + leftMotors = Arrays.asList(leftFront, leftRear); + rightMotors = Arrays.asList(rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + if (RUN_USING_ENCODER) { + setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) { + setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + } + + // TODO: reverse any motors using DcMotor.setDirection() + + // TODO: if desired, use setLocalizer() to change the localization method + // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...)); + + trajectorySequenceRunner = new TrajectorySequenceRunner( + follower, HEADING_PID, batteryVoltageSensor, + new ArrayList<>(), new ArrayList<>(), new ArrayList<>(), new ArrayList<>() + ); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) { + return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, accelConstraint); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) { + return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, accelConstraint); + } + + public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) { + return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, accelConstraint); + } + + public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) { + return new TrajectorySequenceBuilder( + startPose, + VEL_CONSTRAINT, accelConstraint, + MAX_ANG_VEL, MAX_ANG_ACCEL + ); + } + + public void turnAsync(double angle) { + trajectorySequenceRunner.followTrajectorySequenceAsync( + trajectorySequenceBuilder(getPoseEstimate()) + .turn(angle) + .build() + ); + } + + public void turn(double angle) { + turnAsync(angle); + waitForIdle(); + } + + public void followTrajectoryAsync(Trajectory trajectory) { + trajectorySequenceRunner.followTrajectorySequenceAsync( + trajectorySequenceBuilder(trajectory.start()) + .addTrajectory(trajectory) + .build() + ); + } + + public void followTrajectory(Trajectory trajectory) { + followTrajectoryAsync(trajectory); + waitForIdle(); + } + + public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) { + trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence); + } + + public void followTrajectorySequence(TrajectorySequence trajectorySequence) { + followTrajectorySequenceAsync(trajectorySequence); + waitForIdle(); + } + + public Pose2d getLastError() { + return trajectorySequenceRunner.getLastPoseError(); + } + + + public void update() { + updatePoseEstimate(); + DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity()); + if (signal != null) setDriveSignal(signal); + } + + public void waitForIdle() { + while (!Thread.currentThread().isInterrupted() && isBusy()) + update(); + } + + public boolean isBusy() { + return trajectorySequenceRunner.isBusy(); + } + + public void setMode(DcMotor.RunMode runMode) { + for (DcMotorEx motor : motors) { + motor.setMode(runMode); + } + } + + public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) { + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(zeroPowerBehavior); + } + } + + public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) { + PIDFCoefficients compensatedCoefficients = new PIDFCoefficients( + coefficients.p, coefficients.i, coefficients.d, + coefficients.f * 12 / batteryVoltageSensor.getVoltage() + ); + for (DcMotorEx motor : motors) { + motor.setPIDFCoefficients(runMode, compensatedCoefficients); + } + } + + public void setWeightedDrivePower(Pose2d drivePower) { + Pose2d vel = drivePower; + + if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getHeading()) > 1) { + // re-normalize the powers according to the weights + double denom = VX_WEIGHT * Math.abs(drivePower.getX()) + + OMEGA_WEIGHT * Math.abs(drivePower.getHeading()); + + vel = new Pose2d( + VX_WEIGHT * drivePower.getX(), + 0, + OMEGA_WEIGHT * drivePower.getHeading() + ).div(denom); + } else { + // Ensure the y axis is zeroed out. + vel = new Pose2d(drivePower.getX(), 0, drivePower.getHeading()); + } + + setDrivePower(vel); + } + + @NonNull + @Override + public List getWheelPositions() { + double leftSum = 0, rightSum = 0; + for (DcMotorEx leftMotor : leftMotors) { + leftSum += encoderTicksToInches(leftMotor.getCurrentPosition()); + } + for (DcMotorEx rightMotor : rightMotors) { + rightSum += encoderTicksToInches(rightMotor.getCurrentPosition()); + } + return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size()); + } + + public List getWheelVelocities() { + double leftSum = 0, rightSum = 0; + for (DcMotorEx leftMotor : leftMotors) { + leftSum += encoderTicksToInches(leftMotor.getVelocity()); + } + for (DcMotorEx rightMotor : rightMotors) { + rightSum += encoderTicksToInches(rightMotor.getVelocity()); + } + return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size()); + } + + @Override + public void setMotorPowers(double v, double v1) { + for (DcMotorEx leftMotor : leftMotors) { + leftMotor.setPower(v); + } + for (DcMotorEx rightMotor : rightMotors) { + rightMotor.setPower(v1); + } + } + + @Override + public double getRawExternalHeading() { + return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + } + + @Override + public Double getExternalHeadingVelocity() { + return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate; + } + + public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) { + return new MinVelocityConstraint(Arrays.asList( + new AngularVelocityConstraint(maxAngularVel), + new TankVelocityConstraint(maxVel, trackWidth) + )); + } + + public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) { + return new ProfileAccelerationConstraint(maxAccel); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java new file mode 100644 index 000000000000..03cdd6addc11 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java @@ -0,0 +1,99 @@ +package org.firstinspires.ftc.teamcode.drive; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.util.Encoder; + +import java.util.Arrays; +import java.util.List; + +/* + * Sample tracking wheel localizer implementation assuming the standard configuration: + * + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | + * | | + * | | + * \--------------/ + * + */ +@Config +public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { + public static double TICKS_PER_REV = 0; + public static double WHEEL_RADIUS = 2; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed + + public static double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels + public static double FORWARD_OFFSET = 4; // in; offset of the lateral wheel + + private Encoder leftEncoder, rightEncoder, frontEncoder; + + private List lastEncPositions, lastEncVels; + + public StandardTrackingWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) { + super(Arrays.asList( + new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left + new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right + new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front + )); + + lastEncPositions = lastTrackingEncPositions; + lastEncVels = lastTrackingEncVels; + + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "leftEncoder")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightEncoder")); + frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontEncoder")); + + // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) + } + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + @NonNull + @Override + public List getWheelPositions() { + int leftPos = leftEncoder.getCurrentPosition(); + int rightPos = rightEncoder.getCurrentPosition(); + int frontPos = frontEncoder.getCurrentPosition(); + + lastEncPositions.clear(); + lastEncPositions.add(leftPos); + lastEncPositions.add(rightPos); + lastEncPositions.add(frontPos); + + return Arrays.asList( + encoderTicksToInches(leftPos), + encoderTicksToInches(rightPos), + encoderTicksToInches(frontPos) + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + int leftVel = (int) leftEncoder.getCorrectedVelocity(); + int rightVel = (int) rightEncoder.getCorrectedVelocity(); + int frontVel = (int) frontEncoder.getCorrectedVelocity(); + + lastEncVels.clear(); + lastEncVels.add(leftVel); + lastEncVels.add(rightVel); + lastEncVels.add(frontVel); + + return Arrays.asList( + encoderTicksToInches(leftVel), + encoderTicksToInches(rightVel), + encoderTicksToInches(frontVel) + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java new file mode 100644 index 000000000000..8e0c62140fae --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java @@ -0,0 +1,221 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_RPM; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.rpmToVelocity; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.util.LoggingUtil; +import org.firstinspires.ftc.teamcode.util.RegressionUtil; + +import java.util.ArrayList; +import java.util.List; + +/* + * Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an + * outline of the procedure: + * 1. Slowly ramp the motor power and record encoder values along the way. + * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV) + * and an optional intercept (kStatic). + * 3. Accelerate the robot (apply constant power) and record the encoder counts. + * 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear + * regression. + */ +@Config +@Autonomous(group = "drive") +public class AutomaticFeedforwardTuner extends LinearOpMode { + public static double MAX_POWER = 0.7; + public static double DISTANCE = 100; // in + + @Override + public void runOpMode() throws InterruptedException { + if (RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " + + "when using the built-in drive motor velocity PID."); + } + + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Press play to begin the feedforward tuning routine"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Would you like to fit kStatic?"); + telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no"); + telemetry.update(); + + boolean fitIntercept = false; + while (!isStopRequested()) { + if (gamepad1.y) { + fitIntercept = true; + while (!isStopRequested() && gamepad1.y) { + idle(); + } + break; + } else if (gamepad1.b) { + while (!isStopRequested() && gamepad1.b) { + idle(); + } + break; + } + idle(); + } + + telemetry.clearAll(); + telemetry.addLine(Misc.formatInvariant( + "Place your robot on the field with at least %.2f in of room in front", DISTANCE)); + telemetry.addLine("Press (Y/Δ) to begin"); + telemetry.update(); + + while (!isStopRequested() && !gamepad1.y) { + idle(); + } + while (!isStopRequested() && gamepad1.y) { + idle(); + } + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + double maxVel = rpmToVelocity(MAX_RPM); + double finalVel = MAX_POWER * maxVel; + double accel = (finalVel * finalVel) / (2.0 * DISTANCE); + double rampTime = Math.sqrt(2.0 * DISTANCE / accel); + + List timeSamples = new ArrayList<>(); + List positionSamples = new ArrayList<>(); + List powerSamples = new ArrayList<>(); + + drive.setPoseEstimate(new Pose2d()); + + double startTime = clock.seconds(); + while (!isStopRequested()) { + double elapsedTime = clock.seconds() - startTime; + if (elapsedTime > rampTime) { + break; + } + double vel = accel * elapsedTime; + double power = vel / maxVel; + + timeSamples.add(elapsedTime); + positionSamples.add(drive.getPoseEstimate().getX()); + powerSamples.add(power); + + drive.setDrivePower(new Pose2d(power, 0.0, 0.0)); + drive.updatePoseEstimate(); + } + drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0)); + + RegressionUtil.RampResult rampResult = RegressionUtil.fitRampData( + timeSamples, positionSamples, powerSamples, fitIntercept, + LoggingUtil.getLogFile(Misc.formatInvariant( + "DriveRampRegression-%d.csv", System.currentTimeMillis()))); + + telemetry.clearAll(); + telemetry.addLine("Quasi-static ramp up test complete"); + if (fitIntercept) { + telemetry.addLine(Misc.formatInvariant("kV = %.5f, kStatic = %.5f (R^2 = %.2f)", + rampResult.kV, rampResult.kStatic, rampResult.rSquare)); + } else { + telemetry.addLine(Misc.formatInvariant("kV = %.5f (R^2 = %.2f)", + rampResult.kStatic, rampResult.rSquare)); + } + telemetry.addLine("Would you like to fit kA?"); + telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no"); + telemetry.update(); + + boolean fitAccelFF = false; + while (!isStopRequested()) { + if (gamepad1.y) { + fitAccelFF = true; + while (!isStopRequested() && gamepad1.y) { + idle(); + } + break; + } else if (gamepad1.b) { + while (!isStopRequested() && gamepad1.b) { + idle(); + } + break; + } + idle(); + } + + if (fitAccelFF) { + telemetry.clearAll(); + telemetry.addLine("Place the robot back in its starting position"); + telemetry.addLine("Press (Y/Δ) to continue"); + telemetry.update(); + + while (!isStopRequested() && !gamepad1.y) { + idle(); + } + while (!isStopRequested() && gamepad1.y) { + idle(); + } + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + double maxPowerTime = DISTANCE / maxVel; + + timeSamples.clear(); + positionSamples.clear(); + powerSamples.clear(); + + drive.setPoseEstimate(new Pose2d()); + drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0)); + + startTime = clock.seconds(); + while (!isStopRequested()) { + double elapsedTime = clock.seconds() - startTime; + if (elapsedTime > maxPowerTime) { + break; + } + + timeSamples.add(elapsedTime); + positionSamples.add(drive.getPoseEstimate().getX()); + powerSamples.add(MAX_POWER); + + drive.updatePoseEstimate(); + } + drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0)); + + RegressionUtil.AccelResult accelResult = RegressionUtil.fitAccelData( + timeSamples, positionSamples, powerSamples, rampResult, + LoggingUtil.getLogFile(Misc.formatInvariant( + "DriveAccelRegression-%d.csv", System.currentTimeMillis()))); + + telemetry.clearAll(); + telemetry.addLine("Constant power test complete"); + telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)", + accelResult.kA, accelResult.rSquare)); + telemetry.update(); + } + + while (!isStopRequested()) { + idle(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java new file mode 100644 index 000000000000..a78ad3796bd7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +/* + * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base + * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the + * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer + * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're + * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once + * you've successfully connected, start the program, and your robot will begin moving forward and + * backward. You should observe the target position (green) and your pose estimate (blue) and adjust + * your follower PID coefficients such that you follow the target position as accurately as possible. + * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. + * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. + * These coefficients can be tuned live in dashboard. + * + * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It + * is recommended that you use the FollowerPIDTuner opmode for further fine tuning. + */ +@Config +@Autonomous(group = "drive") +public class BackAndForth extends LinearOpMode { + + public static double DISTANCE = 50; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d()) + .forward(DISTANCE) + .build(); + + Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end()) + .back(DISTANCE) + .build(); + + waitForStart(); + + while (opModeIsActive() && !isStopRequested()) { + drive.followTrajectory(trajectoryForward); + drive.followTrajectory(trajectoryBackward); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java new file mode 100644 index 000000000000..931b742083a9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java @@ -0,0 +1,171 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +import java.util.List; + +/* + * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed- + * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as + * important as the positional parameters. Like the other manual tuning routines, this op mode + * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's + * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC + * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully + * connected, start the program, and your robot will begin moving forward and backward according to + * a motion profile. Your job is to graph the velocity errors over time and adjust the PID + * coefficients (note: the tuning variable will not appear until the op mode finishes initializing). + * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the + * MOTOR_VELO_PID field. + * + * Recommended tuning process: + * + * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to + * mitigate oscillations. + * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached. + * 3. Back off kP and kD a little until the response is less oscillatory (but without lag). + * + * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the + * user to reset the position of the bot in the event that it drifts off the path. + * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. + */ +@Config +@Autonomous(group = "drive") +public class DriveVelocityPIDTuner extends LinearOpMode { + public static double DISTANCE = 72; // in + + enum Mode { + DRIVER_MODE, + TUNING_MODE + } + + private static MotionProfile generateProfile(boolean movingForward) { + MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); + MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); + } + + @Override + public void runOpMode() { + if (!RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" + + "PID is not in use", getClass().getSimpleName()); + } + + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Mode mode = Mode.TUNING_MODE; + + double lastKp = MOTOR_VELO_PID.p; + double lastKi = MOTOR_VELO_PID.i; + double lastKd = MOTOR_VELO_PID.d; + double lastKf = MOTOR_VELO_PID.f; + + drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Ready!"); + telemetry.update(); + telemetry.clearAll(); + + waitForStart(); + + if (isStopRequested()) return; + + boolean movingForwards = true; + MotionProfile activeProfile = generateProfile(true); + double profileStart = clock.seconds(); + + + while (!isStopRequested()) { + telemetry.addData("mode", mode); + + switch (mode) { + case TUNING_MODE: + if (gamepad1.y) { + mode = Mode.DRIVER_MODE; + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + // calculate and set the motor power + double profileTime = clock.seconds() - profileStart; + + if (profileTime > activeProfile.duration()) { + // generate a new profile + movingForwards = !movingForwards; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + MotionState motionState = activeProfile.get(profileTime); + double targetPower = kV * motionState.getV(); + drive.setDrivePower(new Pose2d(targetPower, 0, 0)); + + List velocities = drive.getWheelVelocities(); + + // update telemetry + telemetry.addData("targetVelocity", motionState.getV()); + for (int i = 0; i < velocities.size(); i++) { + telemetry.addData("measuredVelocity" + i, velocities.get(i)); + telemetry.addData( + "error" + i, + motionState.getV() - velocities.get(i) + ); + } + break; + case DRIVER_MODE: + if (gamepad1.b) { + drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + mode = Mode.TUNING_MODE; + movingForwards = true; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + break; + } + + if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d + || lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) { + drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + + lastKp = MOTOR_VELO_PID.p; + lastKi = MOTOR_VELO_PID.i; + lastKd = MOTOR_VELO_PID.d; + lastKf = MOTOR_VELO_PID.f; + } + + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java new file mode 100644 index 000000000000..63f577e16989 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; + +/* + * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base + * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the + * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer + * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're + * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once + * you've successfully connected, start the program, and your robot will begin driving in a square. + * You should observe the target position (green) and your pose estimate (blue) and adjust your + * follower PID coefficients such that you follow the target position as accurately as possible. + * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID. + * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID. + * These coefficients can be tuned live in dashboard. + */ +@Config +@Autonomous(group = "drive") +public class FollowerPIDTuner extends LinearOpMode { + public static double DISTANCE = 48; // in + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0); + + drive.setPoseEstimate(startPose); + + waitForStart(); + + if (isStopRequested()) return; + + while (!isStopRequested()) { + TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose) + .forward(DISTANCE) + .turn(Math.toRadians(90)) + .forward(DISTANCE) + .turn(Math.toRadians(90)) + .forward(DISTANCE) + .turn(Math.toRadians(90)) + .forward(DISTANCE) + .turn(Math.toRadians(90)) + .build(); + drive.followTrajectorySequence(trajSeq); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java new file mode 100644 index 000000000000..8411792bda91 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java @@ -0,0 +1,45 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +/** + * This is a simple teleop routine for testing localization. Drive the robot around like a normal + * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight + * errors are not out of the ordinary, especially with sudden drive motions). The goal of this + * exercise is to ascertain whether the localizer has been configured properly (note: the pure + * encoder localizer heading may be significantly off if the track width has not been tuned). + */ +@TeleOp(group = "drive") +public class LocalizationTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + waitForStart(); + + while (!isStopRequested()) { + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + + drive.update(); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("x", poseEstimate.getX()); + telemetry.addData("y", poseEstimate.getY()); + telemetry.addData("heading", poseEstimate.getHeading()); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java new file mode 100644 index 000000000000..0d01bcb96157 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java @@ -0,0 +1,154 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; +import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.drive.DriveSignal; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.kinematics.Kinematics; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +import java.util.Objects; + +/* + * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary, + * tuning these coefficients is just as important as the positional parameters. Like the other + * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard, + * connect your computer to the RC's WiFi network. In your browser, navigate to + * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if + * you are using the Control Hub. Once you've successfully connected, start the program, and your + * robot will begin moving forward and backward according to a motion profile. Your job is to graph + * the velocity errors over time and adjust the feedforward coefficients. Once you've found a + * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file. + * + * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the + * user to reset the position of the bot in the event that it drifts off the path. + * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. + */ +@Config +@Autonomous(group = "drive") +public class ManualFeedforwardTuner extends LinearOpMode { + public static double DISTANCE = 72; // in + + private FtcDashboard dashboard = FtcDashboard.getInstance(); + + private SampleMecanumDrive drive; + + enum Mode { + DRIVER_MODE, + TUNING_MODE + } + + private Mode mode; + + private static MotionProfile generateProfile(boolean movingForward) { + MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0); + MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0); + return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL); + } + + @Override + public void runOpMode() { + if (RUN_USING_ENCODER) { + RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " + + "when using the built-in drive motor velocity PID."); + } + + Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry()); + + drive = new SampleMecanumDrive(hardwareMap); + + final VoltageSensor voltageSensor = hardwareMap.voltageSensor.iterator().next(); + + mode = Mode.TUNING_MODE; + + NanoClock clock = NanoClock.system(); + + telemetry.addLine("Ready!"); + telemetry.update(); + telemetry.clearAll(); + + waitForStart(); + + if (isStopRequested()) return; + + boolean movingForwards = true; + MotionProfile activeProfile = generateProfile(true); + double profileStart = clock.seconds(); + + + while (!isStopRequested()) { + telemetry.addData("mode", mode); + + switch (mode) { + case TUNING_MODE: + if (gamepad1.y) { + mode = Mode.DRIVER_MODE; + } + + // calculate and set the motor power + double profileTime = clock.seconds() - profileStart; + + if (profileTime > activeProfile.duration()) { + // generate a new profile + movingForwards = !movingForwards; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + MotionState motionState = activeProfile.get(profileTime); + double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic); + + final double NOMINAL_VOLTAGE = 12.0; + final double voltage = voltageSensor.getVoltage(); + drive.setDrivePower(new Pose2d(NOMINAL_VOLTAGE / voltage * targetPower, 0, 0)); + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + double currentVelo = poseVelo.getX(); + + // update telemetry + telemetry.addData("targetVelocity", motionState.getV()); + telemetry.addData("measuredVelocity", currentVelo); + telemetry.addData("error", motionState.getV() - currentVelo); + break; + case DRIVER_MODE: + if (gamepad1.b) { + mode = Mode.TUNING_MODE; + movingForwards = true; + activeProfile = generateProfile(movingForwards); + profileStart = clock.seconds(); + } + + drive.setWeightedDrivePower( + new Pose2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x, + -gamepad1.right_stick_x + ) + ); + break; + } + + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java new file mode 100644 index 000000000000..05d82657803f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java @@ -0,0 +1,73 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +import java.util.Objects; + +/** + * This routine is designed to calculate the maximum angular velocity your bot can achieve under load. + *

+ * Upon pressing start, your bot will turn at max power for RUNTIME seconds. + *

+ * Further fine tuning of MAX_ANG_VEL may be desired. + */ + +@Config +@Autonomous(group = "drive") +public class MaxAngularVeloTuner extends LinearOpMode { + public static double RUNTIME = 4.0; + + private ElapsedTime timer; + private double maxAngVelocity = 0.0; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds."); + telemetry.addLine("Please ensure you have enough space cleared."); + telemetry.addLine(""); + telemetry.addLine("Press start when ready."); + telemetry.update(); + + waitForStart(); + + telemetry.clearAll(); + telemetry.update(); + + drive.setDrivePower(new Pose2d(0, 0, 1)); + timer = new ElapsedTime(); + + while (!isStopRequested() && timer.seconds() < RUNTIME) { + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + + maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity); + } + + drive.setDrivePower(new Pose2d()); + + telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity); + telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity)); + telemetry.addData("Max Recommended Angular Velocity (rad)", maxAngVelocity * 0.8); + telemetry.addData("Max Recommended Angular Velocity (deg)", Math.toDegrees(maxAngVelocity * 0.8)); + telemetry.update(); + + while (!isStopRequested()) idle(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java new file mode 100644 index 000000000000..ddca6cd1a4c3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java @@ -0,0 +1,84 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +import java.util.Objects; + +/** + * This routine is designed to calculate the maximum velocity your bot can achieve under load. It + * will also calculate the effective kF value for your velocity PID. + *

+ * Upon pressing start, your bot will run at max power for RUNTIME seconds. + *

+ * Further fine tuning of kF may be desired. + */ +@Config +@Autonomous(group = "drive") +public class MaxVelocityTuner extends LinearOpMode { + public static double RUNTIME = 2.0; + + private ElapsedTime timer; + private double maxVelocity = 0.0; + + private VoltageSensor batteryVoltageSensor; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); + + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds."); + telemetry.addLine("Please ensure you have enough space cleared."); + telemetry.addLine(""); + telemetry.addLine("Press start when ready."); + telemetry.update(); + + waitForStart(); + + telemetry.clearAll(); + telemetry.update(); + + drive.setDrivePower(new Pose2d(1, 0, 0)); + timer = new ElapsedTime(); + + while (!isStopRequested() && timer.seconds() < RUNTIME) { + drive.updatePoseEstimate(); + + Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer."); + + maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity); + } + + drive.setDrivePower(new Pose2d()); + + double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity)); + + telemetry.addData("Max Velocity", maxVelocity); + telemetry.addData("Max Recommended Velocity", maxVelocity * 0.8); + telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) idle(); + } + + private double veloInchesToTicks(double inchesPerSec) { + return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java new file mode 100644 index 000000000000..023cfccc3825 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java @@ -0,0 +1,93 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +/** + * This is a simple teleop routine for debugging your motor configuration. + * Pressing each of the buttons will power its respective motor. + * + * Button Mappings: + * + * Xbox/PS4 Button - Motor + * X / ▢ - Front Left + * Y / Δ - Front Right + * B / O - Rear Right + * A / X - Rear Left + * The buttons are mapped to match the wheels spatially if you + * were to rotate the gamepad 45deg°. x/square is the front left + * ________ and each button corresponds to the wheel as you go clockwise + * / ______ \ + * ------------.-' _ '-..+ Front of Bot + * / _ ( Y ) _ \ ^ + * | ( X ) _ ( B ) | Front Left \ Front Right + * ___ '. ( A ) /| Wheel \ Wheel + * .' '. '-._____.-' .' (x/▢) \ (Y/Δ) + * | | | \ + * '.___.' '. | Rear Left \ Rear Right + * '. / Wheel \ Wheel + * \. .' (A/X) \ (B/O) + * \________/ + * + * Uncomment the @Disabled tag below to use this opmode. + */ +@Disabled +@Config +@TeleOp(group = "drive") +public class MotorDirectionDebugger extends LinearOpMode { + public static double MOTOR_POWER = 0.7; + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + telemetry.addLine("Press play to begin the debugging opmode"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + + while (!isStopRequested()) { + telemetry.addLine("Press each button to turn on its respective motor"); + telemetry.addLine(); + telemetry.addLine("Xbox/PS4 Button - Motor"); + telemetry.addLine("  X / ▢         - Front Left"); + telemetry.addLine("  Y / Δ         - Front Right"); + telemetry.addLine("  B / O         - Rear  Right"); + telemetry.addLine("  A / X         - Rear  Left"); + telemetry.addLine(); + + if(gamepad1.x) { + drive.setMotorPowers(MOTOR_POWER, 0, 0, 0); + telemetry.addLine("Running Motor: Front Left"); + } else if(gamepad1.y) { + drive.setMotorPowers(0, 0, 0, MOTOR_POWER); + telemetry.addLine("Running Motor: Front Right"); + } else if(gamepad1.b) { + drive.setMotorPowers(0, 0, MOTOR_POWER, 0); + telemetry.addLine("Running Motor: Rear Right"); + } else if(gamepad1.a) { + drive.setMotorPowers(0, MOTOR_POWER, 0, 0); + telemetry.addLine("Running Motor: Rear Left"); + } else { + drive.setMotorPowers(0, 0, 0, 0); + telemetry.addLine("Running Motor: None"); + } + + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java new file mode 100644 index 000000000000..d7316766a18e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +/* + * This is an example of a more complex path to really test the tuning. + */ +@Autonomous(group = "drive") +public class SplineTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + if (isStopRequested()) return; + + Trajectory traj = drive.trajectoryBuilder(new Pose2d()) + .splineTo(new Vector2d(30, 30), 0) + .build(); + + drive.followTrajectory(traj); + + sleep(2000); + + drive.followTrajectory( + drive.trajectoryBuilder(traj.end(), true) + .splineTo(new Vector2d(0, 0), Math.toRadians(180)) + .build() + ); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java new file mode 100644 index 000000000000..992d03aea1db --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +/* + * This is a simple routine to test translational drive capabilities. + */ +@Config +@Autonomous(group = "drive") +public class StrafeTest extends LinearOpMode { + public static double DISTANCE = 60; // in + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) + .strafeRight(DISTANCE) + .build(); + + waitForStart(); + + if (isStopRequested()) return; + + drive.followTrajectory(trajectory); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("finalX", poseEstimate.getX()); + telemetry.addData("finalY", poseEstimate.getY()); + telemetry.addData("finalHeading", poseEstimate.getHeading()); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) ; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java new file mode 100644 index 000000000000..e1b27e1c9a6d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +/* + * This is a simple routine to test translational drive capabilities. + */ +@Config +@Autonomous(group = "drive") +public class StraightTest extends LinearOpMode { + public static double DISTANCE = 60; // in + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) + .forward(DISTANCE) + .build(); + + waitForStart(); + + if (isStopRequested()) return; + + drive.followTrajectory(trajectory); + + Pose2d poseEstimate = drive.getPoseEstimate(); + telemetry.addData("finalX", poseEstimate.getX()); + telemetry.addData("finalY", poseEstimate.getY()); + telemetry.addData("finalHeading", poseEstimate.getHeading()); + telemetry.update(); + + while (!isStopRequested() && opModeIsActive()) ; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java new file mode 100644 index 000000000000..ffce0233c67d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java @@ -0,0 +1,88 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.MovingStatistics; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +/* + * This routine determines the effective track width. The procedure works by executing a point turn + * with a given angle and measuring the difference between that angle and the actual angle (as + * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient + * given angle / actual angle gives a multiplicative adjustment to the estimated track width + * (effective track width = estimated track width * given angle / actual angle). The routine repeats + * this procedure a few times and averages the values for additional accuracy. Note: a relatively + * accurate track width estimate is important or else the angular constraints will be thrown off. + */ +@Config +@Autonomous(group = "drive") +public class TrackWidthTuner extends LinearOpMode { + public static double ANGLE = 180; // deg + public static int NUM_TRIALS = 5; + public static int DELAY = 1000; // ms + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + // TODO: if you haven't already, set the localizer to something that doesn't depend on + // drive encoders for computing the heading + + telemetry.addLine("Press play to begin the track width tuner routine"); + telemetry.addLine("Make sure your robot has enough clearance to turn smoothly"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS); + for (int i = 0; i < NUM_TRIALS; i++) { + drive.setPoseEstimate(new Pose2d()); + + // it is important to handle heading wraparounds + double headingAccumulator = 0; + double lastHeading = 0; + + drive.turnAsync(Math.toRadians(ANGLE)); + + while (!isStopRequested() && drive.isBusy()) { + double heading = drive.getPoseEstimate().getHeading(); + headingAccumulator += Angle.normDelta(heading - lastHeading); + lastHeading = heading; + + drive.update(); + } + + double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator; + trackWidthStats.add(trackWidth); + + sleep(DELAY); + } + + telemetry.clearAll(); + telemetry.addLine("Tuning complete"); + telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)", + trackWidthStats.getMean(), + trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS))); + telemetry.update(); + + while (!isStopRequested()) { + idle(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java new file mode 100644 index 000000000000..ebe97f28b0d9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java @@ -0,0 +1,104 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.MovingStatistics; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer; + +/** + * This routine determines the effective forward offset for the lateral tracking wheel. + * The procedure executes a point turn at a given angle for a certain number of trials, + * along with a specified delay in milliseconds. The purpose of this is to track the + * change in the y position during the turn. The offset, or distance, of the lateral tracking + * wheel from the center or rotation allows the wheel to spin during a point turn, leading + * to an incorrect measurement for the y position. This creates an arc around around + * the center of rotation with an arc length of change in y and a radius equal to the forward + * offset. We can compute this offset by calculating (change in y position) / (change in heading) + * which returns the radius if the angle (change in heading) is in radians. This is based + * on the arc length formula of length = theta * radius. + * + * To run this routine, simply adjust the desired angle and specify the number of trials + * and the desired delay. Then, run the procedure. Once it finishes, it will print the + * average of all the calculated forward offsets derived from the calculation. This calculated + * forward offset is then added onto the current forward offset to produce an overall estimate + * for the forward offset. You can run this procedure as many times as necessary until a + * satisfactory result is produced. + */ +@Config +@Autonomous(group="drive") +public class TrackingWheelForwardOffsetTuner extends LinearOpMode { + public static double ANGLE = 180; // deg + public static int NUM_TRIALS = 5; + public static int DELAY = 1000; // ms + + @Override + public void runOpMode() throws InterruptedException { + Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) { + RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the " + + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" + + "(hardwareMap));\" is called in SampleMecanumDrive.java"); + } + + telemetry.addLine("Press play to begin the forward offset tuner"); + telemetry.addLine("Make sure your robot has enough clearance to turn smoothly"); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.addLine("Running..."); + telemetry.update(); + + MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS); + for (int i = 0; i < NUM_TRIALS; i++) { + drive.setPoseEstimate(new Pose2d()); + + // it is important to handle heading wraparounds + double headingAccumulator = 0; + double lastHeading = 0; + + drive.turnAsync(Math.toRadians(ANGLE)); + + while (!isStopRequested() && drive.isBusy()) { + double heading = drive.getPoseEstimate().getHeading(); + headingAccumulator += Angle.norm(heading - lastHeading); + lastHeading = heading; + + drive.update(); + } + + double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET + + drive.getPoseEstimate().getY() / headingAccumulator; + forwardOffsetStats.add(forwardOffset); + + sleep(DELAY); + } + + telemetry.clearAll(); + telemetry.addLine("Tuning complete"); + telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)", + forwardOffsetStats.getMean(), + forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS))); + telemetry.update(); + + while (!isStopRequested()) { + idle(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java new file mode 100644 index 000000000000..a11059c45986 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java @@ -0,0 +1,130 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer; + +/** + * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s + * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel + * wheels. + * + * Tuning Routine: + * + * 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical + * measured value. This need only be an estimated value as you will be tuning it anyways. + * + * 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an + * similar mark right below the indicator on your bot. This will be your reference point to + * ensure you've turned exactly 360°. + * + * 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help + * identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard, + * connect your computer to the RC's WiFi network. In your browser, navigate to + * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash + * if you are using the Control Hub. + * Ensure the field is showing (select the field view in top right of the page). + * + * 4. Press play to begin the tuning routine. + * + * 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise. + * + * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns. + * + * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators + * on the bot and on the ground you created earlier should be lined up. + * + * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your + * StandardTrackingWheelLocalizer.java class. + * + * 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value + * yourself. Read the heading output and follow the advice stated in the note below to manually + * nudge the values yourself. + * + * Note: + * It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with + * a line from the circumference to the center should be present, representing the bot. The line + * indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in + * dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than + * the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the + * actual bot, the LATERAL_DISTANCE should be increased. + * + * If your drawn bot oscillates around a point in dashboard, don't worry. This is because the + * position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the + * effective center of rotation. You can ignore this effect. The center of rotation will be offset + * slightly but your heading will still be fine. This does not affect your overall tracking + * precision. The heading should still line up. + */ +@Config +@TeleOp(group = "drive") +public class TrackingWheelLateralDistanceTuner extends LinearOpMode { + public static int NUM_TURNS = 10; + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) { + RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the " + + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer" + + "(hardwareMap));\" is called in SampleMecanumDrive.java"); + } + + telemetry.addLine("Prior to beginning the routine, please read the directions " + + "located in the comments of the opmode file."); + telemetry.addLine("Press play to begin the tuning routine."); + telemetry.addLine(""); + telemetry.addLine("Press Y/△ to stop the routine."); + telemetry.update(); + + waitForStart(); + + if (isStopRequested()) return; + + telemetry.clearAll(); + telemetry.update(); + + double headingAccumulator = 0; + double lastHeading = 0; + + boolean tuningFinished = false; + + while (!isStopRequested() && !tuningFinished) { + Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x); + drive.setDrivePower(vel); + + drive.update(); + + double heading = drive.getPoseEstimate().getHeading(); + double deltaHeading = heading - lastHeading; + + headingAccumulator += Angle.normDelta(deltaHeading); + lastHeading = heading; + + telemetry.clearAll(); + telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator)); + telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading)); + telemetry.addLine(); + telemetry.addLine("Press Y/△ to conclude routine"); + telemetry.update(); + + if (gamepad1.y) + tuningFinished = true; + } + + telemetry.clearAll(); + telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°"); + telemetry.addLine("Effective LATERAL_DISTANCE: " + + (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE); + + telemetry.update(); + + while (!isStopRequested()) idle(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java new file mode 100644 index 000000000000..0637d19efea4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.drive.opmode; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; + +/* + * This is a simple routine to test turning capabilities. + */ +@Config +@Autonomous(group = "drive") +public class TurnTest extends LinearOpMode { + public static double ANGLE = 90; // deg + + @Override + public void runOpMode() throws InterruptedException { + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + waitForStart(); + + if (isStopRequested()) return; + + drive.turn(Math.toRadians(ANGLE)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java new file mode 100644 index 000000000000..081d69578461 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.trajectorysequence; + + +public class EmptySequenceException extends RuntimeException { } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java new file mode 100644 index 000000000000..29032e685cc0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.trajectorysequence; + +import com.acmerobotics.roadrunner.geometry.Pose2d; + +import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment; + +import java.util.Collections; +import java.util.List; + +public class TrajectorySequence { + private final List sequenceList; + + public TrajectorySequence(List sequenceList) { + if (sequenceList.size() == 0) throw new EmptySequenceException(); + + this.sequenceList = Collections.unmodifiableList(sequenceList); + } + + public Pose2d start() { + return sequenceList.get(0).getStartPose(); + } + + public Pose2d end() { + return sequenceList.get(sequenceList.size() - 1).getEndPose(); + } + + public double duration() { + double total = 0.0; + + for (SequenceSegment segment : sequenceList) { + total += segment.getDuration(); + } + + return total; + } + + public SequenceSegment get(int i) { + return sequenceList.get(i); + } + + public int size() { + return sequenceList.size(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java new file mode 100644 index 000000000000..8166db30e9b9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java @@ -0,0 +1,679 @@ +package org.firstinspires.ftc.teamcode.trajectorysequence; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.path.PathContinuityViolationException; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.trajectory.DisplacementMarker; +import com.acmerobotics.roadrunner.trajectory.DisplacementProducer; +import com.acmerobotics.roadrunner.trajectory.MarkerCallback; +import com.acmerobotics.roadrunner.trajectory.SpatialMarker; +import com.acmerobotics.roadrunner.trajectory.TemporalMarker; +import com.acmerobotics.roadrunner.trajectory.TimeProducer; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint; +import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; +import com.acmerobotics.roadrunner.util.Angle; + +import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment; +import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TrajectorySegment; +import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TurnSegment; +import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.WaitSegment; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; +import java.util.List; + +public class TrajectorySequenceBuilder { + private final double resolution = 0.25; + + private final TrajectoryVelocityConstraint baseVelConstraint; + private final TrajectoryAccelerationConstraint baseAccelConstraint; + + private TrajectoryVelocityConstraint currentVelConstraint; + private TrajectoryAccelerationConstraint currentAccelConstraint; + + private final double baseTurnConstraintMaxAngVel; + private final double baseTurnConstraintMaxAngAccel; + + private double currentTurnConstraintMaxAngVel; + private double currentTurnConstraintMaxAngAccel; + + private final List sequenceSegments; + + private final List temporalMarkers; + private final List displacementMarkers; + private final List spatialMarkers; + + private Pose2d lastPose; + + private double tangentOffset; + + private boolean setAbsoluteTangent; + private double absoluteTangent; + + private TrajectoryBuilder currentTrajectoryBuilder; + + private double currentDuration; + private double currentDisplacement; + + private double lastDurationTraj; + private double lastDisplacementTraj; + + public TrajectorySequenceBuilder( + Pose2d startPose, + Double startTangent, + TrajectoryVelocityConstraint baseVelConstraint, + TrajectoryAccelerationConstraint baseAccelConstraint, + double baseTurnConstraintMaxAngVel, + double baseTurnConstraintMaxAngAccel + ) { + this.baseVelConstraint = baseVelConstraint; + this.baseAccelConstraint = baseAccelConstraint; + + this.currentVelConstraint = baseVelConstraint; + this.currentAccelConstraint = baseAccelConstraint; + + this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel; + this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel; + + this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel; + this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel; + + sequenceSegments = new ArrayList<>(); + + temporalMarkers = new ArrayList<>(); + displacementMarkers = new ArrayList<>(); + spatialMarkers = new ArrayList<>(); + + lastPose = startPose; + + tangentOffset = 0.0; + + setAbsoluteTangent = (startTangent != null); + absoluteTangent = startTangent != null ? startTangent : 0.0; + + currentTrajectoryBuilder = null; + + currentDuration = 0.0; + currentDisplacement = 0.0; + + lastDurationTraj = 0.0; + lastDisplacementTraj = 0.0; + } + + public TrajectorySequenceBuilder( + Pose2d startPose, + TrajectoryVelocityConstraint baseVelConstraint, + TrajectoryAccelerationConstraint baseAccelConstraint, + double baseTurnConstraintMaxAngVel, + double baseTurnConstraintMaxAngAccel + ) { + this( + startPose, null, + baseVelConstraint, baseAccelConstraint, + baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel + ); + } + + public TrajectorySequenceBuilder lineTo(Vector2d endPosition) { + return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder lineTo( + Vector2d endPosition, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder lineToConstantHeading(Vector2d endPosition) { + return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder lineToConstantHeading( + Vector2d endPosition, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder lineToLinearHeading(Pose2d endPose) { + return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder lineToLinearHeading( + Pose2d endPose, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder lineToSplineHeading(Pose2d endPose) { + return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder lineToSplineHeading( + Pose2d endPose, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder strafeTo(Vector2d endPosition) { + return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder strafeTo( + Vector2d endPosition, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder forward(double distance) { + return addPath(() -> currentTrajectoryBuilder.forward(distance, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder forward( + double distance, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.forward(distance, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder back(double distance) { + return addPath(() -> currentTrajectoryBuilder.back(distance, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder back( + double distance, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.back(distance, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder strafeLeft(double distance) { + return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder strafeLeft( + double distance, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder strafeRight(double distance) { + return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder strafeRight( + double distance, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder splineTo(Vector2d endPosition, double endHeading) { + return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder splineTo( + Vector2d endPosition, + double endHeading, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder splineToConstantHeading(Vector2d endPosition, double endHeading) { + return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder splineToConstantHeading( + Vector2d endPosition, + double endHeading, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder splineToLinearHeading(Pose2d endPose, double endHeading) { + return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder splineToLinearHeading( + Pose2d endPose, + double endHeading, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, velConstraint, accelConstraint)); + } + + public TrajectorySequenceBuilder splineToSplineHeading(Pose2d endPose, double endHeading) { + return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint)); + } + + public TrajectorySequenceBuilder splineToSplineHeading( + Pose2d endPose, + double endHeading, + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, velConstraint, accelConstraint)); + } + + private TrajectorySequenceBuilder addPath(AddPathCallback callback) { + if (currentTrajectoryBuilder == null) newPath(); + + try { + callback.run(); + } catch (PathContinuityViolationException e) { + newPath(); + callback.run(); + } + + Trajectory builtTraj = currentTrajectoryBuilder.build(); + + double durationDifference = builtTraj.duration() - lastDurationTraj; + double displacementDifference = builtTraj.getPath().length() - lastDisplacementTraj; + + lastPose = builtTraj.end(); + currentDuration += durationDifference; + currentDisplacement += displacementDifference; + + lastDurationTraj = builtTraj.duration(); + lastDisplacementTraj = builtTraj.getPath().length(); + + return this; + } + + public TrajectorySequenceBuilder setTangent(double tangent) { + setAbsoluteTangent = true; + absoluteTangent = tangent; + + pushPath(); + + return this; + } + + private TrajectorySequenceBuilder setTangentOffset(double offset) { + setAbsoluteTangent = false; + + this.tangentOffset = offset; + this.pushPath(); + + return this; + } + + public TrajectorySequenceBuilder setReversed(boolean reversed) { + return reversed ? this.setTangentOffset(Math.toRadians(180.0)) : this.setTangentOffset(0.0); + } + + public TrajectorySequenceBuilder setConstraints( + TrajectoryVelocityConstraint velConstraint, + TrajectoryAccelerationConstraint accelConstraint + ) { + this.currentVelConstraint = velConstraint; + this.currentAccelConstraint = accelConstraint; + + return this; + } + + public TrajectorySequenceBuilder resetConstraints() { + this.currentVelConstraint = this.baseVelConstraint; + this.currentAccelConstraint = this.baseAccelConstraint; + + return this; + } + + public TrajectorySequenceBuilder setVelConstraint(TrajectoryVelocityConstraint velConstraint) { + this.currentVelConstraint = velConstraint; + + return this; + } + + public TrajectorySequenceBuilder resetVelConstraint() { + this.currentVelConstraint = this.baseVelConstraint; + + return this; + } + + public TrajectorySequenceBuilder setAccelConstraint(TrajectoryAccelerationConstraint accelConstraint) { + this.currentAccelConstraint = accelConstraint; + + return this; + } + + public TrajectorySequenceBuilder resetAccelConstraint() { + this.currentAccelConstraint = this.baseAccelConstraint; + + return this; + } + + public TrajectorySequenceBuilder setTurnConstraint(double maxAngVel, double maxAngAccel) { + this.currentTurnConstraintMaxAngVel = maxAngVel; + this.currentTurnConstraintMaxAngAccel = maxAngAccel; + + return this; + } + + public TrajectorySequenceBuilder resetTurnConstraint() { + this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel; + this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel; + + return this; + } + + public TrajectorySequenceBuilder addTemporalMarker(MarkerCallback callback) { + return this.addTemporalMarker(currentDuration, callback); + } + + public TrajectorySequenceBuilder UNSTABLE_addTemporalMarkerOffset(double offset, MarkerCallback callback) { + return this.addTemporalMarker(currentDuration + offset, callback); + } + + public TrajectorySequenceBuilder addTemporalMarker(double time, MarkerCallback callback) { + return this.addTemporalMarker(0.0, time, callback); + } + + public TrajectorySequenceBuilder addTemporalMarker(double scale, double offset, MarkerCallback callback) { + return this.addTemporalMarker(time -> scale * time + offset, callback); + } + + public TrajectorySequenceBuilder addTemporalMarker(TimeProducer time, MarkerCallback callback) { + this.temporalMarkers.add(new TemporalMarker(time, callback)); + return this; + } + + public TrajectorySequenceBuilder addSpatialMarker(Vector2d point, MarkerCallback callback) { + this.spatialMarkers.add(new SpatialMarker(point, callback)); + return this; + } + + public TrajectorySequenceBuilder addDisplacementMarker(MarkerCallback callback) { + return this.addDisplacementMarker(currentDisplacement, callback); + } + + public TrajectorySequenceBuilder UNSTABLE_addDisplacementMarkerOffset(double offset, MarkerCallback callback) { + return this.addDisplacementMarker(currentDisplacement + offset, callback); + } + + public TrajectorySequenceBuilder addDisplacementMarker(double displacement, MarkerCallback callback) { + return this.addDisplacementMarker(0.0, displacement, callback); + } + + public TrajectorySequenceBuilder addDisplacementMarker(double scale, double offset, MarkerCallback callback) { + return addDisplacementMarker((displacement -> scale * displacement + offset), callback); + } + + public TrajectorySequenceBuilder addDisplacementMarker(DisplacementProducer displacement, MarkerCallback callback) { + displacementMarkers.add(new DisplacementMarker(displacement, callback)); + + return this; + } + + public TrajectorySequenceBuilder turn(double angle) { + return turn(angle, currentTurnConstraintMaxAngVel, currentTurnConstraintMaxAngAccel); + } + + public TrajectorySequenceBuilder turn(double angle, double maxAngVel, double maxAngAccel) { + pushPath(); + + MotionProfile turnProfile = MotionProfileGenerator.generateSimpleMotionProfile( + new MotionState(lastPose.getHeading(), 0.0, 0.0, 0.0), + new MotionState(lastPose.getHeading() + angle, 0.0, 0.0, 0.0), + maxAngVel, + maxAngAccel + ); + + sequenceSegments.add(new TurnSegment(lastPose, angle, turnProfile, Collections.emptyList())); + + lastPose = new Pose2d( + lastPose.getX(), lastPose.getY(), + Angle.norm(lastPose.getHeading() + angle) + ); + + currentDuration += turnProfile.duration(); + + return this; + } + + public TrajectorySequenceBuilder waitSeconds(double seconds) { + pushPath(); + sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList())); + + currentDuration += seconds; + return this; + } + + public TrajectorySequenceBuilder addTrajectory(Trajectory trajectory) { + pushPath(); + + sequenceSegments.add(new TrajectorySegment(trajectory)); + return this; + } + + private void pushPath() { + if (currentTrajectoryBuilder != null) { + Trajectory builtTraj = currentTrajectoryBuilder.build(); + sequenceSegments.add(new TrajectorySegment(builtTraj)); + } + + currentTrajectoryBuilder = null; + } + + private void newPath() { + if (currentTrajectoryBuilder != null) + pushPath(); + + lastDurationTraj = 0.0; + lastDisplacementTraj = 0.0; + + double tangent = setAbsoluteTangent ? absoluteTangent : Angle.norm(lastPose.getHeading() + tangentOffset); + + currentTrajectoryBuilder = new TrajectoryBuilder(lastPose, tangent, currentVelConstraint, currentAccelConstraint, resolution); + } + + public TrajectorySequence build() { + pushPath(); + + List globalMarkers = convertMarkersToGlobal( + sequenceSegments, + temporalMarkers, displacementMarkers, spatialMarkers + ); + projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments); + + return new TrajectorySequence(sequenceSegments); + } + + private List convertMarkersToGlobal( + List sequenceSegments, + List temporalMarkers, + List displacementMarkers, + List spatialMarkers + ) { + ArrayList trajectoryMarkers = new ArrayList<>(); + + // Convert temporal markers + for (TemporalMarker marker : temporalMarkers) { + trajectoryMarkers.add( + new TrajectoryMarker(marker.getProducer().produce(currentDuration), marker.getCallback()) + ); + } + + // Convert displacement markers + for (DisplacementMarker marker : displacementMarkers) { + double time = displacementToTime( + sequenceSegments, + marker.getProducer().produce(currentDisplacement) + ); + + trajectoryMarkers.add( + new TrajectoryMarker( + time, + marker.getCallback() + ) + ); + } + + // Convert spatial markers + for (SpatialMarker marker : spatialMarkers) { + trajectoryMarkers.add( + new TrajectoryMarker( + pointToTime(sequenceSegments, marker.getPoint()), + marker.getCallback() + ) + ); + } + + return trajectoryMarkers; + } + + private void projectGlobalMarkersToLocalSegments(List markers, List sequenceSegments) { + if (sequenceSegments.isEmpty()) return; + + markers.sort(Comparator.comparingDouble(TrajectoryMarker::getTime)); + + double timeOffset = 0.0; + int markerIndex = 0; + for (SequenceSegment segment : sequenceSegments) { + while (markerIndex < markers.size()) { + TrajectoryMarker marker = markers.get(markerIndex); + if (marker.getTime() >= timeOffset + segment.getDuration()) { + break; + } + + segment.getMarkers().add(new TrajectoryMarker( + Math.max(0.0, marker.getTime()) - timeOffset, marker.getCallback())); + ++markerIndex; + } + + timeOffset += segment.getDuration(); + } + + SequenceSegment segment = sequenceSegments.get(sequenceSegments.size() - 1); + while (markerIndex < markers.size()) { + TrajectoryMarker marker = markers.get(markerIndex); + segment.getMarkers().add(new TrajectoryMarker(segment.getDuration(), marker.getCallback())); + ++markerIndex; + } + } + + // Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private + // note: this assumes that the profile position is monotonic increasing + private Double motionProfileDisplacementToTime(MotionProfile profile, double s) { + double tLo = 0.0; + double tHi = profile.duration(); + while (!(Math.abs(tLo - tHi) < 1e-6)) { + double tMid = 0.5 * (tLo + tHi); + if (profile.get(tMid).getX() > s) { + tHi = tMid; + } else { + tLo = tMid; + } + } + return 0.5 * (tLo + tHi); + } + + private Double displacementToTime(List sequenceSegments, double s) { + double currentTime = 0.0; + double currentDisplacement = 0.0; + + for (SequenceSegment segment : sequenceSegments) { + if (segment instanceof TrajectorySegment) { + TrajectorySegment thisSegment = (TrajectorySegment) segment; + + double segmentLength = thisSegment.getTrajectory().getPath().length(); + + if (currentDisplacement + segmentLength > s) { + double target = s - currentDisplacement; + double timeInSegment = motionProfileDisplacementToTime( + thisSegment.getTrajectory().getProfile(), + target + ); + + return currentTime + timeInSegment; + } else { + currentDisplacement += segmentLength; + } + } + + currentTime += segment.getDuration(); + } + + return currentTime; + } + + private Double pointToTime(List sequenceSegments, Vector2d point) { + class ComparingPoints { + private final double distanceToPoint; + private final double totalDisplacement; + private final double thisPathDisplacement; + + public ComparingPoints(double distanceToPoint, double totalDisplacement, double thisPathDisplacement) { + this.distanceToPoint = distanceToPoint; + this.totalDisplacement = totalDisplacement; + this.thisPathDisplacement = thisPathDisplacement; + } + } + + List projectedPoints = new ArrayList<>(); + + for (SequenceSegment segment : sequenceSegments) { + if (segment instanceof TrajectorySegment) { + TrajectorySegment thisSegment = (TrajectorySegment) segment; + + double displacement = thisSegment.getTrajectory().getPath().project(point, 0.25); + Vector2d projectedPoint = thisSegment.getTrajectory().getPath().get(displacement).vec(); + double distanceToPoint = point.minus(projectedPoint).norm(); + + double totalDisplacement = 0.0; + + for (ComparingPoints comparingPoint : projectedPoints) { + totalDisplacement += comparingPoint.totalDisplacement; + } + + totalDisplacement += displacement; + + projectedPoints.add(new ComparingPoints(distanceToPoint, displacement, totalDisplacement)); + } + } + + ComparingPoints closestPoint = null; + + for (ComparingPoints comparingPoint : projectedPoints) { + if (closestPoint == null) { + closestPoint = comparingPoint; + continue; + } + + if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint) + closestPoint = comparingPoint; + } + + return displacementToTime(sequenceSegments, closestPoint.thisPathDisplacement); + } + + private interface AddPathCallback { + void run(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java new file mode 100644 index 000000000000..e05af9c5ce4c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java @@ -0,0 +1,306 @@ +package org.firstinspires.ftc.teamcode.trajectorysequence; + +import androidx.annotation.Nullable; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.control.PIDFController; +import com.acmerobotics.roadrunner.drive.DriveSignal; +import com.acmerobotics.roadrunner.followers.TrajectoryFollower; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.profile.MotionState; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +import org.firstinspires.ftc.teamcode.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment; +import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TrajectorySegment; +import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TurnSegment; +import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.WaitSegment; +import org.firstinspires.ftc.teamcode.util.DashboardUtil; +import org.firstinspires.ftc.teamcode.util.LogFiles; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.LinkedList; +import java.util.List; + +@Config +public class TrajectorySequenceRunner { + public static String COLOR_INACTIVE_TRAJECTORY = "#4caf507a"; + public static String COLOR_INACTIVE_TURN = "#7c4dff7a"; + public static String COLOR_INACTIVE_WAIT = "#dd2c007a"; + + public static String COLOR_ACTIVE_TRAJECTORY = "#4CAF50"; + public static String COLOR_ACTIVE_TURN = "#7c4dff"; + public static String COLOR_ACTIVE_WAIT = "#dd2c00"; + + public static int POSE_HISTORY_LIMIT = 100; + + private final TrajectoryFollower follower; + + private final PIDFController turnController; + + private final NanoClock clock; + + private TrajectorySequence currentTrajectorySequence; + private double currentSegmentStartTime; + private int currentSegmentIndex; + private int lastSegmentIndex; + + private Pose2d lastPoseError = new Pose2d(); + + List remainingMarkers = new ArrayList<>(); + + private final FtcDashboard dashboard; + private final LinkedList poseHistory = new LinkedList<>(); + + private VoltageSensor voltageSensor; + + private List lastDriveEncPositions, lastDriveEncVels, lastTrackingEncPositions, lastTrackingEncVels; + + public TrajectorySequenceRunner( + TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients, VoltageSensor voltageSensor, + List lastDriveEncPositions, List lastDriveEncVels, List lastTrackingEncPositions, List lastTrackingEncVels + ) { + this.follower = follower; + + turnController = new PIDFController(headingPIDCoefficients); + turnController.setInputBounds(0, 2 * Math.PI); + + this.voltageSensor = voltageSensor; + + this.lastDriveEncPositions = lastDriveEncPositions; + this.lastDriveEncVels = lastDriveEncVels; + this.lastTrackingEncPositions = lastTrackingEncPositions; + this.lastTrackingEncVels = lastTrackingEncVels; + + clock = NanoClock.system(); + + dashboard = FtcDashboard.getInstance(); + dashboard.setTelemetryTransmissionInterval(25); + } + + public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) { + currentTrajectorySequence = trajectorySequence; + currentSegmentStartTime = clock.seconds(); + currentSegmentIndex = 0; + lastSegmentIndex = -1; + } + + public @Nullable + DriveSignal update(Pose2d poseEstimate, Pose2d poseVelocity) { + Pose2d targetPose = null; + DriveSignal driveSignal = null; + + TelemetryPacket packet = new TelemetryPacket(); + Canvas fieldOverlay = packet.fieldOverlay(); + + SequenceSegment currentSegment = null; + + if (currentTrajectorySequence != null) { + if (currentSegmentIndex >= currentTrajectorySequence.size()) { + for (TrajectoryMarker marker : remainingMarkers) { + marker.getCallback().onMarkerReached(); + } + + remainingMarkers.clear(); + + currentTrajectorySequence = null; + } + + if (currentTrajectorySequence == null) + return new DriveSignal(); + + double now = clock.seconds(); + boolean isNewTransition = currentSegmentIndex != lastSegmentIndex; + + currentSegment = currentTrajectorySequence.get(currentSegmentIndex); + + if (isNewTransition) { + currentSegmentStartTime = now; + lastSegmentIndex = currentSegmentIndex; + + for (TrajectoryMarker marker : remainingMarkers) { + marker.getCallback().onMarkerReached(); + } + + remainingMarkers.clear(); + + remainingMarkers.addAll(currentSegment.getMarkers()); + Collections.sort(remainingMarkers, (t1, t2) -> Double.compare(t1.getTime(), t2.getTime())); + } + + double deltaTime = now - currentSegmentStartTime; + + if (currentSegment instanceof TrajectorySegment) { + Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory(); + + if (isNewTransition) + follower.followTrajectory(currentTrajectory); + + if (!follower.isFollowing()) { + currentSegmentIndex++; + + driveSignal = new DriveSignal(); + } else { + driveSignal = follower.update(poseEstimate, poseVelocity); + lastPoseError = follower.getLastError(); + } + + targetPose = currentTrajectory.get(deltaTime); + } else if (currentSegment instanceof TurnSegment) { + MotionState targetState = ((TurnSegment) currentSegment).getMotionProfile().get(deltaTime); + + turnController.setTargetPosition(targetState.getX()); + + double correction = turnController.update(poseEstimate.getHeading()); + + double targetOmega = targetState.getV(); + double targetAlpha = targetState.getA(); + + lastPoseError = new Pose2d(0, 0, turnController.getLastError()); + + Pose2d startPose = currentSegment.getStartPose(); + targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX()); + + driveSignal = new DriveSignal( + new Pose2d(0, 0, targetOmega + correction), + new Pose2d(0, 0, targetAlpha) + ); + + if (deltaTime >= currentSegment.getDuration()) { + currentSegmentIndex++; + driveSignal = new DriveSignal(); + } + } else if (currentSegment instanceof WaitSegment) { + lastPoseError = new Pose2d(); + + targetPose = currentSegment.getStartPose(); + driveSignal = new DriveSignal(); + + if (deltaTime >= currentSegment.getDuration()) { + currentSegmentIndex++; + } + } + + while (remainingMarkers.size() > 0 && deltaTime > remainingMarkers.get(0).getTime()) { + remainingMarkers.get(0).getCallback().onMarkerReached(); + remainingMarkers.remove(0); + } + } + + poseHistory.add(poseEstimate); + + if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) { + poseHistory.removeFirst(); + } + + final double NOMINAL_VOLTAGE = 12.0; + double voltage = voltageSensor.getVoltage(); + if (driveSignal != null && !DriveConstants.RUN_USING_ENCODER) { + driveSignal = new DriveSignal( + driveSignal.getVel().times(NOMINAL_VOLTAGE / voltage), + driveSignal.getAccel().times(NOMINAL_VOLTAGE / voltage) + ); + } + + if (targetPose != null) { + LogFiles.record( + targetPose, poseEstimate, voltage, + lastDriveEncPositions, lastDriveEncVels, lastTrackingEncPositions, lastTrackingEncVels + ); + } + + packet.put("x", poseEstimate.getX()); + packet.put("y", poseEstimate.getY()); + packet.put("heading (deg)", Math.toDegrees(poseEstimate.getHeading())); + + packet.put("xError", getLastPoseError().getX()); + packet.put("yError", getLastPoseError().getY()); + packet.put("headingError (deg)", Math.toDegrees(getLastPoseError().getHeading())); + + draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate); + + dashboard.sendTelemetryPacket(packet); + + return driveSignal; + } + + private void draw( + Canvas fieldOverlay, + TrajectorySequence sequence, SequenceSegment currentSegment, + Pose2d targetPose, Pose2d poseEstimate + ) { + if (sequence != null) { + for (int i = 0; i < sequence.size(); i++) { + SequenceSegment segment = sequence.get(i); + + if (segment instanceof TrajectorySegment) { + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY); + + DashboardUtil.drawSampledPath(fieldOverlay, ((TrajectorySegment) segment).getTrajectory().getPath()); + } else if (segment instanceof TurnSegment) { + Pose2d pose = segment.getStartPose(); + + fieldOverlay.setFill(COLOR_INACTIVE_TURN); + fieldOverlay.fillCircle(pose.getX(), pose.getY(), 2); + } else if (segment instanceof WaitSegment) { + Pose2d pose = segment.getStartPose(); + + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke(COLOR_INACTIVE_WAIT); + fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3); + } + } + } + + if (currentSegment != null) { + if (currentSegment instanceof TrajectorySegment) { + Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory(); + + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY); + + DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.getPath()); + } else if (currentSegment instanceof TurnSegment) { + Pose2d pose = currentSegment.getStartPose(); + + fieldOverlay.setFill(COLOR_ACTIVE_TURN); + fieldOverlay.fillCircle(pose.getX(), pose.getY(), 3); + } else if (currentSegment instanceof WaitSegment) { + Pose2d pose = currentSegment.getStartPose(); + + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke(COLOR_ACTIVE_WAIT); + fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3); + } + } + + if (targetPose != null) { + fieldOverlay.setStrokeWidth(1); + fieldOverlay.setStroke("#4CAF50"); + DashboardUtil.drawRobot(fieldOverlay, targetPose); + } + + fieldOverlay.setStroke("#3F51B5"); + DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory); + + fieldOverlay.setStroke("#3F51B5"); + DashboardUtil.drawRobot(fieldOverlay, poseEstimate); + } + + public Pose2d getLastPoseError() { + return lastPoseError; + } + + public boolean isBusy() { + return currentTrajectorySequence != null; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java new file mode 100644 index 000000000000..84bccfe1de19 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java @@ -0,0 +1,41 @@ +package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; + +import java.util.ArrayList; +import java.util.List; + +public abstract class SequenceSegment { + private final double duration; + private final Pose2d startPose; + private final Pose2d endPose; + private final List markers; + + protected SequenceSegment( + double duration, + Pose2d startPose, Pose2d endPose, + List markers + ) { + this.duration = duration; + this.startPose = startPose; + this.endPose = endPose; + this.markers = new ArrayList<>(markers); + } + + public double getDuration() { + return this.duration; + } + + public Pose2d getStartPose() { + return startPose; + } + + public Pose2d getEndPose() { + return endPose; + } + + public List getMarkers() { + return markers; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java new file mode 100644 index 000000000000..2e735d471ba7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment; + +import com.acmerobotics.roadrunner.trajectory.Trajectory; + +import java.util.Collections; + +public final class TrajectorySegment extends SequenceSegment { + private final Trajectory trajectory; + + public TrajectorySegment(Trajectory trajectory) { + // Note: Markers are already stored in the `Trajectory` itself. + // This class should not hold any markers + super(trajectory.duration(), trajectory.start(), trajectory.end(), Collections.emptyList()); + this.trajectory = trajectory; + } + + public Trajectory getTrajectory() { + return this.trajectory; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java new file mode 100644 index 000000000000..11995dba7102 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java @@ -0,0 +1,36 @@ +package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.profile.MotionProfile; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; +import com.acmerobotics.roadrunner.util.Angle; + +import java.util.List; + +public final class TurnSegment extends SequenceSegment { + private final double totalRotation; + private final MotionProfile motionProfile; + + public TurnSegment(Pose2d startPose, double totalRotation, MotionProfile motionProfile, List markers) { + super( + motionProfile.duration(), + startPose, + new Pose2d( + startPose.getX(), startPose.getY(), + Angle.norm(startPose.getHeading() + totalRotation) + ), + markers + ); + + this.totalRotation = totalRotation; + this.motionProfile = motionProfile; + } + + public final double getTotalRotation() { + return this.totalRotation; + } + + public final MotionProfile getMotionProfile() { + return this.motionProfile; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java new file mode 100644 index 000000000000..62d2ba46ee8d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java @@ -0,0 +1,12 @@ +package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; + +import java.util.List; + +public final class WaitSegment extends SequenceSegment { + public WaitSegment(Pose2d pose, double seconds, List markers) { + super(seconds, pose, pose, markers); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java new file mode 100644 index 000000000000..1b34a958d5aa --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java @@ -0,0 +1,70 @@ +package org.firstinspires.ftc.teamcode.util; + +import androidx.annotation.Nullable; + +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; +import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig; +import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager; +import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig; + +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; + +import java.io.IOException; +import java.io.InputStream; + +/** + * Set of utilities for loading trajectories from assets (the plugin save location). + */ +public class AssetsTrajectoryManager { + + /** + * Loads the group config. + */ + public static @Nullable + TrajectoryGroupConfig loadGroupConfig() { + try { + InputStream inputStream = AppUtil.getDefContext().getAssets().open( + "trajectory/" + TrajectoryConfigManager.GROUP_FILENAME); + return TrajectoryConfigManager.loadGroupConfig(inputStream); + } catch (IOException e) { + return null; + } + } + + /** + * Loads a trajectory config with the given name. + */ + public static @Nullable TrajectoryConfig loadConfig(String name) { + try { + InputStream inputStream = AppUtil.getDefContext().getAssets().open( + "trajectory/" + name + ".yaml"); + return TrajectoryConfigManager.loadConfig(inputStream); + } catch (IOException e) { + return null; + } + } + + /** + * Loads a trajectory builder with the given name. + */ + public static @Nullable TrajectoryBuilder loadBuilder(String name) { + TrajectoryGroupConfig groupConfig = loadGroupConfig(); + TrajectoryConfig config = loadConfig(name); + if (groupConfig == null || config == null) { + return null; + } + return config.toTrajectoryBuilder(groupConfig); + } + + /** + * Loads a trajectory with the given name. + */ + public static @Nullable Trajectory load(String name) { + TrajectoryBuilder builder = loadBuilder(name); + if (builder == null) { + return null; + } + return builder.build(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java new file mode 100644 index 000000000000..42271d65d9bc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java @@ -0,0 +1,45 @@ +package org.firstinspires.ftc.teamcode.util; + +/** + * IMU axes signs in the order XYZ (after remapping). + */ +public enum AxesSigns { + PPP(0b000), + PPN(0b001), + PNP(0b010), + PNN(0b011), + NPP(0b100), + NPN(0b101), + NNP(0b110), + NNN(0b111); + + public final int bVal; + + AxesSigns(int bVal) { + this.bVal = bVal; + } + + public static AxesSigns fromBinaryValue(int bVal) { + int maskedVal = bVal & 0x07; + switch (maskedVal) { + case 0b000: + return AxesSigns.PPP; + case 0b001: + return AxesSigns.PPN; + case 0b010: + return AxesSigns.PNP; + case 0b011: + return AxesSigns.PNN; + case 0b100: + return AxesSigns.NPP; + case 0b101: + return AxesSigns.NPN; + case 0b110: + return AxesSigns.NNP; + case 0b111: + return AxesSigns.NNN; + default: + throw new IllegalStateException("Unexpected value for maskedVal: " + maskedVal); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java new file mode 100644 index 000000000000..5b8279b56566 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode.util; + +/** + * A direction for an axis to be remapped to + */ +public enum AxisDirection { + POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java new file mode 100644 index 000000000000..ce11d8dc4353 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java @@ -0,0 +1,54 @@ +package org.firstinspires.ftc.teamcode.util; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.path.Path; + +import java.util.List; + +/** + * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases. + */ +public class DashboardUtil { + private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches + private static final double ROBOT_RADIUS = 9; // in + + + public static void drawPoseHistory(Canvas canvas, List poseHistory) { + double[] xPoints = new double[poseHistory.size()]; + double[] yPoints = new double[poseHistory.size()]; + for (int i = 0; i < poseHistory.size(); i++) { + Pose2d pose = poseHistory.get(i); + xPoints[i] = pose.getX(); + yPoints[i] = pose.getY(); + } + canvas.strokePolyline(xPoints, yPoints); + } + + public static void drawSampledPath(Canvas canvas, Path path, double resolution) { + int samples = (int) Math.ceil(path.length() / resolution); + double[] xPoints = new double[samples]; + double[] yPoints = new double[samples]; + double dx = path.length() / (samples - 1); + for (int i = 0; i < samples; i++) { + double displacement = i * dx; + Pose2d pose = path.get(displacement); + xPoints[i] = pose.getX(); + yPoints[i] = pose.getY(); + } + canvas.strokePolyline(xPoints, yPoints); + } + + public static void drawSampledPath(Canvas canvas, Path path) { + drawSampledPath(canvas, path, DEFAULT_RESOLUTION); + } + + public static void drawRobot(Canvas canvas, Pose2d pose) { + canvas.strokeCircle(pose.getX(), pose.getY(), ROBOT_RADIUS); + Vector2d v = pose.headingVec().times(ROBOT_RADIUS); + double x1 = pose.getX() + v.getX() / 2, y1 = pose.getY() + v.getY() / 2; + double x2 = pose.getX() + v.getX(), y2 = pose.getY() + v.getY(); + canvas.strokeLine(x1, y1, x2, y2); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java new file mode 100644 index 000000000000..f724c5242ddb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java @@ -0,0 +1,125 @@ +package org.firstinspires.ftc.teamcode.util; + +import com.acmerobotics.roadrunner.util.NanoClock; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +/** + * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding + * slot's motor direction + */ +public class Encoder { + private final static int CPS_STEP = 0x10000; + + private static double inverseOverflow(double input, double estimate) { + // convert to uint16 + int real = (int) input & 0xffff; + // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits + // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window + real += ((real % 20) / 4) * CPS_STEP; + // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by + real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP; + return real; + } + + public enum Direction { + FORWARD(1), + REVERSE(-1); + + private int multiplier; + + Direction(int multiplier) { + this.multiplier = multiplier; + } + + public int getMultiplier() { + return multiplier; + } + } + + private DcMotorEx motor; + private NanoClock clock; + + private Direction direction; + + private int lastPosition; + private int velocityEstimateIdx; + private double[] velocityEstimates; + private double lastUpdateTime; + + public Encoder(DcMotorEx motor, NanoClock clock) { + this.motor = motor; + this.clock = clock; + + this.direction = Direction.FORWARD; + + this.lastPosition = 0; + this.velocityEstimates = new double[3]; + this.lastUpdateTime = clock.seconds(); + } + + public Encoder(DcMotorEx motor) { + this(motor, NanoClock.system()); + } + + public Direction getDirection() { + return direction; + } + + private int getMultiplier() { + return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1); + } + + /** + * Allows you to set the direction of the counts and velocity without modifying the motor's direction state + * @param direction either reverse or forward depending on if encoder counts should be negated + */ + public void setDirection(Direction direction) { + this.direction = direction; + } + + /** + * Gets the position from the underlying motor and adjusts for the set direction. + * Additionally, this method updates the velocity estimates used for compensated velocity + * + * @return encoder position + */ + public int getCurrentPosition() { + int multiplier = getMultiplier(); + int currentPosition = motor.getCurrentPosition() * multiplier; + if (currentPosition != lastPosition) { + double currentTime = clock.seconds(); + double dt = currentTime - lastUpdateTime; + velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt; + velocityEstimateIdx = (velocityEstimateIdx + 1) % 3; + lastPosition = currentPosition; + lastUpdateTime = currentTime; + } + return currentPosition; + } + + /** + * Gets the velocity directly from the underlying motor and compensates for the direction + * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore) + * + * @return raw velocity + */ + public double getRawVelocity() { + int multiplier = getMultiplier(); + return motor.getVelocity() * multiplier; + } + + /** + * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity + * that are lost in overflow due to velocity being transmitted as 16 bits. + * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly. + * + * @return corrected velocity + */ + public double getCorrectedVelocity() { + double median = velocityEstimates[0] > velocityEstimates[1] + ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2])) + : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2])); + return inverseOverflow(getRawVelocity(), median); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java new file mode 100644 index 000000000000..8338eb1c638b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java @@ -0,0 +1,273 @@ +package org.firstinspires.ftc.teamcode.util; + +import android.annotation.SuppressLint; +import android.content.Context; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.fasterxml.jackson.core.JsonFactory; +import com.fasterxml.jackson.databind.ObjectMapper; +import com.fasterxml.jackson.databind.ObjectWriter; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl; +import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier; +import com.qualcomm.robotcore.util.RobotLog; +import com.qualcomm.robotcore.util.WebHandlerManager; + +import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar; +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; +import org.firstinspires.ftc.teamcode.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; +import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer; + +import java.io.File; +import java.io.FileInputStream; +import java.io.IOException; +import java.text.DateFormat; +import java.text.SimpleDateFormat; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Date; +import java.util.List; +import java.util.Objects; + +import fi.iki.elonen.NanoHTTPD; + +public final class LogFiles { + private static final File ROOT = + new File(AppUtil.ROOT_FOLDER + "/RoadRunner/logs/"); + + public static LogFile log = new LogFile("uninitialized"); + + public static class LogFile { + public String version = "quickstart1 v2"; + + public String opModeName; + public long msInit = System.currentTimeMillis(); + public long nsInit = System.nanoTime(); + public long nsStart, nsStop; + + public double ticksPerRev = DriveConstants.TICKS_PER_REV; + public double maxRpm = DriveConstants.MAX_RPM; + public boolean runUsingEncoder = DriveConstants.RUN_USING_ENCODER; + public double motorP = DriveConstants.MOTOR_VELO_PID.p; + public double motorI = DriveConstants.MOTOR_VELO_PID.i; + public double motorD = DriveConstants.MOTOR_VELO_PID.d; + public double motorF = DriveConstants.MOTOR_VELO_PID.f; + public double wheelRadius = DriveConstants.WHEEL_RADIUS; + public double gearRatio = DriveConstants.GEAR_RATIO; + public double trackWidth = DriveConstants.TRACK_WIDTH; + public double kV = DriveConstants.kV; + public double kA = DriveConstants.kA; + public double kStatic = DriveConstants.kStatic; + public double maxVel = DriveConstants.MAX_VEL; + public double maxAccel = DriveConstants.MAX_ACCEL; + public double maxAngVel = DriveConstants.MAX_ANG_VEL; + public double maxAngAccel = DriveConstants.MAX_ANG_ACCEL; + + public double mecTransP = SampleMecanumDrive.TRANSLATIONAL_PID.kP; + public double mecTransI = SampleMecanumDrive.TRANSLATIONAL_PID.kI; + public double mecTransD = SampleMecanumDrive.TRANSLATIONAL_PID.kD; + public double mecHeadingP = SampleMecanumDrive.HEADING_PID.kP; + public double mecHeadingI = SampleMecanumDrive.HEADING_PID.kI; + public double mecHeadingD = SampleMecanumDrive.HEADING_PID.kD; + public double mecLateralMultiplier = SampleMecanumDrive.LATERAL_MULTIPLIER; + + public double tankAxialP = SampleTankDrive.AXIAL_PID.kP; + public double tankAxialI = SampleTankDrive.AXIAL_PID.kI; + public double tankAxialD = SampleTankDrive.AXIAL_PID.kD; + public double tankCrossTrackP = SampleTankDrive.CROSS_TRACK_PID.kP; + public double tankCrossTrackI = SampleTankDrive.CROSS_TRACK_PID.kI; + public double tankCrossTrackD = SampleTankDrive.CROSS_TRACK_PID.kD; + public double tankHeadingP = SampleTankDrive.HEADING_PID.kP; + public double tankHeadingI = SampleTankDrive.HEADING_PID.kI; + public double tankHeadingD = SampleTankDrive.HEADING_PID.kD; + + public double trackingTicksPerRev = StandardTrackingWheelLocalizer.TICKS_PER_REV; + public double trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS; + public double trackingGearRatio = StandardTrackingWheelLocalizer.GEAR_RATIO; + public double trackingLateralDistance = StandardTrackingWheelLocalizer.LATERAL_DISTANCE; + public double trackingForwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET; + + public RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR = DriveConstants.LOGO_FACING_DIR; + public RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR = DriveConstants.USB_FACING_DIR; + + public List nsTimes = new ArrayList<>(); + + public List targetXs = new ArrayList<>(); + public List targetYs = new ArrayList<>(); + public List targetHeadings = new ArrayList<>(); + + public List xs = new ArrayList<>(); + public List ys = new ArrayList<>(); + public List headings = new ArrayList<>(); + + public List voltages = new ArrayList<>(); + + public List> driveEncPositions = new ArrayList<>(); + public List> driveEncVels = new ArrayList<>(); + public List> trackingEncPositions = new ArrayList<>(); + public List> trackingEncVels = new ArrayList<>(); + + public LogFile(String opModeName) { + this.opModeName = opModeName; + } + } + + public static void record( + Pose2d targetPose, Pose2d pose, double voltage, + List lastDriveEncPositions, List lastDriveEncVels, List lastTrackingEncPositions, List lastTrackingEncVels + ) { + long nsTime = System.nanoTime(); + if (nsTime - log.nsStart > 3 * 60 * 1_000_000_000L) { + return; + } + + log.nsTimes.add(nsTime); + + log.targetXs.add(targetPose.getX()); + log.targetYs.add(targetPose.getY()); + log.targetHeadings.add(targetPose.getHeading()); + + log.xs.add(pose.getX()); + log.ys.add(pose.getY()); + log.headings.add(pose.getHeading()); + + log.voltages.add(voltage); + + while (log.driveEncPositions.size() < lastDriveEncPositions.size()) { + log.driveEncPositions.add(new ArrayList<>()); + } + while (log.driveEncVels.size() < lastDriveEncVels.size()) { + log.driveEncVels.add(new ArrayList<>()); + } + while (log.trackingEncPositions.size() < lastTrackingEncPositions.size()) { + log.trackingEncPositions.add(new ArrayList<>()); + } + while (log.trackingEncVels.size() < lastTrackingEncVels.size()) { + log.trackingEncVels.add(new ArrayList<>()); + } + + for (int i = 0; i < lastDriveEncPositions.size(); i++) { + log.driveEncPositions.get(i).add(lastDriveEncPositions.get(i)); + } + for (int i = 0; i < lastDriveEncVels.size(); i++) { + log.driveEncVels.get(i).add(lastDriveEncVels.get(i)); + } + for (int i = 0; i < lastTrackingEncPositions.size(); i++) { + log.trackingEncPositions.get(i).add(lastTrackingEncPositions.get(i)); + } + for (int i = 0; i < lastTrackingEncVels.size(); i++) { + log.trackingEncVels.get(i).add(lastTrackingEncVels.get(i)); + } + } + + private static final OpModeManagerNotifier.Notifications notifHandler = new OpModeManagerNotifier.Notifications() { + @SuppressLint("SimpleDateFormat") + final DateFormat dateFormat = new SimpleDateFormat("yyyy_MM_dd__HH_mm_ss_SSS"); + + final ObjectWriter jsonWriter = new ObjectMapper(new JsonFactory()) + .writerWithDefaultPrettyPrinter(); + + @Override + public void onOpModePreInit(OpMode opMode) { + log = new LogFile(opMode.getClass().getCanonicalName()); + + // clean up old files + File[] fs = Objects.requireNonNull(ROOT.listFiles()); + Arrays.sort(fs, (a, b) -> Long.compare(a.lastModified(), b.lastModified())); + long totalSizeBytes = 0; + for (File f : fs) { + totalSizeBytes += f.length(); + } + + int i = 0; + while (i < fs.length && totalSizeBytes >= 32 * 1000 * 1000) { + totalSizeBytes -= fs[i].length(); + if (!fs[i].delete()) { + RobotLog.setGlobalErrorMsg("Unable to delete file " + fs[i].getAbsolutePath()); + } + ++i; + } + } + + @Override + public void onOpModePreStart(OpMode opMode) { + log.nsStart = System.nanoTime(); + } + + @Override + public void onOpModePostStop(OpMode opMode) { + log.nsStop = System.nanoTime(); + + if (!(opMode instanceof OpModeManagerImpl.DefaultOpMode)) { + //noinspection ResultOfMethodCallIgnored + ROOT.mkdirs(); + + String filename = dateFormat.format(new Date(log.msInit)) + "__" + opMode.getClass().getSimpleName() + ".json"; + File file = new File(ROOT, filename); + try { + jsonWriter.writeValue(file, log); + } catch (IOException e) { + RobotLog.setGlobalErrorMsg(new RuntimeException(e), + "Unable to write data to " + file.getAbsolutePath()); + } + } + } + }; + + @WebHandlerRegistrar + public static void registerRoutes(Context context, WebHandlerManager manager) { + //noinspection ResultOfMethodCallIgnored + ROOT.mkdirs(); + + // op mode manager only stores a weak reference, so we need to keep notifHandler alive ourselves + // don't use @OnCreateEventLoop because it's unreliable + OpModeManagerImpl.getOpModeManagerOfActivity( + AppUtil.getInstance().getActivity() + ).registerListener(notifHandler); + + manager.register("/logs", session -> { + final StringBuilder sb = new StringBuilder(); + sb.append("Logs

    "); + File[] fs = Objects.requireNonNull(ROOT.listFiles()); + Arrays.sort(fs, (a, b) -> Long.compare(b.lastModified(), a.lastModified())); + for (File f : fs) { + sb.append("
  • "); + sb.append(f.getName()); + sb.append("
    • "); + } + sb.append("
"); + return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.OK, + NanoHTTPD.MIME_HTML, sb.toString()); + }); + + manager.register("/logs/download", session -> { + final String[] pairs = session.getQueryParameterString().split("&"); + if (pairs.length != 1) { + return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST, + NanoHTTPD.MIME_PLAINTEXT, "expected one query parameter, got " + pairs.length); + } + + final String[] parts = pairs[0].split("="); + if (!parts[0].equals("file")) { + return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST, + NanoHTTPD.MIME_PLAINTEXT, "expected file query parameter, got " + parts[0]); + } + + File f = new File(ROOT, parts[1]); + if (!f.exists()) { + return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND, + NanoHTTPD.MIME_PLAINTEXT, "file " + f + " doesn't exist"); + } + + return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK, + "application/json", new FileInputStream(f)); + }); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java new file mode 100644 index 000000000000..2c558f15ac12 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java @@ -0,0 +1,60 @@ +package org.firstinspires.ftc.teamcode.util; + +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; + +import java.io.File; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +/** + * Utility functions for log files. + */ +public class LoggingUtil { + public static final File ROAD_RUNNER_FOLDER = + new File(AppUtil.ROOT_FOLDER + "/RoadRunner/"); + + private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now + + private static void buildLogList(List logFiles, File dir) { + for (File file : dir.listFiles()) { + if (file.isDirectory()) { + buildLogList(logFiles, file); + } else { + logFiles.add(file); + } + } + } + + private static void pruneLogsIfNecessary() { + List logFiles = new ArrayList<>(); + buildLogList(logFiles, ROAD_RUNNER_FOLDER); + Collections.sort(logFiles, (lhs, rhs) -> + Long.compare(lhs.lastModified(), rhs.lastModified())); + + long dirSize = 0; + for (File file: logFiles) { + dirSize += file.length(); + } + + while (dirSize > LOG_QUOTA) { + if (logFiles.size() == 0) break; + File fileToRemove = logFiles.remove(0); + dirSize -= fileToRemove.length(); + //noinspection ResultOfMethodCallIgnored + fileToRemove.delete(); + } + } + + /** + * Obtain a log file with the provided name + */ + public static File getLogFile(String name) { + //noinspection ResultOfMethodCallIgnored + ROAD_RUNNER_FOLDER.mkdirs(); + + pruneLogsIfNecessary(); + + return new File(ROAD_RUNNER_FOLDER, name); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java new file mode 100644 index 000000000000..243dd7eb0e69 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java @@ -0,0 +1,124 @@ +package org.firstinspires.ftc.teamcode.util; + +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.internal.system.Misc; + +import java.util.HashMap; +import java.util.Map; + +/** + * Collection of utilites for interacting with Lynx modules. + */ +public class LynxModuleUtil { + + private static final LynxFirmwareVersion MIN_VERSION = new LynxFirmwareVersion(1, 8, 2); + + /** + * Parsed representation of a Lynx module firmware version. + */ + public static class LynxFirmwareVersion implements Comparable { + public final int major; + public final int minor; + public final int eng; + + public LynxFirmwareVersion(int major, int minor, int eng) { + this.major = major; + this.minor = minor; + this.eng = eng; + } + + @Override + public boolean equals(Object other) { + if (other instanceof LynxFirmwareVersion) { + LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other; + return major == otherVersion.major && minor == otherVersion.minor && + eng == otherVersion.eng; + } else { + return false; + } + } + + @Override + public int compareTo(LynxFirmwareVersion other) { + int majorComp = Integer.compare(major, other.major); + if (majorComp == 0) { + int minorComp = Integer.compare(minor, other.minor); + if (minorComp == 0) { + return Integer.compare(eng, other.eng); + } else { + return minorComp; + } + } else { + return majorComp; + } + } + + @Override + public String toString() { + return Misc.formatInvariant("%d.%d.%d", major, minor, eng); + } + } + + /** + * Retrieve and parse Lynx module firmware version. + * @param module Lynx module + * @return parsed firmware version + */ + public static LynxFirmwareVersion getFirmwareVersion(LynxModule module) { + String versionString = module.getNullableFirmwareVersionString(); + if (versionString == null) { + return null; + } + + String[] parts = versionString.split("[ :,]+"); + try { + // note: for now, we ignore the hardware entry + return new LynxFirmwareVersion( + Integer.parseInt(parts[3]), + Integer.parseInt(parts[5]), + Integer.parseInt(parts[7]) + ); + } catch (NumberFormatException e) { + return null; + } + } + + /** + * Exception indicating an outdated Lynx firmware version. + */ + public static class LynxFirmwareVersionException extends RuntimeException { + public LynxFirmwareVersionException(String detailMessage) { + super(detailMessage); + } + } + + /** + * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement. + * @param hardwareMap hardware map containing Lynx modules + */ + public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) { + Map outdatedModules = new HashMap<>(); + for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { + LynxFirmwareVersion version = getFirmwareVersion(module); + if (version == null || version.compareTo(MIN_VERSION) < 0) { + for (String name : hardwareMap.getNamesOf(module)) { + outdatedModules.put(name, version); + } + } + } + if (outdatedModules.size() > 0) { + StringBuilder msgBuilder = new StringBuilder(); + msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n"); + msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n", + MIN_VERSION.toString())); + for (Map.Entry entry : outdatedModules.entrySet()) { + msgBuilder.append(Misc.formatInvariant( + "\t%s: %s\n", entry.getKey(), + entry.getValue() == null ? "Unknown" : entry.getValue().toString())); + } + throw new LynxFirmwareVersionException(msgBuilder.toString()); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java new file mode 100644 index 000000000000..c3bf488ee90d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java @@ -0,0 +1,156 @@ +package org.firstinspires.ftc.teamcode.util; + +import androidx.annotation.Nullable; + +import com.acmerobotics.roadrunner.kinematics.Kinematics; + +import org.apache.commons.math3.stat.regression.SimpleRegression; + +import java.io.File; +import java.io.FileNotFoundException; +import java.io.PrintWriter; +import java.util.ArrayList; +import java.util.List; + +/** + * Various regression utilities. + */ +public class RegressionUtil { + + /** + * Feedforward parameter estimates from the ramp regression and additional summary statistics + */ + public static class RampResult { + public final double kV, kStatic, rSquare; + + public RampResult(double kV, double kStatic, double rSquare) { + this.kV = kV; + this.kStatic = kStatic; + this.rSquare = rSquare; + } + } + + /** + * Feedforward parameter estimates from the ramp regression and additional summary statistics + */ + public static class AccelResult { + public final double kA, rSquare; + + public AccelResult(double kA, double rSquare) { + this.kA = kA; + this.rSquare = rSquare; + } + } + + /** + * Numerically compute dy/dx from the given x and y values. The returned list is padded to match + * the length of the original sequences. + * + * @param x x-values + * @param y y-values + * @return derivative values + */ + private static List numericalDerivative(List x, List y) { + List deriv = new ArrayList<>(x.size()); + for (int i = 1; i < x.size() - 1; i++) { + deriv.add( + (y.get(i + 1) - y.get(i - 1)) / + (x.get(i + 1) - x.get(i - 1)) + ); + } + // copy endpoints to pad output + deriv.add(0, deriv.get(0)); + deriv.add(deriv.get(deriv.size() - 1)); + return deriv; + } + + /** + * Run regression to compute velocity and static feedforward from ramp test data. + * + * Here's the general procedure for gathering the requisite data: + * 1. Slowly ramp the motor power/voltage and record encoder values along the way. + * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope + * (kV) and an optional intercept (kStatic). + * + * @param timeSamples time samples + * @param positionSamples position samples + * @param powerSamples power samples + * @param fitStatic fit kStatic + * @param file log file + */ + public static RampResult fitRampData(List timeSamples, List positionSamples, + List powerSamples, boolean fitStatic, + @Nullable File file) { + if (file != null) { + try (PrintWriter pw = new PrintWriter(file)) { + pw.println("time,position,power"); + for (int i = 0; i < timeSamples.size(); i++) { + double time = timeSamples.get(i); + double pos = positionSamples.get(i); + double power = powerSamples.get(i); + pw.println(time + "," + pos + "," + power); + } + } catch (FileNotFoundException e) { + // ignore + } + } + + List velSamples = numericalDerivative(timeSamples, positionSamples); + + SimpleRegression rampReg = new SimpleRegression(fitStatic); + for (int i = 0; i < timeSamples.size(); i++) { + double vel = velSamples.get(i); + double power = powerSamples.get(i); + + rampReg.addData(vel, power); + } + + return new RampResult(Math.abs(rampReg.getSlope()), Math.abs(rampReg.getIntercept()), + rampReg.getRSquare()); + } + + /** + * Run regression to compute acceleration feedforward. + * + * @param timeSamples time samples + * @param positionSamples position samples + * @param powerSamples power samples + * @param rampResult ramp result + * @param file log file + */ + public static AccelResult fitAccelData(List timeSamples, List positionSamples, + List powerSamples, RampResult rampResult, + @Nullable File file) { + if (file != null) { + try (PrintWriter pw = new PrintWriter(file)) { + pw.println("time,position,power"); + for (int i = 0; i < timeSamples.size(); i++) { + double time = timeSamples.get(i); + double pos = positionSamples.get(i); + double power = powerSamples.get(i); + pw.println(time + "," + pos + "," + power); + } + } catch (FileNotFoundException e) { + // ignore + } + } + + List velSamples = numericalDerivative(timeSamples, positionSamples); + List accelSamples = numericalDerivative(timeSamples, velSamples); + + SimpleRegression accelReg = new SimpleRegression(false); + for (int i = 0; i < timeSamples.size(); i++) { + double vel = velSamples.get(i); + double accel = accelSamples.get(i); + double power = powerSamples.get(i); + + double powerFromVel = Kinematics.calculateMotorFeedforward( + vel, 0.0, rampResult.kV, 0.0, rampResult.kStatic); + double powerFromAccel = power - powerFromVel; + + accelReg.addData(accel, powerFromAccel); + } + + return new AccelResult(Math.abs(accelReg.getSlope()), accelReg.getRSquare()); + } +} diff --git a/build.dependencies.gradle b/build.dependencies.gradle index c714d2ff5f7f..c89aefd65849 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -1,6 +1,7 @@ repositories { mavenCentral() google() // Needed for androidx + maven { url = 'https://maven.brott.dev/' } } dependencies { @@ -13,5 +14,8 @@ dependencies { implementation 'org.firstinspires.ftc:FtcCommon:10.0.0' implementation 'org.firstinspires.ftc:Vision:10.0.0' implementation 'androidx.appcompat:appcompat:1.7.0' + implementation('com.acmerobotics.dashboard:dashboard:0.4.16') { + exclude group: 'org.firstinspires.ftc' + } } From 91cef18bcf63e9f2723c0938b52f5d3504619010 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Wed, 27 Nov 2024 21:14:59 +0800 Subject: [PATCH 29/93] Feat:Fix some low errors --- TeamCode/build.gradle | 3 +-- build.dependencies.gradle | 16 ++++++++-------- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 4684f01ec788..8a4e34ccc19d 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -27,6 +27,5 @@ dependencies { implementation project(':FtcRobotController') implementation 'org.apache.commons:commons-math3:3.6.1' implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' - implementation 'com.acmerobotics.roadrunner:core:0.5.7' - implementation 'com.acmerobotics.roadrunner:quickstart:0.5.7' + implementation 'com.acmerobotics.roadrunner:core:0.5.6' } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index c89aefd65849..8daf889f9314 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -5,14 +5,14 @@ repositories { } dependencies { - implementation 'org.firstinspires.ftc:Inspection:10.0.0' - implementation 'org.firstinspires.ftc:Blocks:10.0.0' - implementation 'org.firstinspires.ftc:RobotCore:10.0.0' - implementation 'org.firstinspires.ftc:RobotServer:10.0.0' - implementation 'org.firstinspires.ftc:OnBotJava:10.0.0' - implementation 'org.firstinspires.ftc:Hardware:10.0.0' - implementation 'org.firstinspires.ftc:FtcCommon:10.0.0' - implementation 'org.firstinspires.ftc:Vision:10.0.0' + implementation 'org.firstinspires.ftc:Inspection:10.1.0' + implementation 'org.firstinspires.ftc:Blocks:10.1.0' + implementation 'org.firstinspires.ftc:RobotCore:10.1.0' + implementation 'org.firstinspires.ftc:RobotServer:10.1.0' + implementation 'org.firstinspires.ftc:OnBotJava:10.1.0' + implementation 'org.firstinspires.ftc:Hardware:10.1.0' + implementation 'org.firstinspires.ftc:FtcCommon:10.1.0' + implementation 'org.firstinspires.ftc:Vision:10.1.0' implementation 'androidx.appcompat:appcompat:1.7.0' implementation('com.acmerobotics.dashboard:dashboard:0.4.16') { exclude group: 'org.firstinspires.ftc' From 54241be2e163ee72133f44723d2b160c6dcfa0d3 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Thu, 28 Nov 2024 20:04:49 +0800 Subject: [PATCH 30/93] Feat:Add ServoTest --- .../firstinspires/ftc/teamcode/ServoTest.java | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTest.java new file mode 100644 index 000000000000..8781669d70ec --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTest.java @@ -0,0 +1,38 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Servo; + +public class ServoTest extends LinearOpMode { + + private Servo servo1; + private double position = 0.5; + private final double step = 0.01; + + @Override + public void runOpMode() { + // 初始化舵机 + servo1 = hardwareMap.get(Servo.class, "servo1"); + + waitForStart(); + + while (opModeIsActive()) { + // 控制舵机位置 + if (gamepad1.a && position + step <= 1.0) { + + position += step; + servo1.setPosition(position); + sleep(50); + } else if (gamepad1.b && position - step >= 0) { + + position -= step; + servo1.setPosition(position); + sleep(50); + } + + // 添加其他舵机控制逻辑 + telemetry.addData("Servo Position", servo1.getPosition()); + telemetry.update(); + } + } +} From 451788fb76f89eb469dea448c61b5a65c27d68c1 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Thu, 28 Nov 2024 20:54:34 +0800 Subject: [PATCH 31/93] third commit to improve the vision --- .../org/firstinspires/ftc/teamcode/TEST3.java | 34 ++++++++ .../ftc/teamcode/hardware/RobotVision2.java | 86 +++++++++++++++++++ 2 files changed, 120 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision2.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java new file mode 100644 index 000000000000..8573e9c9ca47 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java @@ -0,0 +1,34 @@ +//测试摄像头角度识别功能 +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.hardware.RobotVision2; + +@TeleOp + + + +public class TEST3 extends LinearOpMode { + private RobotVision2 robotVision2; + + @Override + public void runOpMode() { + robotVision2 = new RobotVision2(); + + robotVision2.initialize(hardwareMap); // 初始化摄像头 + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + // 显示角度检测结果 + telemetry.addData("Detected Angle", robotVision2.getDetectedAngle()); + telemetry.update(); + + sleep(100); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision2.java new file mode 100644 index 000000000000..75d5811e65a3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision2.java @@ -0,0 +1,86 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.Rect; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvPipeline; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; + +public class RobotVision2 { + private OpenCvCamera webcam; + private double detectedAngle = 0; // 角度变量 + + public void initialize(HardwareMap hardwareMap) { + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + webcam.setPipeline(new CenterAnglePipeline()); + + webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + // 处理摄像头打开错误 + } + }); + } + + public double getDetectedAngle() { + return detectedAngle; + } + + private class CenterAnglePipeline extends OpenCvPipeline { + Mat gray = new Mat(); + Mat blurred = new Mat(); + Mat edges = new Mat(); + Mat lines = new Mat(); + + @Override + public Mat processFrame(Mat input) { + // 转换为灰度图像 + Imgproc.cvtColor(input, gray, Imgproc.COLOR_RGB2GRAY); + + // 高斯模糊，减少噪声 + Imgproc.GaussianBlur(gray, blurred, new Size(5, 5), 0); + + // 获取图像中心区域 (3/4的画面) + Rect centerRect = new Rect(input.width() / 8, input.height() / 8, input.width() * 3 / 4, input.height() * 3 / 4); + Mat centerMat = blurred.submat(centerRect); + + // 边缘检测 + Imgproc.Canny(centerMat, edges, 50, 150); + + // 霍夫直线变换检测线条 + Imgproc.HoughLinesP(edges, lines, 1, Math.PI / 180, 50, 50, 10); + + double angle = 0; + if (lines.rows() > 0) { + for (int i = 0; i < lines.rows(); i++) { + double[] line = lines.get(i, 0); + double dx = line[2] - line[0]; + double dy = line[3] - line[1]; + angle = Math.toDegrees(Math.atan2(dy, dx)); + if (angle < 0) { + angle += 180; + } + angle = Math.abs(angle - 90); + break; // 考虑简单起见只取第一条检测到的线条 + } + } + + detectedAngle = angle; + + return input; + } + } +} \ No newline at end of file From 78514df70705789d7c51fa6d04071ee5c7778fb9 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Thu, 28 Nov 2024 20:55:28 +0800 Subject: [PATCH 32/93] third commit to improve the vision --- .../src/main/java/org/firstinspires/ftc/teamcode/TEST3.java | 2 +- .../org/firstinspires/ftc/teamcode/hardware/RobotVision2.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java index 8573e9c9ca47..cf64b508d5bb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java @@ -1,4 +1,4 @@ -//测试摄像头角度识别功能 +//测试摄像头角度识别功能1 package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision2.java index 75d5811e65a3..32d976f0cdcc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision2.java @@ -1,5 +1,5 @@ package org.firstinspires.ftc.teamcode.hardware; - +// import com.qualcomm.robotcore.hardware.HardwareMap; import org.opencv.core.Core; import org.opencv.core.Mat; From faadba1697d94b42afb31f9b0914ee8ab126ca06 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Thu, 28 Nov 2024 20:55:30 +0800 Subject: [PATCH 33/93] Feat:Fixed some errors --- .../main/java/org/firstinspires/ftc/teamcode/ServoTest.java | 3 +++ build.dependencies.gradle | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTest.java index 8781669d70ec..342cace6e681 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTest.java @@ -1,8 +1,11 @@ package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.Servo; +@TeleOp + public class ServoTest extends LinearOpMode { private Servo servo1; diff --git a/build.dependencies.gradle b/build.dependencies.gradle index ad4b55d5c9d5..df7d3f998335 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -12,7 +12,7 @@ dependencies { implementation 'org.firstinspires.ftc:Hardware:10.1.0' implementation 'org.firstinspires.ftc:FtcCommon:10.1.0' implementation 'org.firstinspires.ftc:Vision:10.1.0' - implementation 'androidx.appcompat:appcompat:1.7.0' + implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'org.openftc:easyopencv:1.7.3' } From 462f8672127736fde87b85d87bd12c07d92b6173 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Thu, 28 Nov 2024 20:59:22 +0800 Subject: [PATCH 34/93] forth commit to improve the vision --- .../main/java/org/firstinspires/ftc/teamcode/TEST3.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java index cf64b508d5bb..5a61e09ebc4e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java @@ -3,6 +3,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.hardware.RobotVision; import org.firstinspires.ftc.teamcode.hardware.RobotVision2; @TeleOp @@ -11,6 +13,7 @@ public class TEST3 extends LinearOpMode { private RobotVision2 robotVision2; + private RobotVision robotVision; @Override public void runOpMode() { @@ -18,6 +21,10 @@ public void runOpMode() { robotVision2.initialize(hardwareMap); // 初始化摄像头 + robotVision = new RobotVision(); + + robotVision.initialize(hardwareMap); // 初始化摄像头 + telemetry.addData("Status", "Initialized"); telemetry.update(); @@ -25,6 +32,7 @@ public void runOpMode() { while (opModeIsActive()) { // 显示角度检测结果 + telemetry.addData("Center Color", robotVision.getDetectedColor()); telemetry.addData("Detected Angle", robotVision2.getDetectedAngle()); telemetry.update(); From fb1c5b6ec9c50b08be6670cba252745d890af6db Mon Sep 17 00:00:00 2001 From: Ehicy Date: Thu, 28 Nov 2024 21:01:30 +0800 Subject: [PATCH 35/93] forth commit to improve the vision --- .../src/main/java/org/firstinspires/ftc/teamcode/TEST3.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java index 5a61e09ebc4e..6116b13fa1b6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java @@ -1,4 +1,4 @@ -//测试摄像头角度识别功能1 +//测试摄像头角度识别功能 package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; From d133d0876474c218429ffe0d830b11305c4b9c90 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Thu, 28 Nov 2024 21:03:52 +0800 Subject: [PATCH 36/93] fuck the commit to improve the vision --- .../main/java/org/firstinspires/ftc/teamcode/TEST3.java | 8 -------- 1 file changed, 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java index 6116b13fa1b6..8573e9c9ca47 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java @@ -3,8 +3,6 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.hardware.RobotVision; import org.firstinspires.ftc.teamcode.hardware.RobotVision2; @TeleOp @@ -13,7 +11,6 @@ public class TEST3 extends LinearOpMode { private RobotVision2 robotVision2; - private RobotVision robotVision; @Override public void runOpMode() { @@ -21,10 +18,6 @@ public void runOpMode() { robotVision2.initialize(hardwareMap); // 初始化摄像头 - robotVision = new RobotVision(); - - robotVision.initialize(hardwareMap); // 初始化摄像头 - telemetry.addData("Status", "Initialized"); telemetry.update(); @@ -32,7 +25,6 @@ public void runOpMode() { while (opModeIsActive()) { // 显示角度检测结果 - telemetry.addData("Center Color", robotVision.getDetectedColor()); telemetry.addData("Detected Angle", robotVision2.getDetectedAngle()); telemetry.update(); From cc4b3e99b7bc5f4104c6aae79c1929ef9417290e Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 29 Nov 2024 15:30:54 +0800 Subject: [PATCH 37/93] temp commit --- .../ftc/teamcode/ManualOpMode.java | 29 ++++++++++++++ .../ftc/teamcode/hardware/RobotTop.java | 38 +++++++++---------- build.dependencies.gradle | 2 +- 3 files changed, 49 insertions(+), 20 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 5af69501f122..85cf9a2b397a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -2,6 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; import org.firstinspires.ftc.teamcode.hardware.RobotChassis; import org.firstinspires.ftc.teamcode.hardware.RobotTop; @@ -15,12 +16,18 @@ public class ManualOpMode extends LinearOpMode { public void runOpMode() { robotChassis = new RobotChassis(this); robotTop = new RobotTop(this); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); int liftPosition; LiftState liftState = LiftState.BOTTOM; + ArmState armState = ArmState.IDLE; + waitForStart(); while (opModeIsActive()){ robotChassis.driveRobot(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + + //robotLift liftPosition = robotTop.getLiftPosition(); if(liftState == LiftState.BOTTOM){ if(gamepad1.y && liftPosition <= 1100){ @@ -41,13 +48,35 @@ public void runOpMode() { robotTop.setLeftPower(-0.2); } } + + //robotArm + if(gamepad1.x && !previousGamepad1.x){ + if(armState == ArmState.IDLE){ + robotTop.setArmStretchPosition(0.6); + }else if(armState == ArmState.STRETCHED){ + robotTop.setArmStretchPosition(0); + } + } + if(gamepad1.a && !previousGamepad1.a){ + if(armState == ArmState.STRETCHED){ + + }else if(armState == ArmState.TURNED){ + + } + } + + //telemetry telemetry.addData("pos", liftPosition); telemetry.addData("state", liftState); telemetry.update(); + previousGamepad1.copy(gamepad1); sleep(10); } } } enum LiftState{ BOTTOM,TOP +} +enum ArmState{ + IDLE,STRETCHED,TURNED } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java index 10c56aff61d2..25b14a7e074a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java @@ -32,12 +32,11 @@ public RobotTop(LinearOpMode opMode) { public void init() { leftLiftMotor = hardwareMap.get(DcMotor.class, "leftLift"); rightLiftMotor = hardwareMap.get(DcMotor.class, "rightLift"); - armStretchServo = hardwareMap.get(Servo.class, "Stretch"); - armTurnServo = hardwareMap.get(Servo.class, "Turn"); - armSpinXServo = hardwareMap.get(Servo.class,"SpinX"); - armSpinYServo = hardwareMap.get(Servo.class,"SpinY"); - armGrabServo = hardwareMap.get(Servo.class,"Grab"); - + armStretchServo = hardwareMap.get(Servo.class, "armStretch"); + armTurnServo = hardwareMap.get(Servo.class, "armTurn"); + armSpinXServo = hardwareMap.get(Servo.class, "armSpinX"); + armSpinYServo = hardwareMap.get(Servo.class, "armSpinY"); + armGrabServo = hardwareMap.get(Servo.class, "armGrab"); leftLiftMotor.setDirection(DcMotor.Direction.FORWARD); rightLiftMotor.setDirection(DcMotor.Direction.REVERSE); @@ -55,6 +54,7 @@ public void setLeftPower(double power) { leftLiftMotor.setPower(power); rightLiftMotor.setPower(power); } + public int getLiftPosition() { return leftLiftMotor.getCurrentPosition(); } @@ -62,7 +62,7 @@ public int getLiftPosition() { public void setArmStretchPosition(double position){ armStretchServo.setPosition(position); } - public double getarmStretchPosition() { + public double getArmStretchPosition() { return armStretchServo.getPosition(); } @@ -73,24 +73,24 @@ public double getArmTurnPosition() { return armTurnServo.getPosition(); } - public void setArmSpinXServoPosition(double position){ - armTurnServo.setPosition(position); + public void setArmSpinXPosition(double position){ + armSpinXServo.setPosition(position); } - public double getArmSpinXServoPosition() { - return armTurnServo.getPosition(); + public double getArmSpinXPosition() { + return armSpinXServo.getPosition(); } - public void setArmSpinYServoPosition(double position){ - armTurnServo.setPosition(position); + public void setArmSpinYPosition(double position){ + armSpinYServo.setPosition(position); } - public double getArmSpinYServoPosition() { - return armTurnServo.getPosition(); + public double getArmSpinYPosition() { + return armSpinYServo.getPosition(); } - public void setArmGrabServoPosition(double position){ - armTurnServo.setPosition(position); + public void setArmGrabPosition(double position){ + armGrabServo.setPosition(position); } - public double getArmGrabServoPosition() { - return armTurnServo.getPosition(); + public double getArmGrabPosition() { + return armGrabServo.getPosition(); } } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index ad4b55d5c9d5..df7d3f998335 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -12,7 +12,7 @@ dependencies { implementation 'org.firstinspires.ftc:Hardware:10.1.0' implementation 'org.firstinspires.ftc:FtcCommon:10.1.0' implementation 'org.firstinspires.ftc:Vision:10.1.0' - implementation 'androidx.appcompat:appcompat:1.7.0' + implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'org.openftc:easyopencv:1.7.3' } From 72e0bc01e753f7e2630ed722f5656c927138315f Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Fri, 29 Nov 2024 16:53:38 +0800 Subject: [PATCH 38/93] Feat:Rename the VisionCameraTest --- .../ftc/teamcode/{TEST3.java => VisionCameraAngleTest.java} | 2 +- .../ftc/teamcode/{TEST2.java => VisionCameraColorTest.java} | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{TEST3.java => VisionCameraAngleTest.java} (93%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{TEST2.java => VisionCameraColorTest.java} (93%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionCameraAngleTest.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionCameraAngleTest.java index 8573e9c9ca47..a48c9db9352d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionCameraAngleTest.java @@ -9,7 +9,7 @@ -public class TEST3 extends LinearOpMode { +public class VisionCameraAngleTest extends LinearOpMode { private RobotVision2 robotVision2; @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionCameraColorTest.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionCameraColorTest.java index 903b96831e4c..b4faafbed18d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionCameraColorTest.java @@ -7,7 +7,7 @@ @TeleOp -public class TEST2 extends LinearOpMode { +public class VisionCameraColorTest extends LinearOpMode { private RobotVision robotVision; @Override From 7f0312e7c6bb2b3da7f09065e6bab3982bbdf01d Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 29 Nov 2024 16:56:02 +0800 Subject: [PATCH 39/93] temp commit2 --- .../ftc/teamcode/ManualOpMode.java | 34 ++++++-- .../ftc/teamcode/hardware/RobotAuto.java | 3 +- .../{TEST2.java => test/ColorTest.java} | 4 +- .../ftc/teamcode/test/LeftTest.java | 46 +++++++++++ .../ftc/teamcode/test/MecanumTeleOp.java | 57 ++++++++++++++ .../ftc/teamcode/test/ServoTest.java | 41 ++++++++++ .../ftc/teamcode/test/ThreadTest.java | 78 +++++++++++++++++++ .../ftc/teamcode/test/WheelOffset.java | 63 +++++++++++++++ 8 files changed, 317 insertions(+), 9 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{TEST2.java => test/ColorTest.java} (89%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/LeftTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MecanumTeleOp.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ThreadTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 85cf9a2b397a..cb36829317c3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -20,6 +20,9 @@ public void runOpMode() { previousGamepad1.copy(gamepad1); int liftPosition; + double armSpinXPos = 0.5; + double armSpinYPos = 0.07; + boolean armGrabbing = false; LiftState liftState = LiftState.BOTTOM; ArmState armState = ArmState.IDLE; @@ -52,25 +55,46 @@ public void runOpMode() { //robotArm if(gamepad1.x && !previousGamepad1.x){ if(armState == ArmState.IDLE){ - robotTop.setArmStretchPosition(0.6); + robotTop.setArmStretchPosition(0.71); }else if(armState == ArmState.STRETCHED){ - robotTop.setArmStretchPosition(0); + robotTop.setArmStretchPosition(0.38); } } if(gamepad1.a && !previousGamepad1.a){ if(armState == ArmState.STRETCHED){ - + //TODO }else if(armState == ArmState.TURNED){ - + //TODO + } + } + if(gamepad1.b && !previousGamepad1.b){ + if(armGrabbing){ + robotTop.setArmGrabPosition(0); + }else{ + robotTop.setArmGrabPosition(0.53); } } + if (gamepad1.dpad_up) { + armSpinXPos = Math.min(1,armSpinXPos + 0.05); + robotTop.setArmSpinXPosition(armSpinXPos); + } else if (gamepad1.dpad_down) { + armSpinXPos = Math.max(0, armSpinXPos - 0.05); + robotTop.setArmSpinXPosition(armSpinXPos); + } + if (gamepad1.dpad_right) { + armSpinYPos = Math.min(1,armSpinYPos + 0.05); + robotTop.setArmSpinYPosition(armSpinYPos); + } else if (gamepad1.dpad_left) { + armSpinYPos = Math.max(0, armSpinYPos - 0.05); + robotTop.setArmSpinYPosition(armSpinYPos); + } //telemetry telemetry.addData("pos", liftPosition); telemetry.addData("state", liftState); telemetry.update(); previousGamepad1.copy(gamepad1); - sleep(10); + sleep(50); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java index 2cc095daa296..77a3ea67e4d2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java @@ -72,10 +72,9 @@ public RobotAuto driveStraight(double maxDriveSpeed, return this; } public double getSteeringCorrection(double desiredHeading, double proportionalGain) { - double targetHeading = desiredHeading; // Save for telemetry // Determine the heading current error - double headingError = targetHeading - getHeading(); + double headingError = desiredHeading - getHeading(); // Normalize the error to be within +/- 180 degrees while (headingError > 180) headingError -= 360; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ColorTest.java similarity index 89% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ColorTest.java index 903b96831e4c..6c28a5bc371b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TEST2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ColorTest.java @@ -1,5 +1,5 @@ //测试摄像头颜色识别功能 -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.test; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -7,7 +7,7 @@ @TeleOp -public class TEST2 extends LinearOpMode { +public class ColorTest extends LinearOpMode { private RobotVision robotVision; @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/LeftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/LeftTest.java new file mode 100644 index 000000000000..a87ec37baa26 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/LeftTest.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.test; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +@TeleOp +public class LeftTest extends LinearOpMode { + @Override + public void runOpMode(){ + DcMotor leftLiftMotor = hardwareMap.get(DcMotor.class , "leftLift"); + DcMotor rightLiftMotor = hardwareMap.get(DcMotor.class, "rightLift"); + + leftLiftMotor.setDirection(DcMotor.Direction.FORWARD); + rightLiftMotor.setDirection(DcMotor.Direction.REVERSE); + + leftLiftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightLiftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + leftLiftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + waitForStart(); + + if (isStopRequested()) return; + double liftSpeed = 0; + double pos = 0; + + while (opModeIsActive()) { + pos = leftLiftMotor.getCurrentPosition(); + boolean y = gamepad1.y; + boolean a = gamepad1.a; + if(y && pos <= 1200){ + liftSpeed =0.5; + } else if (a && pos >= 100) { + liftSpeed = -0.5; + }else{liftSpeed = 0;} + leftLiftMotor.setPower(liftSpeed); + rightLiftMotor.setPower(liftSpeed); + telemetry.addData("pos",leftLiftMotor.getCurrentPosition()); + telemetry.update(); + sleep(10); + } + //[-219, 1071] + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MecanumTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MecanumTeleOp.java new file mode 100644 index 000000000000..927af3702ebf --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MecanumTeleOp.java @@ -0,0 +1,57 @@ +package org.firstinspires.ftc.teamcode.test; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +@TeleOp +public class MecanumTeleOp extends LinearOpMode { + @Override + public void runOpMode() { + // Declare our motors + // Make sure your ID's match your configuration + DcMotor frontLeftMotor = hardwareMap.dcMotor.get("FL"); + DcMotor backLeftMotor = hardwareMap.dcMotor.get("BL"); + DcMotor frontRightMotor = hardwareMap.dcMotor.get("FR"); + DcMotor backRightMotor = hardwareMap.dcMotor.get("BR"); + + // Reverse the right side motors. This may be wrong for your setup. + // If your robot moves backwards when commanded to go forwards, + // reverse the left side instead. + // See the note about this earlier on this page. +// frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); +// backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE); + + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + double y = gamepad1.left_stick_y; // Remember, Y stick value is reversed + double x = -gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing + double rx = -gamepad1.right_stick_x; + + frontLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + final double LEFT_REDUCTION = 0.98; + + // Denominator is the largest motor power (absolute value) or 1 + // This ensures all the powers maintain the same ratio, + // but only if at least one is out of the range [-1, 1] + double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1); + double frontLeftPower = (y + x + rx) / denominator * LEFT_REDUCTION; + double backLeftPower = (y - x + rx) / denominator * LEFT_REDUCTION; + double frontRightPower = (y - x - rx) / denominator; + double backRightPower = (y + x - rx) / denominator; + + frontLeftMotor.setPower(frontLeftPower); + backLeftMotor.setPower(backLeftPower); + frontRightMotor.setPower(frontRightPower); + backRightMotor.setPower(backRightPower); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java new file mode 100644 index 000000000000..ed69b71adfd3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java @@ -0,0 +1,41 @@ +package org.firstinspires.ftc.teamcode.test; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +@TeleOp +public class ServoTest extends LinearOpMode { + @Override + public void runOpMode(){ + Servo armServo = hardwareMap.get(Servo.class , "arm"); + //stretch(1): + //turn(3): [0.38-back, 0.71-out] + //grab: [0-open, 0.53-close] + //spinY(4): default: 0.07 + //spinX(2): default: 0.5 + + waitForStart(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + double servoPos = 0; + + while (opModeIsActive()){ + boolean x = gamepad1.x; + boolean b = gamepad1.b; + if(x){ + servoPos += 0.01; + }if(b){ + servoPos -= 0.01; + } + + armServo.setPosition(servoPos); + telemetry.addData("pos", servoPos); + telemetry.update(); + + previousGamepad1.copy(gamepad1); + sleep(100); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ThreadTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ThreadTest.java new file mode 100644 index 000000000000..60e5c974b123 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ThreadTest.java @@ -0,0 +1,78 @@ +package org.firstinspires.ftc.teamcode.test; + +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.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +//It proves out to be useless. +//Never use Thread in the Robot! + +@Disabled +public class ThreadTest extends LinearOpMode { + @Override + public void runOpMode(){ + Servo armServo = hardwareMap.get(Servo.class , "arm"); + waitForStart(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + double a = 0; + Thread t = new Thread(){ + @Override + public void run() { + super.run(); + try { + telemetry.addData("q", 1); + telemetry.update(); + armServo.setPosition(0.9); + sleep(3000); + armServo.setPosition(0.3); + sleep(3000); + armServo.setPosition(0.9); + sleep(3000); + armServo.setPosition(0.3); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + } + }; + t.start(); + while (opModeIsActive()){ + a += 1; + telemetry.addData("time", a); + telemetry.update(); + if(gamepad1.y && !previousGamepad1.y) { + Thread th = timedSetPosition(armServo, 0.2, 2000); + th.start(); + } + sleep(1000); + previousGamepad1.copy(gamepad1); + } + } + public Thread timedSetPosition(Servo servo, double position, double millisecond){ + telemetry.addData("going to run",1); + return new Thread(){ + @Override + public void run() { + final int TICK_MS = 50; + double deltaPos = position - servo.getPosition(); + double iterationCount = millisecond / TICK_MS; + double perPos = deltaPos / iterationCount; + telemetry.addData("running", 1); + try{ + super.start(); + double servoPos = servo.getPosition(); + for(; iterationCount <= 0; iterationCount--){ + servoPos += perPos; + servo.setPosition(servoPos); + sleep(TICK_MS); + telemetry.addData("for_repetition", 1); + } + }catch (InterruptedException e){ + throw new RuntimeException(e); + } + } + }; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java new file mode 100644 index 000000000000..554b5e081814 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java @@ -0,0 +1,63 @@ +package org.firstinspires.ftc.teamcode.test; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + + +@TeleOp +public class WheelOffset extends LinearOpMode { + @Override + public void runOpMode(){ + DcMotor frontLeftMotor = hardwareMap.dcMotor.get("FL"); + DcMotor backLeftMotor = hardwareMap.dcMotor.get("BL"); + DcMotor frontRightMotor = hardwareMap.dcMotor.get("FR"); + DcMotor backRightMotor = hardwareMap.dcMotor.get("BR"); + waitForStart(); + + double k1=1,k2=0.8535,k3=0.9985,k4 = 0.849; + + frontLeftMotor.setPower(1*k1); + backLeftMotor.setPower(1*k2); + frontRightMotor.setPower(1*k3); + backRightMotor.setPower(1*k4); + boolean flag = true; + while (opModeIsActive()){ + if(getRuntime() >= 3 && flag){ + int FLPos = frontLeftMotor.getCurrentPosition(); + int BLPos = backLeftMotor.getCurrentPosition(); + int FRPos = frontLeftMotor.getCurrentPosition(); + int BRPos = backRightMotor.getCurrentPosition(); + frontLeftMotor.setPower(0); + backLeftMotor.setPower(0); + frontRightMotor.setPower(0); + backRightMotor.setPower(0); + telemetry.addData("FL", FLPos); + telemetry.addData("BL", BLPos); + telemetry.addData("FR", FRPos); + telemetry.addData("BR", BRPos); + double[] pos = {FLPos,BLPos,FRPos,BRPos}; + double[] rate = {1, pos[0]/pos[1], pos[0]/pos[2], pos[0]/pos[3]}; + telemetry.addData("rate origin", rate[0]); + telemetry.addData("rate origin", rate[1]); + telemetry.addData("rate origin", rate[2]); + telemetry.addData("rate origin", rate[3]); + double maxinum = Math.max(Math.max(rate[0],rate[1]),Math.max(rate[2],rate[3])); + double coefficient = 1/maxinum; + int t = 0; + for (double each:rate + ) { + rate[t] = each * coefficient; + t += 1; + } + telemetry.addData("k1", rate[0]); + telemetry.addData("k2", rate[1]); + telemetry.addData("k3", rate[2]); + telemetry.addData("k4", rate[3]); + telemetry.update(); + flag = false; + } + sleep(50); + } + } +} From fb271565174a01c907994968694ed5509f05cf76 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Fri, 29 Nov 2024 17:04:40 +0800 Subject: [PATCH 40/93] Feat:Add Tests package --- .../org/firstinspires/ftc/teamcode/{ => Tests}/ServoTest.java | 2 +- .../ftc/teamcode/{ => Tests}/VisionCameraAngleTest.java | 2 +- .../ftc/teamcode/{ => Tests}/VisionCameraColorTest.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => Tests}/ServoTest.java (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => Tests}/VisionCameraAngleTest.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => Tests}/VisionCameraColorTest.java (94%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/ServoTest.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/ServoTest.java index 342cace6e681..ac806149f7bb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/ServoTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.Tests; 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/VisionCameraAngleTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraAngleTest.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionCameraAngleTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraAngleTest.java index a48c9db9352d..60091644f7fe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionCameraAngleTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraAngleTest.java @@ -1,5 +1,5 @@ //测试摄像头角度识别功能 -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.Tests; 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/VisionCameraColorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraColorTest.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionCameraColorTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraColorTest.java index b4faafbed18d..000bb95ab02e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/VisionCameraColorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraColorTest.java @@ -1,5 +1,5 @@ //测试摄像头颜色识别功能 -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.Tests; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; From 97550a0c690bc3272ecce3994feba72a2e95522a Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Fri, 29 Nov 2024 17:09:37 +0800 Subject: [PATCH 41/93] Feat:Enhanced dtx's Vision --- .../ftc/teamcode/Tests/VisionCameraAngleTest.java | 10 +++++----- .../ftc/teamcode/Tests/VisionCameraColorTest.java | 10 +++++----- .../{RobotVision2.java => RobotVisionAngle.java} | 6 +++--- .../{RobotVision.java => RobotVisionColor.java} | 2 +- 4 files changed, 14 insertions(+), 14 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/{RobotVision2.java => RobotVisionAngle.java} (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/{RobotVision.java => RobotVisionColor.java} (99%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraAngleTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraAngleTest.java index 60091644f7fe..7cc70c94fca8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraAngleTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraAngleTest.java @@ -3,20 +3,20 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.hardware.RobotVision2; +import org.firstinspires.ftc.teamcode.hardware.RobotVisionAngle; @TeleOp public class VisionCameraAngleTest extends LinearOpMode { - private RobotVision2 robotVision2; + private RobotVisionAngle robotVisionAngle; @Override public void runOpMode() { - robotVision2 = new RobotVision2(); + robotVisionAngle = new RobotVisionAngle(); - robotVision2.initialize(hardwareMap); // 初始化摄像头 + robotVisionAngle.initialize(hardwareMap); // 初始化摄像头 telemetry.addData("Status", "Initialized"); telemetry.update(); @@ -25,7 +25,7 @@ public void runOpMode() { while (opModeIsActive()) { // 显示角度检测结果 - telemetry.addData("Detected Angle", robotVision2.getDetectedAngle()); + telemetry.addData("Detected Angle", robotVisionAngle.getDetectedAngle()); telemetry.update(); sleep(100); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraColorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraColorTest.java index 000bb95ab02e..b3dd72bd60b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraColorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/VisionCameraColorTest.java @@ -3,18 +3,18 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.hardware.RobotVision; +import org.firstinspires.ftc.teamcode.hardware.RobotVisionColor; @TeleOp public class VisionCameraColorTest extends LinearOpMode { - private RobotVision robotVision; + private RobotVisionColor robotVisionColor; @Override public void runOpMode() { - robotVision = new RobotVision(); + robotVisionColor = new RobotVisionColor(); - robotVision.initialize(hardwareMap); // 初始化摄像头 + robotVisionColor.initialize(hardwareMap); // 初始化摄像头 telemetry.addData("Status", "Initialized"); telemetry.update(); @@ -23,7 +23,7 @@ public void runOpMode() { while (opModeIsActive()) { // 显示颜色检测结果 - telemetry.addData("Center Color", robotVision.getDetectedColor()); + telemetry.addData("Center Color", robotVisionColor.getDetectedColor()); telemetry.update(); sleep(100); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision2.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java index 32d976f0cdcc..094b43014698 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java @@ -1,7 +1,7 @@ package org.firstinspires.ftc.teamcode.hardware; -// + import com.qualcomm.robotcore.hardware.HardwareMap; -import org.opencv.core.Core; + import org.opencv.core.Mat; import org.opencv.core.Rect; import org.opencv.core.Size; @@ -12,7 +12,7 @@ import org.openftc.easyopencv.OpenCvPipeline; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -public class RobotVision2 { +public class RobotVisionAngle { private OpenCvCamera webcam; private double detectedAngle = 0; // 角度变量 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionColor.java similarity index 99% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionColor.java index 283bab7f45de..52ef1ce1d48f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionColor.java @@ -16,7 +16,7 @@ import org.openftc.easyopencv.OpenCvPipeline; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -public class RobotVision { +public class RobotVisionColor { private OpenCvCamera webcam; private String detectedColor = "Unknown"; From be85839e9fe449162a690bda756b52e925edb6b1 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 29 Nov 2024 20:05:20 +0800 Subject: [PATCH 42/93] Feat: slow down the servos --- .../ftc/teamcode/ManualOpMode.java | 54 ++++++++++++++++--- .../ftc/teamcode/test/ServoTest.java | 4 +- 2 files changed, 48 insertions(+), 10 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 75065fd50433..8a178fb8b20c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -20,6 +20,8 @@ public void runOpMode() { previousGamepad1.copy(gamepad1); int liftPosition; + double armStretchPos = 0; + double armTurnPos = 0; double armSpinXPos = 0.5; double armSpinYPos = 0.07; boolean armGrabbing = false; @@ -55,23 +57,55 @@ public void runOpMode() { //robotArm if(gamepad1.x && !previousGamepad1.x){ if(armState == ArmState.IDLE){ - robotTop.setArmStretchPosition(0.3); + armStretchPos = 0.3; + armState = ArmState.STRETCHED; + robotTop.setArmStretchPosition(armStretchPos); }else if(armState == ArmState.STRETCHED){ - robotTop.setArmStretchPosition(0); + armState = ArmState.WITHDRAWING; + } + } + if(armState == ArmState.WITHDRAWING){ + if(armStretchPos <= 0){ + armState = ArmState.IDLE; + }else{ + armStretchPos -= 0.04; + robotTop.setArmStretchPosition(armStretchPos); } } if(gamepad1.a && !previousGamepad1.a){ if(armState == ArmState.STRETCHED){ - robotTop.setArmTurnPosition(0.71); + armState = ArmState.TURNING_OUT; }else if(armState == ArmState.TURNED){ - robotTop.setArmTurnPosition(0.38); + armState = ArmState.TURNING_BACK; + } + } + if(armState == ArmState.TURNING_OUT){ + if(armTurnPos >= 0.68){ + armTurnPos = 0.71; + robotTop.setArmTurnPosition(armTurnPos); + armState = ArmState.TURNED; + }else{ + armTurnPos += 0.03; + robotTop.setArmTurnPosition(armTurnPos); + } + } + if(armState == ArmState.TURNING_BACK){ + if(armTurnPos <= 0.42){ + armTurnPos = 0.38; + robotTop.setArmTurnPosition(armTurnPos); + armState = ArmState.STRETCHED; + }else{ + armTurnPos -= 0.03; + robotTop.setArmTurnPosition(armTurnPos); } } if(gamepad1.b && !previousGamepad1.b){ if(armGrabbing){ - robotTop.setArmGrabPosition(0); + robotTop.setArmGrabPosition(0.73); + armGrabbing = false; }else{ - robotTop.setArmGrabPosition(0.53); + robotTop.setArmGrabPosition(0.2); + armGrabbing = true; } } if (gamepad1.dpad_up) { @@ -90,8 +124,12 @@ public void runOpMode() { } //telemetry - telemetry.addData("pos", liftPosition); + telemetry.addData("liftPos", liftPosition); telemetry.addData("state", liftState); + telemetry.addData("ArmState", armState); + telemetry.addData("XPos", armSpinXPos); + telemetry.addData("YPos", armSpinYPos); + telemetry.addData("grab", armGrabbing); telemetry.update(); previousGamepad1.copy(gamepad1); sleep(50); @@ -102,5 +140,5 @@ enum LiftState{ BOTTOM,TOP } enum ArmState{ - IDLE,STRETCHED,TURNED + IDLE,WITHDRAWING,STRETCHED,TURNING_OUT,TURNED,TURNING_BACK } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java index ed69b71adfd3..f175c2af2864 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java @@ -9,8 +9,8 @@ public class ServoTest extends LinearOpMode { @Override public void runOpMode(){ - Servo armServo = hardwareMap.get(Servo.class , "arm"); - //stretch(1): + Servo armServo = hardwareMap.get(Servo.class , "armGrab"); + //stretch(1): [0, 0.3] //turn(3): [0.38-back, 0.71-out] //grab: [0-open, 0.53-close] //spinY(4): default: 0.07 From f4fc949080b62062a948c10903125fbfaa8e2471 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 29 Nov 2024 20:05:20 +0800 Subject: [PATCH 43/93] Feat: slow down the servos --- .../ftc/teamcode/ManualOpMode.java | 54 ++++++++++++++++--- .../ftc/teamcode/test/ServoTest.java | 6 +-- 2 files changed, 49 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 75065fd50433..8a178fb8b20c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -20,6 +20,8 @@ public void runOpMode() { previousGamepad1.copy(gamepad1); int liftPosition; + double armStretchPos = 0; + double armTurnPos = 0; double armSpinXPos = 0.5; double armSpinYPos = 0.07; boolean armGrabbing = false; @@ -55,23 +57,55 @@ public void runOpMode() { //robotArm if(gamepad1.x && !previousGamepad1.x){ if(armState == ArmState.IDLE){ - robotTop.setArmStretchPosition(0.3); + armStretchPos = 0.3; + armState = ArmState.STRETCHED; + robotTop.setArmStretchPosition(armStretchPos); }else if(armState == ArmState.STRETCHED){ - robotTop.setArmStretchPosition(0); + armState = ArmState.WITHDRAWING; + } + } + if(armState == ArmState.WITHDRAWING){ + if(armStretchPos <= 0){ + armState = ArmState.IDLE; + }else{ + armStretchPos -= 0.04; + robotTop.setArmStretchPosition(armStretchPos); } } if(gamepad1.a && !previousGamepad1.a){ if(armState == ArmState.STRETCHED){ - robotTop.setArmTurnPosition(0.71); + armState = ArmState.TURNING_OUT; }else if(armState == ArmState.TURNED){ - robotTop.setArmTurnPosition(0.38); + armState = ArmState.TURNING_BACK; + } + } + if(armState == ArmState.TURNING_OUT){ + if(armTurnPos >= 0.68){ + armTurnPos = 0.71; + robotTop.setArmTurnPosition(armTurnPos); + armState = ArmState.TURNED; + }else{ + armTurnPos += 0.03; + robotTop.setArmTurnPosition(armTurnPos); + } + } + if(armState == ArmState.TURNING_BACK){ + if(armTurnPos <= 0.42){ + armTurnPos = 0.38; + robotTop.setArmTurnPosition(armTurnPos); + armState = ArmState.STRETCHED; + }else{ + armTurnPos -= 0.03; + robotTop.setArmTurnPosition(armTurnPos); } } if(gamepad1.b && !previousGamepad1.b){ if(armGrabbing){ - robotTop.setArmGrabPosition(0); + robotTop.setArmGrabPosition(0.73); + armGrabbing = false; }else{ - robotTop.setArmGrabPosition(0.53); + robotTop.setArmGrabPosition(0.2); + armGrabbing = true; } } if (gamepad1.dpad_up) { @@ -90,8 +124,12 @@ public void runOpMode() { } //telemetry - telemetry.addData("pos", liftPosition); + telemetry.addData("liftPos", liftPosition); telemetry.addData("state", liftState); + telemetry.addData("ArmState", armState); + telemetry.addData("XPos", armSpinXPos); + telemetry.addData("YPos", armSpinYPos); + telemetry.addData("grab", armGrabbing); telemetry.update(); previousGamepad1.copy(gamepad1); sleep(50); @@ -102,5 +140,5 @@ enum LiftState{ BOTTOM,TOP } enum ArmState{ - IDLE,STRETCHED,TURNED + IDLE,WITHDRAWING,STRETCHED,TURNING_OUT,TURNED,TURNING_BACK } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java index ed69b71adfd3..bd88efefbb72 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java @@ -9,10 +9,10 @@ public class ServoTest extends LinearOpMode { @Override public void runOpMode(){ - Servo armServo = hardwareMap.get(Servo.class , "arm"); - //stretch(1): + Servo armServo = hardwareMap.get(Servo.class , "armGrab"); + //stretch(1): [0, 0.3] //turn(3): [0.38-back, 0.71-out] - //grab: [0-open, 0.53-close] + //grab(5): [0-open, 0.53-close] //spinY(4): default: 0.07 //spinX(2): default: 0.5 From ed24a6d4406a27a9cb47cd688111822faca00baf Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Fri, 29 Nov 2024 20:24:38 +0800 Subject: [PATCH 44/93] Feat:Fixed a lot errors --- MeepMeep/.gitignore | 1 + MeepMeep/build.gradle | 17 +++++++++++++++++ settings.gradle | 1 + 3 files changed, 19 insertions(+) create mode 100644 MeepMeep/.gitignore create mode 100644 MeepMeep/build.gradle diff --git a/MeepMeep/.gitignore b/MeepMeep/.gitignore new file mode 100644 index 000000000000..42afabfd2abe --- /dev/null +++ b/MeepMeep/.gitignore @@ -0,0 +1 @@ +/build \ No newline at end of file diff --git a/MeepMeep/build.gradle b/MeepMeep/build.gradle new file mode 100644 index 000000000000..2a2d8e26a0b9 --- /dev/null +++ b/MeepMeep/build.gradle @@ -0,0 +1,17 @@ +plugins { + id 'java-library' +} + +java { + sourceCompatibility = JavaVersion.VERSION_18 + targetCompatibility = JavaVersion.VERSION_18 +} + +repositories { + maven { url = 'https://jitpack.io' } + maven { url = 'https://maven.brott.dev/' } +} + +dependencies { + implementation 'com.github.rh-robotics:MeepMeep:v1.0.0' +} \ No newline at end of file diff --git a/settings.gradle b/settings.gradle index 9e2cfb3b4b03..a56e34042da1 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,2 +1,3 @@ include ':FtcRobotController' include ':TeamCode' +include ':MeepMeep' From ab63216dd7294242956f09297bdc179929fcee32 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Fri, 29 Nov 2024 20:25:27 +0800 Subject: [PATCH 45/93] Feat:Fixed a lot errors --- .../java/com/example/meepmeep/MeepTurbo.java | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java diff --git a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java new file mode 100644 index 000000000000..8b30039e08d6 --- /dev/null +++ b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java @@ -0,0 +1,35 @@ +package com.example.meepmeep; + +import com.acmerobotics.roadrunner.geometry.Pose2d; + +import org.rowlandhall.meepmeep.MeepMeep; +import org.rowlandhall.meepmeep.roadrunner.DefaultBotBuilder; +import org.rowlandhall.meepmeep.roadrunner.entity.RoadRunnerBotEntity; + +public class MeepTurbo { + public static void main(String[] args) { + MeepMeep meepMeep = new MeepMeep(800); + + RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep) + // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .followTrajectorySequence(drive -> drive.trajectorySequenceBuilder(new Pose2d(0, 0, 0)) + .forward(30) + .turn(Math.toRadians(90)) + .forward(30) + .turn(Math.toRadians(90)) + .forward(30) + .turn(Math.toRadians(90)) + .forward(30) + .turn(Math.toRadians(90)) + .build()); + + + meepMeep.setBackground(MeepMeep.Background.FIELD_INTOTHEDEEP_JUICE_DARK) + .setDarkMode(true) + .setBackgroundAlpha(0.95f) + .addEntity(myBot) + .start(); + } + } + From 231868209a04df969f7670c4f7e4d69e0098bcb4 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 29 Nov 2024 20:36:57 +0800 Subject: [PATCH 46/93] Sort the files, add RoadRunner --- .../ftc/teamcode/ManualOpModeTest.java | 21 ---------- .../ftc/teamcode/Tests/ServoTests.java | 38 ------------------- .../teamcode/{ => test}/AUTOOpModeTest.java | 2 +- 3 files changed, 1 insertion(+), 60 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpModeTest.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/ServoTests.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => test}/AUTOOpModeTest.java (93%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpModeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpModeTest.java deleted file mode 100644 index 9b187e0f497a..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpModeTest.java +++ /dev/null @@ -1,21 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; - -public class ManualOpModeTest extends LinearOpMode -{ - - DcMotor motor; - @Override - public void runOpMode() throws InterruptedException { - - waitForStart(); - float x = gamepad1.left_stick_x; - if ( gamepad1.left_stick_x > 0) { - - motor.setPower(x); - - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/ServoTests.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/ServoTests.java deleted file mode 100644 index cb6c757d81df..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tests/ServoTests.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.firstinspires.ftc.teamcode.Tests; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.Servo; - -public class ServoTests extends LinearOpMode { - - private Servo servo1; - private double position = 0.5; - private final double step = 0.01; - - @Override - public void runOpMode() { - // 初始化舵机 - servo1 = hardwareMap.get(Servo.class, "servo1"); - - waitForStart(); - - while (opModeIsActive()) { - // 控制舵机位置 - if (gamepad1.a && position + step <= 1.0) { - - position += step; - servo1.setPosition(position); - sleep(50); - } else if (gamepad1.b && position - step >= 0) { - - position -= step; - servo1.setPosition(position); - sleep(50); - } - - // 添加其他舵机控制逻辑 - telemetry.addData("Servo Position", servo1.getPosition()); - telemetry.update(); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AUTOOpModeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AUTOOpModeTest.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AUTOOpModeTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AUTOOpModeTest.java index c4fe82c6689d..c714fecda2bf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AUTOOpModeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AUTOOpModeTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.test; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; From d6869138c7bf8f2d2af03bf32f8e33ca0bc448d9 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 29 Nov 2024 21:15:03 +0800 Subject: [PATCH 47/93] Sort the files, add RoadRunner --- .../main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java | 2 +- .../java/org/firstinspires/ftc/teamcode/test/ServoTest.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 8a178fb8b20c..c4a9671b3fec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -101,7 +101,7 @@ public void runOpMode() { } if(gamepad1.b && !previousGamepad1.b){ if(armGrabbing){ - robotTop.setArmGrabPosition(0.73); + robotTop.setArmGrabPosition(0.83); armGrabbing = false; }else{ robotTop.setArmGrabPosition(0.2); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java index bd88efefbb72..bd05f52f59ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java @@ -9,7 +9,7 @@ public class ServoTest extends LinearOpMode { @Override public void runOpMode(){ - Servo armServo = hardwareMap.get(Servo.class , "armGrab"); + Servo armServo = hardwareMap.get(Servo.class , "arm"); //stretch(1): [0, 0.3] //turn(3): [0.38-back, 0.71-out] //grab(5): [0-open, 0.53-close] From 8171aceb081f02a224a037965c485d982d98727b Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Fri, 29 Nov 2024 21:23:54 +0800 Subject: [PATCH 48/93] Feat:Fixed errors --- .../java/org/firstinspires/ftc/teamcode/test/ServoTest.java | 2 +- build.dependencies.gradle | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java index bd88efefbb72..bd05f52f59ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java @@ -9,7 +9,7 @@ public class ServoTest extends LinearOpMode { @Override public void runOpMode(){ - Servo armServo = hardwareMap.get(Servo.class , "armGrab"); + Servo armServo = hardwareMap.get(Servo.class , "arm"); //stretch(1): [0, 0.3] //turn(3): [0.38-back, 0.71-out] //grab(5): [0-open, 0.53-close] diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 8daf889f9314..fa64bc0e7a95 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -13,7 +13,7 @@ dependencies { implementation 'org.firstinspires.ftc:Hardware:10.1.0' implementation 'org.firstinspires.ftc:FtcCommon:10.1.0' implementation 'org.firstinspires.ftc:Vision:10.1.0' - implementation 'androidx.appcompat:appcompat:1.7.0' + implementation 'androidx.appcompat:appcompat:1.2.0' implementation('com.acmerobotics.dashboard:dashboard:0.4.16') { exclude group: 'org.firstinspires.ftc' } From 4a6c15fdc6b4f0ed3a634dc6f1298a254ac093b8 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Fri, 29 Nov 2024 22:07:38 +0800 Subject: [PATCH 49/93] Feat:Added MeepMeepTest --- MeepMeep/build.gradle | 4 +- .../java/com/example/meepmeep/MeepTurbo.java | 39 ++++++++++++------- 2 files changed, 28 insertions(+), 15 deletions(-) diff --git a/MeepMeep/build.gradle b/MeepMeep/build.gradle index 2a2d8e26a0b9..cb8c19758d0b 100644 --- a/MeepMeep/build.gradle +++ b/MeepMeep/build.gradle @@ -3,8 +3,8 @@ plugins { } java { - sourceCompatibility = JavaVersion.VERSION_18 - targetCompatibility = JavaVersion.VERSION_18 + sourceCompatibility = JavaVersion.VERSION_17 + targetCompatibility = JavaVersion.VERSION_17 } repositories { diff --git a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java index 8b30039e08d6..cf56cc860d78 100644 --- a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java +++ b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java @@ -1,35 +1,48 @@ package com.example.meepmeep; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; import org.rowlandhall.meepmeep.MeepMeep; +import org.rowlandhall.meepmeep.core.colorscheme.scheme.ColorSchemeRedDark; import org.rowlandhall.meepmeep.roadrunner.DefaultBotBuilder; import org.rowlandhall.meepmeep.roadrunner.entity.RoadRunnerBotEntity; public class MeepTurbo { public static void main(String[] args) { + // Declare a MeepMeep instance + // With a field size of 800 pixels MeepMeep meepMeep = new MeepMeep(800); RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep) - // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width + // Required: Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) - .followTrajectorySequence(drive -> drive.trajectorySequenceBuilder(new Pose2d(0, 0, 0)) - .forward(30) - .turn(Math.toRadians(90)) - .forward(30) - .turn(Math.toRadians(90)) - .forward(30) - .turn(Math.toRadians(90)) - .forward(30) - .turn(Math.toRadians(90)) - .build()); + // Option: Set theme. Default = ColorSchemeRedDark() + .setColorScheme(new ColorSchemeRedDark()) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(new Pose2d(0, 0, 0)) + .forward(30) + .turn(Math.toRadians(90)) + .forward(30) + .addDisplacementMarker(() -> { + /* Everything in the marker callback should be commented out */ + // bot.shooter.shoot() + // bot.wobbleArm.lower() + }) + .turn(Math.toRadians(90)) + .splineTo(new Vector2d(10, 15), 0) + .turn(Math.toRadians(90)) + .build() + ); - meepMeep.setBackground(MeepMeep.Background.FIELD_INTOTHEDEEP_JUICE_DARK) + // Set field image + meepMeep.setBackground(MeepMeep.Background.FIELD_FREIGHTFRENZY_ADI_DARK) .setDarkMode(true) + // Background opacity from 0-1 .setBackgroundAlpha(0.95f) .addEntity(myBot) .start(); - } } +} From 4606a63ccc0fe98a9e2e49da3258cb2d847991f7 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 29 Nov 2024 23:43:54 +0800 Subject: [PATCH 50/93] Style: use constants & formatting --- .../ftc/teamcode/ManualOpMode.java | 109 +++++++++++------- .../ftc/teamcode/hardware/RobotChassis.java | 2 + .../ftc/teamcode/hardware/RobotTop.java | 15 ++- 3 files changed, 77 insertions(+), 49 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index c4a9671b3fec..43fd246f2c63 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -12,6 +12,25 @@ public class ManualOpMode extends LinearOpMode { RobotChassis robotChassis; RobotTop robotTop; + // servoName(port): positionRange[a, b]; defaultPos + // -------------------------------------------- + // stretch(1): [0-back, 0.3-out]; default: 0 + // spinX(2): [0, 1]; default: 0.5 + // turn(3): [0.38-back, 0.71-out]; default: 0.38 + // spinY(4): [0, 1]; default: 0.07 + // grab(5): [0.2-open, 0.83-close]; default: 0.2 + + final int LIFT_TOP_POSITION = 1250; + final int LIFT_BOTTOM_POSITION = 50; + final double STRETCH_BACK_POSITION = 0; + final double STRETCH_OUT_POSITION = 0.3; + final double SPIN_X_DEFAULT_POSITION = 0.5; + final double SPIN_Y_DEFAULT_POSITION = 0.07; + final double TURN_BACK_POSITION = 0.38; + final double TURN_OUT_POSITION = 0.71; + final double GRAB_OPEN_POSITION = 0.2; + final double GRAB_CLOSE_POSITION = 0.83; + @Override public void runOpMode() { robotChassis = new RobotChassis(this); @@ -20,110 +39,110 @@ public void runOpMode() { previousGamepad1.copy(gamepad1); int liftPosition; - double armStretchPos = 0; - double armTurnPos = 0; - double armSpinXPos = 0.5; - double armSpinYPos = 0.07; + double armStretchPos = STRETCH_BACK_POSITION; + double armTurnPos = TURN_BACK_POSITION; + double armSpinXPos = SPIN_X_DEFAULT_POSITION; + double armSpinYPos = SPIN_Y_DEFAULT_POSITION; boolean armGrabbing = false; LiftState liftState = LiftState.BOTTOM; ArmState armState = ArmState.IDLE; waitForStart(); - while (opModeIsActive()){ + while (opModeIsActive()) { robotChassis.driveRobot(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); - //robotLift + // robotLift liftPosition = robotTop.getLiftPosition(); - if(liftState == LiftState.BOTTOM){ - if(gamepad1.y && liftPosition <= 1100){ + if (liftState == LiftState.BOTTOM) { + if (gamepad1.y && liftPosition <= LIFT_TOP_POSITION - 150) { robotTop.setLeftPower(0.7); - }else if (liftPosition >= 1250) { + } else if (liftPosition >= LIFT_TOP_POSITION) { robotTop.setLeftPower(0); liftState = LiftState.TOP; - } else if (liftPosition >= 1100) { + } else if (liftPosition >= LIFT_TOP_POSITION - 150) { robotTop.setLeftPower(0.3); } } else if (liftState == LiftState.TOP) { - if(gamepad1.y && liftPosition >= 200){ + if (gamepad1.y && liftPosition >= LIFT_BOTTOM_POSITION + 150) { robotTop.setLeftPower(-0.6); - } else if (liftPosition <= 50) { + } else if (liftPosition <= LIFT_BOTTOM_POSITION) { robotTop.setLeftPower(0); liftState = LiftState.BOTTOM; - }else if (liftPosition <= 200) { + } else if (liftPosition <= LIFT_BOTTOM_POSITION + 150) { robotTop.setLeftPower(-0.2); } } - //robotArm - if(gamepad1.x && !previousGamepad1.x){ - if(armState == ArmState.IDLE){ - armStretchPos = 0.3; + // robotArm + if (gamepad1.x && !previousGamepad1.x) { + if (armState == ArmState.IDLE) { + armStretchPos = STRETCH_OUT_POSITION; armState = ArmState.STRETCHED; robotTop.setArmStretchPosition(armStretchPos); - }else if(armState == ArmState.STRETCHED){ + } else if (armState == ArmState.STRETCHED) { armState = ArmState.WITHDRAWING; } } - if(armState == ArmState.WITHDRAWING){ - if(armStretchPos <= 0){ + if (armState == ArmState.WITHDRAWING) { + if (armStretchPos <= STRETCH_BACK_POSITION) { armState = ArmState.IDLE; - }else{ + } else { armStretchPos -= 0.04; robotTop.setArmStretchPosition(armStretchPos); } } - if(gamepad1.a && !previousGamepad1.a){ - if(armState == ArmState.STRETCHED){ + if (gamepad1.a && !previousGamepad1.a) { + if (armState == ArmState.STRETCHED) { armState = ArmState.TURNING_OUT; - }else if(armState == ArmState.TURNED){ + } else if (armState == ArmState.TURNED) { armState = ArmState.TURNING_BACK; } } - if(armState == ArmState.TURNING_OUT){ - if(armTurnPos >= 0.68){ - armTurnPos = 0.71; + if (armState == ArmState.TURNING_OUT) { + if (armTurnPos >= TURN_OUT_POSITION - 0.05) { + armTurnPos = TURN_OUT_POSITION; robotTop.setArmTurnPosition(armTurnPos); armState = ArmState.TURNED; - }else{ + } else { armTurnPos += 0.03; robotTop.setArmTurnPosition(armTurnPos); } } - if(armState == ArmState.TURNING_BACK){ - if(armTurnPos <= 0.42){ - armTurnPos = 0.38; + if (armState == ArmState.TURNING_BACK) { + if (armTurnPos <= TURN_BACK_POSITION + 0.05) { + armTurnPos = TURN_BACK_POSITION; robotTop.setArmTurnPosition(armTurnPos); armState = ArmState.STRETCHED; - }else{ + } else { armTurnPos -= 0.03; robotTop.setArmTurnPosition(armTurnPos); } } - if(gamepad1.b && !previousGamepad1.b){ - if(armGrabbing){ - robotTop.setArmGrabPosition(0.83); + if (gamepad1.b && !previousGamepad1.b) { + if (armGrabbing) { + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); armGrabbing = false; - }else{ - robotTop.setArmGrabPosition(0.2); + } else { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); armGrabbing = true; } } if (gamepad1.dpad_up) { - armSpinXPos = Math.min(1,armSpinXPos + 0.05); + armSpinXPos = Math.min(1, armSpinXPos + 0.05); robotTop.setArmSpinXPosition(armSpinXPos); } else if (gamepad1.dpad_down) { armSpinXPos = Math.max(0, armSpinXPos - 0.05); robotTop.setArmSpinXPosition(armSpinXPos); } if (gamepad1.dpad_right) { - armSpinYPos = Math.min(1,armSpinYPos + 0.05); + armSpinYPos = Math.min(1, armSpinYPos + 0.05); robotTop.setArmSpinYPosition(armSpinYPos); } else if (gamepad1.dpad_left) { armSpinYPos = Math.max(0, armSpinYPos - 0.05); robotTop.setArmSpinYPosition(armSpinYPos); } - //telemetry + // telemetry telemetry.addData("liftPos", liftPosition); telemetry.addData("state", liftState); telemetry.addData("ArmState", armState); @@ -136,9 +155,11 @@ public void runOpMode() { } } } -enum LiftState{ - BOTTOM,TOP + +enum LiftState { + BOTTOM, TOP } -enum ArmState{ - IDLE,WITHDRAWING,STRETCHED,TURNING_OUT,TURNED,TURNING_BACK + +enum ArmState { + IDLE, WITHDRAWING, STRETCHED, TURNING_OUT, TURNED, TURNING_BACK } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java index c4766bac9339..27766929b96f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java @@ -16,12 +16,14 @@ public class RobotChassis { private DcMotor leftBackDrive; private DcMotor rightFrontDrive; private DcMotor rightBackDrive; + public RobotChassis(LinearOpMode opMode) { this.opMode = opMode; hardwareMap = opMode.hardwareMap; telemetry = opMode.telemetry; this.initMovement(); } + public void initMovement() { // init motors and servos leftFrontDrive = hardwareMap.get(DcMotor.class, "FL"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java index 25b14a7e074a..d872c008f049 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java @@ -59,37 +59,42 @@ public int getLiftPosition() { return leftLiftMotor.getCurrentPosition(); } - public void setArmStretchPosition(double position){ + public void setArmStretchPosition(double position) { armStretchServo.setPosition(position); } + public double getArmStretchPosition() { return armStretchServo.getPosition(); } - public void setArmTurnPosition(double position){ + public void setArmTurnPosition(double position) { armTurnServo.setPosition(position); } + public double getArmTurnPosition() { return armTurnServo.getPosition(); } - public void setArmSpinXPosition(double position){ + public void setArmSpinXPosition(double position) { armSpinXServo.setPosition(position); } + public double getArmSpinXPosition() { return armSpinXServo.getPosition(); } - public void setArmSpinYPosition(double position){ + public void setArmSpinYPosition(double position) { armSpinYServo.setPosition(position); } + public double getArmSpinYPosition() { return armSpinYServo.getPosition(); } - public void setArmGrabPosition(double position){ + public void setArmGrabPosition(double position) { armGrabServo.setPosition(position); } + public double getArmGrabPosition() { return armGrabServo.getPosition(); } From 866a99f7df6a905e2ae7d1697b92f359ebaecf2d Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sun, 1 Dec 2024 20:29:09 +0800 Subject: [PATCH 51/93] Feat:Added RRTest --- .../ftc/teamcode/test/RRTest.java | 39 +++++++++++++++++++ build.dependencies.gradle | 1 + 2 files changed, 40 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java new file mode 100644 index 000000000000..fea6ed8d2cef --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -0,0 +1,39 @@ +package org.firstinspires.ftc.teamcode.test; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; + + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder; + +@TeleOp(name = "RRTest") +public class RRTest extends LinearOpMode { + + @Override + public void runOpMode() throws InterruptedException { + // 初始化驱动对象 + SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); + + // 定义机器人起始位置 + Pose2d startPose = new Pose2d(23.26, -68.58, Math.toRadians(90.00)); + drive.setPoseEstimate(startPose); + + // 构建轨迹序列 + TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(startPose) + .splineTo(new Vector2d(24.67, -33.13), Math.toRadians(87.72)) + .splineTo(new Vector2d(51.86, -26.28), Math.toRadians(14.14)) + .splineTo(new Vector2d(57.10, -48.64), Math.toRadians(-76.82)) + .build(); + // 等待开始命令 + waitForStart(); + // 开始跟踪轨迹 + if (opModeIsActive()) { + drive.followTrajectorySequence(trajectory0); + } + } +} + diff --git a/build.dependencies.gradle b/build.dependencies.gradle index fa64bc0e7a95..1ec9869b3ed6 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -14,6 +14,7 @@ dependencies { implementation 'org.firstinspires.ftc:FtcCommon:10.1.0' implementation 'org.firstinspires.ftc:Vision:10.1.0' implementation 'androidx.appcompat:appcompat:1.2.0' + implementation 'com.acmerobotics.roadrunner:core:0.5.2' implementation('com.acmerobotics.dashboard:dashboard:0.4.16') { exclude group: 'org.firstinspires.ftc' } From 92baaa9b5d46540fb1118ba26a5ba2a8d3d758f2 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 2 Dec 2024 13:18:48 +0800 Subject: [PATCH 52/93] Feat:Change Op --- .../java/org/firstinspires/ftc/teamcode/test/RRTest.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java index fea6ed8d2cef..4f0b5315a2a6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -4,13 +4,14 @@ import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder; -@TeleOp(name = "RRTest") +@Autonomous (name = "RRTest") + public class RRTest extends LinearOpMode { @Override From 15c0dc19708512dd057eccf4f475665b26dada61 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Mon, 2 Dec 2024 13:28:25 +0800 Subject: [PATCH 53/93] =?UTF-8?q?add=20the=20=E8=B4=9F=E6=95=B0=20in=20rot?= =?UTF-8?q?ate?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../ftc/teamcode/hardware/RobotVisionAngle.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java index 094b43014698..e2d215daaaf4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java @@ -70,10 +70,14 @@ public Mat processFrame(Mat input) { double dx = line[2] - line[0]; double dy = line[3] - line[1]; angle = Math.toDegrees(Math.atan2(dy, dx)); + + // 调整角度方向：向左为正，向右为负 if (angle < 0) { - angle += 180; + angle += 360; + } + if (angle > 180) { + angle -= 360; } - angle = Math.abs(angle - 90); break; // 考虑简单起见只取第一条检测到的线条 } } @@ -83,4 +87,4 @@ public Mat processFrame(Mat input) { return input; } } -} \ No newline at end of file +} From ec67232e571c950af6daef1ce26a9fab881e414d Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 2 Dec 2024 13:29:10 +0800 Subject: [PATCH 54/93] Feat:Change Op --- .../main/java/org/firstinspires/ftc/teamcode/test/RRTest.java | 1 - 1 file changed, 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java index 4f0b5315a2a6..c533275262ed 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -3,7 +3,6 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; - import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; From a5c10b01ca4b3101a68d1317a9cdb31305adeaa1 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Mon, 2 Dec 2024 13:33:32 +0800 Subject: [PATCH 55/93] add the minus in rotate --- .../firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java index e2d215daaaf4..aeb36f6e46ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java @@ -14,7 +14,7 @@ public class RobotVisionAngle { private OpenCvCamera webcam; - private double detectedAngle = 0; // 角度变量 + private double detectedAngle = 0; // 角度变量 public void initialize(HardwareMap hardwareMap) { int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( From 8d00b89cde8df1d3e191805ea75ccf544dbac26e Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 2 Dec 2024 13:41:00 +0800 Subject: [PATCH 56/93] Feat:Replace Space --- .../firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java index ddcdfb5a27c0..c8d5a701fb29 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java @@ -6,8 +6,6 @@ @TeleOp - - public class VisionCameraAngleTest extends LinearOpMode { private RobotVisionAngle robotVisionAngle; From e7dbd99fa452f46d4597aabad904def0c96f3bc0 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 2 Dec 2024 19:24:02 +0800 Subject: [PATCH 57/93] Feat:Fit the robot --- .../ftc/teamcode/drive/SampleMecanumDrive.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java index fcd18aed3cc9..06033ef909cb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java @@ -93,16 +93,16 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); } - // TODO: adjust the names of the following hardware devices to match your configuration + // adjust the names of the following hardware devices to match your configuration imu = hardwareMap.get(IMU.class, "imu"); IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR)); imu.initialize(parameters); - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront = hardwareMap.get(DcMotorEx.class, "FL"); + leftRear = hardwareMap.get(DcMotorEx.class, "BL"); + rightRear = hardwareMap.get(DcMotorEx.class, "BR"); + rightFront = hardwareMap.get(DcMotorEx.class, "FR"); motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront); @@ -122,7 +122,7 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); } - // TODO: reverse any motors using DcMotor.setDirection() + // reverse any motors using DcMotor.setDirection() List lastTrackingEncPositions = new ArrayList<>(); List lastTrackingEncVels = new ArrayList<>(); From 51ed8e0fee67bce27f1ce71cbd12908376a4433c Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 2 Dec 2024 20:20:03 +0800 Subject: [PATCH 58/93] Feat:Fit the robot --- .../ftc/teamcode/drive/DriveConstants.java | 2 +- .../firstinspires/ftc/teamcode/test/RRTest.java | 14 +++++++++++--- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java index 4681679a2103..6670b9339909 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java @@ -32,7 +32,7 @@ public class DriveConstants { * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients * from DriveVelocityPIDTuner. */ - public static final boolean RUN_USING_ENCODER = false; + public static final boolean RUN_USING_ENCODER = true; public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0, getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java index c533275262ed..10be6cff9434 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -24,15 +24,23 @@ public void runOpMode() throws InterruptedException { // 构建轨迹序列 TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(startPose) - .splineTo(new Vector2d(24.67, -33.13), Math.toRadians(87.72)) - .splineTo(new Vector2d(51.86, -26.28), Math.toRadians(14.14)) - .splineTo(new Vector2d(57.10, -48.64), Math.toRadians(-76.82)) + .splineTo(new Vector2d(26.79, 64.23), Math.toRadians(182.66)) + .splineTo(new Vector2d(16.43, 44.71), Math.toRadians(242.05)) + .splineTo(new Vector2d(45.31, 35.15), Math.toRadians(-18.32)) + .splineTo(new Vector2d(46.90, 62.44), Math.toRadians(-33.69)) .build(); + // 等待开始命令 waitForStart(); // 开始跟踪轨迹 if (opModeIsActive()) { drive.followTrajectorySequence(trajectory0); + + while (opModeIsActive()) { + drive.update(); + telemetry.addData("Current Pose", drive.getPoseEstimate()); + telemetry.update(); + } } } } From 8bb0bdcb1c3df30477cb217e65a2a54bd484c8a5 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Mon, 2 Dec 2024 21:15:10 +0800 Subject: [PATCH 59/93] add the distance detection --- .../hardware/RobotCameraDistance.java | 115 ++++++++++++++++++ .../test/VisionCameraDistanceTest.java | 34 ++++++ 2 files changed, 149 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotCameraDistance.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotCameraDistance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotCameraDistance.java new file mode 100644 index 000000000000..b86c7281581e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotCameraDistance.java @@ -0,0 +1,115 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvPipeline; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; + +import java.util.ArrayList; +import java.util.List; + +public class RobotCameraDistance { + private OpenCvCamera webcam; + private String detectedColor; + private double[] polarCoordinates = new double[2]; // 数组形式，0为距离，1为角度 + private Mat hsv = new Mat(); + private Mat mask = new Mat(); + private Mat hierarchy = new Mat(); + private Scalar lowerBound; + private Scalar upperBound; + + public void initialize(HardwareMap hardwareMap, String color) { + this.detectedColor = color; + setColorBounds(color); + + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + webcam.setPipeline(new ColorDetectionPipeline()); + + webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + // 处理摄像头打开错误 + } + }); + } + + public double[] getPolarCoordinates() { + return polarCoordinates; + } + + private void setColorBounds(String color) { + switch (color.toLowerCase()) { + case "red": + lowerBound = new Scalar(0, 100, 100); + upperBound = new Scalar(10, 255, 255); + break; + case "yellow": + lowerBound = new Scalar(20, 100, 100); + upperBound = new Scalar(30, 255, 255); + break; + case "blue": + lowerBound = new Scalar(110, 100, 100); + upperBound = new Scalar(130, 255, 255); + break; + default: + throw new IllegalArgumentException("Unsupported color: " + color); + } + } + + private class ColorDetectionPipeline extends OpenCvPipeline { + @Override + public Mat processFrame(Mat input) { + // 转换为HSV颜色空间 + Imgproc.cvtColor(input, hsv, Imgproc.COLOR_RGB2HSV); + + // 创建颜色掩码 + Core.inRange(hsv, lowerBound, upperBound, mask); + + // 查找轮廓 + List contours = new ArrayList<>(); + Imgproc.findContours(mask, contours, hierarchy, Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); + + // 查找最大矩形 + double maxArea = 0; + Rect largestRect = null; + for (MatOfPoint contour : contours) { + Rect rect = Imgproc.boundingRect(contour); + double area = rect.area(); + if (area > maxArea) { + maxArea = area; + largestRect = rect; + } + } + + // 计算距离和角度 + if (largestRect != null) { + Point rectCenter = new Point(largestRect.x + largestRect.width / 2.0, largestRect.y + largestRect.height / 2.0); + Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); + double distance = Math.sqrt(Math.pow(rectCenter.x - imageCenter.x, 2) + Math.pow(rectCenter.y - imageCenter.y, 2)); + double angle = Math.toDegrees(Math.atan2(rectCenter.y - imageCenter.y, rectCenter.x - imageCenter.x)); + + polarCoordinates[0] = distance; + polarCoordinates[1] = angle; + } + + return input; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java new file mode 100644 index 000000000000..d02a2bd1ac50 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.test;//测试摄像头距离和角度识别功能 + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.hardware.RobotCameraDistance; + +@TeleOp(name = "VisionCameraDistanceTest", group = "Test") +public class VisionCameraDistanceTest extends LinearOpMode { + private RobotCameraDistance robotCameraDistance; + + @Override + public void runOpMode() { + robotCameraDistance = new RobotCameraDistance(); + + robotCameraDistance.initialize(hardwareMap, "red"); // 初始化摄像头，并设置颜色参数为红色 + + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + // 获取极坐标 + double[] polarCoordinates = robotCameraDistance.getPolarCoordinates(); + + // 显示距离和角度检测结果 + telemetry.addData("Distance from Center", polarCoordinates[0]); + telemetry.addData("Angle from Center", polarCoordinates[1]); + telemetry.update(); + + sleep(50); + } + } +} From 06047e35840d675f14c2414fa0ff1d1f6014be3d Mon Sep 17 00:00:00 2001 From: Ehicy Date: Mon, 2 Dec 2024 21:15:41 +0800 Subject: [PATCH 60/93] add the distance detection --- .../teamcode/hardware/RobotVisionAngle.java | 78 ++++++++++++------- 1 file changed, 50 insertions(+), 28 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java index aeb36f6e46ee..760a61328754 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java @@ -1,10 +1,13 @@ package org.firstinspires.ftc.teamcode.hardware; import com.qualcomm.robotcore.hardware.HardwareMap; - import org.opencv.core.Mat; import org.opencv.core.Rect; +import org.opencv.core.Scalar; import org.opencv.core.Size; +import org.opencv.core.MatOfPoint; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; import org.opencv.imgproc.Imgproc; import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; @@ -12,9 +15,12 @@ import org.openftc.easyopencv.OpenCvPipeline; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import java.util.ArrayList; +import java.util.List; + public class RobotVisionAngle { private OpenCvCamera webcam; - private double detectedAngle = 0; // 角度变量 + private double detectedAngle = 0; // 角度变量 public void initialize(HardwareMap hardwareMap) { int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( @@ -43,7 +49,8 @@ private class CenterAnglePipeline extends OpenCvPipeline { Mat gray = new Mat(); Mat blurred = new Mat(); Mat edges = new Mat(); - Mat lines = new Mat(); + Mat hierarchy = new Mat(); + MatOfPoint2f approxCurve = new MatOfPoint2f(); @Override public Mat processFrame(Mat input) { @@ -53,36 +60,51 @@ public Mat processFrame(Mat input) { // 高斯模糊，减少噪声 Imgproc.GaussianBlur(gray, blurred, new Size(5, 5), 0); - // 获取图像中心区域 (3/4的画面) - Rect centerRect = new Rect(input.width() / 8, input.height() / 8, input.width() * 3 / 4, input.height() * 3 / 4); - Mat centerMat = blurred.submat(centerRect); - // 边缘检测 - Imgproc.Canny(centerMat, edges, 50, 150); - - // 霍夫直线变换检测线条 - Imgproc.HoughLinesP(edges, lines, 1, Math.PI / 180, 50, 50, 10); - - double angle = 0; - if (lines.rows() > 0) { - for (int i = 0; i < lines.rows(); i++) { - double[] line = lines.get(i, 0); - double dx = line[2] - line[0]; - double dy = line[3] - line[1]; - angle = Math.toDegrees(Math.atan2(dy, dx)); - - // 调整角度方向：向左为正，向右为负 - if (angle < 0) { - angle += 360; - } - if (angle > 180) { - angle -= 360; + Imgproc.Canny(blurred, edges, 50, 150); + + // 轮廓检测 + List contours = new ArrayList<>(); + Imgproc.findContours(edges, contours, hierarchy, Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); + + // 查找最大矩形 + double maxArea = 0; + Rect largestRect = null; + for (MatOfPoint contour : contours) { + MatOfPoint2f contour2f = new MatOfPoint2f(contour.toArray()); + double peri = Imgproc.arcLength(contour2f, true); + Imgproc.approxPolyDP(contour2f, approxCurve, 0.02 * peri, true); + + if (approxCurve.total() == 4) { + Rect rect = Imgproc.boundingRect(contour); + double area = rect.area(); + if (area > maxArea) { + maxArea = area; + largestRect = rect; } - break; // 考虑简单起见只取第一条检测到的线条 } } - detectedAngle = angle; + // 计算偏角 + if (largestRect != null) { + Point rectCenter = new Point(largestRect.x + largestRect.width / 2.0, largestRect.y + largestRect.height / 2.0); + Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); + double angle = Math.toDegrees(Math.atan2(rectCenter.y - imageCenter.y, rectCenter.x - imageCenter.x)); + + // 调整角度方向：竖直为0度，向左为正，向右为负 + if (angle < -90) { + angle = 90; + } else if (angle > 90) { + angle = -90; + } + + // 四舍五入到最近的15度区间 + int roundedAngle = (int) Math.round(angle / 15.0) * 15; + if (roundedAngle == -90) { + roundedAngle = 90; + } + detectedAngle = roundedAngle; + } return input; } From 9f812936d3ad96ac8e041f0b7e72751075bd6d2d Mon Sep 17 00:00:00 2001 From: Ehicy Date: Tue, 3 Dec 2024 19:20:05 +0800 Subject: [PATCH 61/93] add the new function --- .../hardware/RobotCameraDistance.java | 28 +++++-- .../teamcode/hardware/RobotVisionAngle.java | 74 ++++++------------- .../test/VisionCameraDistanceTest.java | 2 +- 3 files changed, 45 insertions(+), 59 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotCameraDistance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotCameraDistance.java index b86c7281581e..166a8223f866 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotCameraDistance.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotCameraDistance.java @@ -7,7 +7,6 @@ import org.opencv.core.Point; import org.opencv.core.Rect; import org.opencv.core.Scalar; -import org.opencv.core.Size; import org.opencv.imgproc.Imgproc; import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; @@ -19,6 +18,15 @@ import java.util.List; public class RobotCameraDistance { + + // 常量定义 + private static final Scalar LOWER_BOUND_RED = new Scalar(0, 100, 100); + private static final Scalar UPPER_BOUND_RED = new Scalar(10, 255, 255); + private static final Scalar LOWER_BOUND_YELLOW = new Scalar(20, 100, 100); + private static final Scalar UPPER_BOUND_YELLOW = new Scalar(30, 255, 255); + private static final Scalar LOWER_BOUND_BLUE = new Scalar(110, 100, 100); + private static final Scalar UPPER_BOUND_BLUE = new Scalar(130, 255, 255); + private OpenCvCamera webcam; private String detectedColor; private double[] polarCoordinates = new double[2]; // 数组形式，0为距离，1为角度 @@ -57,16 +65,16 @@ public double[] getPolarCoordinates() { private void setColorBounds(String color) { switch (color.toLowerCase()) { case "red": - lowerBound = new Scalar(0, 100, 100); - upperBound = new Scalar(10, 255, 255); + lowerBound = LOWER_BOUND_RED; + upperBound = UPPER_BOUND_RED; break; case "yellow": - lowerBound = new Scalar(20, 100, 100); - upperBound = new Scalar(30, 255, 255); + lowerBound = LOWER_BOUND_YELLOW; + upperBound = UPPER_BOUND_YELLOW; break; case "blue": - lowerBound = new Scalar(110, 100, 100); - upperBound = new Scalar(130, 255, 255); + lowerBound = LOWER_BOUND_BLUE; + upperBound = UPPER_BOUND_BLUE; break; default: throw new IllegalArgumentException("Unsupported color: " + color); @@ -107,6 +115,12 @@ public Mat processFrame(Mat input) { polarCoordinates[0] = distance; polarCoordinates[1] = angle; + + // 在图像上绘制最大矩形和中心点 + Imgproc.rectangle(input, largestRect, new Scalar(0, 255, 0), 2); + Imgproc.circle(input, rectCenter, 5, new Scalar(0, 255, 0), -1); + Imgproc.putText(input, "Distance: " + distance, new Point(10, 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); + Imgproc.putText(input, "Angle: " + angle, new Point(10, 60), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); } return input; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java index 760a61328754..746082974f3c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java @@ -1,13 +1,11 @@ package org.firstinspires.ftc.teamcode.hardware; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.opencv.core.Core; import org.opencv.core.Mat; import org.opencv.core.Rect; import org.opencv.core.Scalar; import org.opencv.core.Size; -import org.opencv.core.MatOfPoint; -import org.opencv.core.MatOfPoint2f; -import org.opencv.core.Point; import org.opencv.imgproc.Imgproc; import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; @@ -15,9 +13,6 @@ import org.openftc.easyopencv.OpenCvPipeline; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import java.util.ArrayList; -import java.util.List; - public class RobotVisionAngle { private OpenCvCamera webcam; private double detectedAngle = 0; // 角度变量 @@ -47,66 +42,43 @@ public double getDetectedAngle() { private class CenterAnglePipeline extends OpenCvPipeline { Mat gray = new Mat(); - Mat blurred = new Mat(); Mat edges = new Mat(); - Mat hierarchy = new Mat(); - MatOfPoint2f approxCurve = new MatOfPoint2f(); + Mat lines = new Mat(); @Override public Mat processFrame(Mat input) { // 转换为灰度图像 Imgproc.cvtColor(input, gray, Imgproc.COLOR_RGB2GRAY); - // 高斯模糊，减少噪声 - Imgproc.GaussianBlur(gray, blurred, new Size(5, 5), 0); + // 获取图像中心区域 (3/4的画面) + Rect centerRect = new Rect(input.width() / 8, input.height() / 8, input.width() * 3 / 4, input.height() * 3 / 4); + Mat centerMat = gray.submat(centerRect); // 边缘检测 - Imgproc.Canny(blurred, edges, 50, 150); - - // 轮廓检测 - List contours = new ArrayList<>(); - Imgproc.findContours(edges, contours, hierarchy, Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); - - // 查找最大矩形 - double maxArea = 0; - Rect largestRect = null; - for (MatOfPoint contour : contours) { - MatOfPoint2f contour2f = new MatOfPoint2f(contour.toArray()); - double peri = Imgproc.arcLength(contour2f, true); - Imgproc.approxPolyDP(contour2f, approxCurve, 0.02 * peri, true); - - if (approxCurve.total() == 4) { - Rect rect = Imgproc.boundingRect(contour); - double area = rect.area(); - if (area > maxArea) { - maxArea = area; - largestRect = rect; + Imgproc.Canny(centerMat, edges, 50, 150); + + // 霍夫直线变换检测线条 + Imgproc.HoughLinesP(edges, lines, 1, Math.PI / 180, 50, 50, 10); + + double angle = 0; + if (lines.rows() > 0) { + for (int i = 0; i < lines.rows(); i++) { + double[] line = lines.get(i, 0); + double dx = line[2] - line[0]; + double dy = line[3] - line[1]; + angle = Math.toDegrees(Math.atan2(dy, dx)); + if (angle < 0) { + angle += 180; } + angle = Math.abs(angle - 90); + break; // 考虑简单起见只取第一条检测到的线条 } } - // 计算偏角 - if (largestRect != null) { - Point rectCenter = new Point(largestRect.x + largestRect.width / 2.0, largestRect.y + largestRect.height / 2.0); - Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); - double angle = Math.toDegrees(Math.atan2(rectCenter.y - imageCenter.y, rectCenter.x - imageCenter.x)); - - // 调整角度方向：竖直为0度，向左为正，向右为负 - if (angle < -90) { - angle = 90; - } else if (angle > 90) { - angle = -90; - } - - // 四舍五入到最近的15度区间 - int roundedAngle = (int) Math.round(angle / 15.0) * 15; - if (roundedAngle == -90) { - roundedAngle = 90; - } - detectedAngle = roundedAngle; - } + detectedAngle = angle; return input; } } } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java index d02a2bd1ac50..2293159716f1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java @@ -12,7 +12,7 @@ public class VisionCameraDistanceTest extends LinearOpMode { public void runOpMode() { robotCameraDistance = new RobotCameraDistance(); - robotCameraDistance.initialize(hardwareMap, "red"); // 初始化摄像头，并设置颜色参数为红色 + robotCameraDistance.initialize(hardwareMap, "blue"); // 初始化摄像头，并设置颜色参数为红色 telemetry.addData("Status", "Initialized"); telemetry.update(); From 2359d29d71ec21d8a526079273cbd1ff5afa4c33 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Tue, 3 Dec 2024 19:35:07 +0800 Subject: [PATCH 62/93] Feat: manual op seemly completed --- .../ftc/teamcode/ManualOpMode.java | 45 ++++++++---- .../ftc/teamcode/hardware/RobotTop.java | 2 + .../ftc/teamcode/test/ServoTest2.java | 68 +++++++++++++++++++ 3 files changed, 103 insertions(+), 12 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest2.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 43fd246f2c63..1a05b07d1d75 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -13,7 +13,12 @@ public class ManualOpMode extends LinearOpMode { RobotTop robotTop; // servoName(port): positionRange[a, b]; defaultPos + // control hub: + // top(3): + // container(4): [0.35-open, 1-close]; default: 1 + // lift(5): [0.2-open, 0.6-close];default: 0.6 // -------------------------------------------- + // expansion hub: // stretch(1): [0-back, 0.3-out]; default: 0 // spinX(2): [0, 1]; default: 0.5 // turn(3): [0.38-back, 0.71-out]; default: 0.38 @@ -22,14 +27,21 @@ public class ManualOpMode extends LinearOpMode { final int LIFT_TOP_POSITION = 1250; final int LIFT_BOTTOM_POSITION = 50; - final double STRETCH_BACK_POSITION = 0; + final double STRETCH_BACK_POSITION = 0.03; final double STRETCH_OUT_POSITION = 0.3; final double SPIN_X_DEFAULT_POSITION = 0.5; - final double SPIN_Y_DEFAULT_POSITION = 0.07; + //TODO: test spinX hovering position + final double SPIN_X_HOVERING_POSITION = 0.53; + //TODO: test the spinX down position + final double SPIN_X_DOWN_POSITION = 0.58; + final double SPIN_Y_DEFAULT_POSITION = 0.1; final double TURN_BACK_POSITION = 0.38; - final double TURN_OUT_POSITION = 0.71; + //TODO: test the hovering position + final double TURN_OUT_HOVERING_POSITION = 0.64; + //TODO: test the grabbing down position + final double TURN_OUT_DOWN_POSITION = 0.71; final double GRAB_OPEN_POSITION = 0.2; - final double GRAB_CLOSE_POSITION = 0.83; + final double GRAB_CLOSE_POSITION = 0.9; @Override public void runOpMode() { @@ -99,9 +111,10 @@ public void runOpMode() { } } if (armState == ArmState.TURNING_OUT) { - if (armTurnPos >= TURN_OUT_POSITION - 0.05) { - armTurnPos = TURN_OUT_POSITION; + if (armTurnPos >= TURN_OUT_HOVERING_POSITION - 0.05) { + armTurnPos = TURN_OUT_HOVERING_POSITION; robotTop.setArmTurnPosition(armTurnPos); + robotTop.setArmSpinXPosition(SPIN_X_HOVERING_POSITION); armState = ArmState.TURNED; } else { armTurnPos += 0.03; @@ -119,19 +132,27 @@ public void runOpMode() { } } if (gamepad1.b && !previousGamepad1.b) { - if (armGrabbing) { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + sleep(500); + if (!armGrabbing) { + robotTop.setArmTurnPosition(TURN_OUT_DOWN_POSITION); + robotTop.setArmSpinXPosition(SPIN_X_DOWN_POSITION); + sleep(200); robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); - armGrabbing = false; - } else { - robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + sleep(500); + robotTop.setArmTurnPosition(TURN_OUT_HOVERING_POSITION); armGrabbing = true; + telemetry.addData("grab", 1); + } else { + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + armGrabbing = false; } } if (gamepad1.dpad_up) { - armSpinXPos = Math.min(1, armSpinXPos + 0.05); + armSpinXPos = Math.min(1, armSpinXPos + 0.02); robotTop.setArmSpinXPosition(armSpinXPos); } else if (gamepad1.dpad_down) { - armSpinXPos = Math.max(0, armSpinXPos - 0.05); + armSpinXPos = Math.max(0, armSpinXPos - 0.02); robotTop.setArmSpinXPosition(armSpinXPos); } if (gamepad1.dpad_right) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java index d872c008f049..4a9cf83b80aa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java @@ -21,6 +21,7 @@ public class RobotTop { private Servo armSpinXServo; private Servo armSpinYServo; private Servo armGrabServo; + private Servo liftServo; public RobotTop(LinearOpMode opMode) { this.opMode = opMode; @@ -37,6 +38,7 @@ public void init() { armSpinXServo = hardwareMap.get(Servo.class, "armSpinX"); armSpinYServo = hardwareMap.get(Servo.class, "armSpinY"); armGrabServo = hardwareMap.get(Servo.class, "armGrab"); + liftServo = hardwareMap.get(Servo.class, "liftServo"); leftLiftMotor.setDirection(DcMotor.Direction.FORWARD); rightLiftMotor.setDirection(DcMotor.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest2.java new file mode 100644 index 000000000000..bf5e67b81871 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest2.java @@ -0,0 +1,68 @@ +package org.firstinspires.ftc.teamcode.test; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +@TeleOp +public class ServoTest2 extends LinearOpMode { + @Override + public void runOpMode(){ + Servo armStretchServo = hardwareMap.get(Servo.class, "armStretch"); + Servo armTurnServo = hardwareMap.get(Servo.class, "armTurn"); + Servo armSpinXServo = hardwareMap.get(Servo.class, "armSpinX"); + Servo armSpinYServo = hardwareMap.get(Servo.class, "armSpinY"); + Servo armGrabServo = hardwareMap.get(Servo.class, "armGrab"); + Servo liftServo = hardwareMap.get(Servo.class, "liftServo"); + Servo containerServo = hardwareMap.get(Servo.class, "containerServo"); + Servo liftTopServo = hardwareMap.get(Servo.class, "liftTop"); + Servo[] servos = {armStretchServo, armTurnServo, armSpinXServo, armSpinYServo, armGrabServo, liftServo, containerServo, liftTopServo}; + String[] names = {"stretch", "turn", "spinX", "spinY", "grab","lift","container","top"}; + //stretch(1): [0, 0.3] + //turn(3): [0.38-back, 0.71-out] + //grab(5): [0-open, 0.53-close] + //spinY(4): default: 0.07 + //spinX(2): default: 0.5 + + waitForStart(); + int index = 0; + boolean activate = false; + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + double servoPos = 0; + Servo servo = servos[index]; + + while (opModeIsActive()){ + if(gamepad1.y){ + servoPos = Math.min(1, servoPos + 0.01); + }if(gamepad1.a){ + servoPos = Math.max(0, servoPos - 0.01); + } + if(gamepad1.b && !previousGamepad1.b){ + index += 1; + index = index % servos.length; + servo = servos[index]; + } + if(gamepad1.x && !previousGamepad1.x){ + activate = !activate; + } + if (activate) { + servo.setPosition(servoPos); + } + + telemetry.addData("Y","add position"); + telemetry.addData("A","minus position"); + telemetry.addData("X","activate"); + telemetry.addData("B","change servo"); + telemetry.addData("--------","-------"); + telemetry.addData("pos", servoPos); + telemetry.addData("servo", names[index]); + telemetry.addData("activated",activate); + telemetry.update(); + + previousGamepad1.copy(gamepad1); + sleep(100); + } + } +} From 2dbc37899de0d1ecc2f8511440e0d1123a6702ae Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Tue, 3 Dec 2024 21:00:17 +0800 Subject: [PATCH 63/93] Feat:Add Sparkfun device --- .../ftc/teamcode/test/RRTest.java | 31 +++++++++----- .../ftc/teamcode/test/SparkFunOTOS.java | 42 +++++++++++++++++++ build.dependencies.gradle | 1 + 3 files changed, 63 insertions(+), 11 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOS.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java index 10be6cff9434..b9eb4592648c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -2,43 +2,52 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; - +import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.test.SparkFunOTOS; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder; - -@Autonomous (name = "RRTest") +@Autonomous(name = "SparkFunRoadRunnerAuto") public class RRTest extends LinearOpMode { + private SparkFunOTOS sparkFunOTOS; + @Override public void runOpMode() throws InterruptedException { + // 初始化SparkFun OTOS传感器 + sparkFunOTOS = new SparkFunOTOS(hardwareMap); + // 初始化驱动对象 SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); // 定义机器人起始位置 - Pose2d startPose = new Pose2d(23.26, -68.58, Math.toRadians(90.00)); + Pose2d startPose = new Pose2d(0, 0, Math.toRadians(90.0)); drive.setPoseEstimate(startPose); // 构建轨迹序列 - TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(startPose) - .splineTo(new Vector2d(26.79, 64.23), Math.toRadians(182.66)) - .splineTo(new Vector2d(16.43, 44.71), Math.toRadians(242.05)) - .splineTo(new Vector2d(45.31, 35.15), Math.toRadians(-18.32)) - .splineTo(new Vector2d(46.90, 62.44), Math.toRadians(-33.69)) + TrajectorySequence trajectory = drive.trajectorySequenceBuilder(startPose) + .splineTo(new Vector2d(30, 30), Math.toRadians(45.0)) + .splineTo(new Vector2d(60, 0), Math.toRadians(0.0)) .build(); // 等待开始命令 waitForStart(); + // 开始跟踪轨迹 if (opModeIsActive()) { - drive.followTrajectorySequence(trajectory0); + drive.followTrajectorySequence(trajectory); while (opModeIsActive()) { + // 更新位置信息 drive.update(); + + // 输出当前位置信息到Telemetry telemetry.addData("Current Pose", drive.getPoseEstimate()); + telemetry.addData("SparkFun OTOS X", sparkFunOTOS.getX()); + telemetry.addData("SparkFun OTOS Y", sparkFunOTOS.getY()); + telemetry.addData("SparkFun OTOS Heading", sparkFunOTOS.getHeading()); telemetry.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOS.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOS.java new file mode 100644 index 000000000000..81d276b2a1dc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOS.java @@ -0,0 +1,42 @@ +package org.firstinspires.ftc.teamcode.test; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.I2cDevice; + +public class SparkFunOTOS { + private I2cDevice otosDevice; + private double x; + private double y; + private double heading; + + public SparkFunOTOS(HardwareMap hardwareMap) { + // 假设 OTOS 传感器连接到 I2C 端口 + otosDevice = hardwareMap.get(I2cDevice.class, "otos"); + // 初始化传感器（具体初始化代码取决于 OTOS 库的实现） + this.x = 0.0; + this.y = 0.0; + this.heading = 0.0; + } + + public double getX() { + // 返回 X 坐标 + return x; // 这里应从传感器获取实际数据 + } + + public double getY() { + // 返回 Y 坐标 + return y; // 这里应从传感器获取实际数据 + } + + public double getHeading() { + // 返回方向（角度） + return heading; // 这里应从传感器获取实际数据 + } + + public void updateData(double newX, double newY, double newHeading) { + // 更新传感器数据 + this.x = newX; + this.y = newY; + this.heading = newHeading; + } +} diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 1ec9869b3ed6..6ac3f4615a21 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -15,6 +15,7 @@ dependencies { implementation 'org.firstinspires.ftc:Vision:10.1.0' implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'com.acmerobotics.roadrunner:core:0.5.2' + implementation 'com.sparkfun:otos:1.0.0' implementation('com.acmerobotics.dashboard:dashboard:0.4.16') { exclude group: 'org.firstinspires.ftc' } From 7fab92823e48f43cd64d4b6603d18459cca793a3 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Tue, 3 Dec 2024 21:25:48 +0800 Subject: [PATCH 64/93] add the new function --- .../teamcode/hardware/RobotVisionAngle.java | 46 +++++++++++++++---- 1 file changed, 37 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java index 746082974f3c..963bdc2045a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java @@ -1,16 +1,17 @@ package org.firstinspires.ftc.teamcode.hardware; import com.qualcomm.robotcore.hardware.HardwareMap; -import org.opencv.core.Core; +import org.opencv.core.CvType; import org.opencv.core.Mat; import org.opencv.core.Rect; -import org.opencv.core.Scalar; import org.opencv.core.Size; +import org.opencv.core.Point; +import org.opencv.core.Scalar; import org.opencv.imgproc.Imgproc; import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; import org.openftc.easyopencv.OpenCvPipeline; +import org.openftc.easyopencv.OpenCvCameraRotation; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; public class RobotVisionAngle { @@ -42,21 +43,29 @@ public double getDetectedAngle() { private class CenterAnglePipeline extends OpenCvPipeline { Mat gray = new Mat(); + Mat blurred = new Mat(); Mat edges = new Mat(); Mat lines = new Mat(); + Mat sharp = new Mat(); + Mat kernel = new Mat(3, 3, CvType.CV_16SC1); @Override public Mat processFrame(Mat input) { // 转换为灰度图像 Imgproc.cvtColor(input, gray, Imgproc.COLOR_RGB2GRAY); - + // 高斯模糊，减少噪声 + Imgproc.GaussianBlur(gray, blurred, new Size(5, 5), 0); + // 锐化内核 + kernel.put(0, 0, 0, -1, 0); + kernel.put(1, 0, -1, 5, -1); + kernel.put(2, 0, 0, -1, 0); + // 锐化处理 + Imgproc.filter2D(blurred, sharp, gray.depth(), kernel); // 获取图像中心区域 (3/4的画面) Rect centerRect = new Rect(input.width() / 8, input.height() / 8, input.width() * 3 / 4, input.height() * 3 / 4); - Mat centerMat = gray.submat(centerRect); - + Mat centerMat = sharp.submat(centerRect); // 边缘检测 Imgproc.Canny(centerMat, edges, 50, 150); - // 霍夫直线变换检测线条 Imgproc.HoughLinesP(edges, lines, 1, Math.PI / 180, 50, 50, 10); @@ -75,10 +84,29 @@ public Mat processFrame(Mat input) { } } - detectedAngle = angle; + // 四舍五入到最接近的10位数 + detectedAngle = Math.round(angle / 10.0) * 10; + + // 获取图像中心点和矩形中心点 + Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); + Point rectCenter = new Point(centerRect.x + centerRect.width / 2.0, centerRect.y + centerRect.height / 2.0); + + // 计算距离和角度 + double distance = Math.sqrt(Math.pow(rectCenter.x - imageCenter.x, 2) + Math.pow(rectCenter.y - imageCenter.y, 2)); + double rectAngle = Math.toDegrees(Math.atan2(rectCenter.y - imageCenter.y, rectCenter.x - imageCenter.x)); + + // 将角度存储到polarCoordinates中 + double[] polarCoordinates = new double[2]; + polarCoordinates[0] = distance; + polarCoordinates[1] = rectAngle; + + // 在图像上绘制最大矩形和中心点 + Imgproc.rectangle(input, centerRect, new Scalar(0, 255, 0), 2); + Imgproc.circle(input, rectCenter, 5, new Scalar(0, 255, 0), -1); + Imgproc.putText(input, "Distance: " + distance, new Point(10, 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); + Imgproc.putText(input, "Angle: " + rectAngle, new Point(10, 60), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); return input; } } } - From 4ab9ba45b39ba3ef9014b3c50c576b986a87c95b Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Wed, 4 Dec 2024 19:16:44 +0800 Subject: [PATCH 65/93] Feat:Fixed Sync problems. --- build.dependencies.gradle | 2 +- build.gradle | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 6ac3f4615a21..72a1d3e2fe11 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -15,7 +15,7 @@ dependencies { implementation 'org.firstinspires.ftc:Vision:10.1.0' implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'com.acmerobotics.roadrunner:core:0.5.2' - implementation 'com.sparkfun:otos:1.0.0' +// implementation 'com.sparkfun:otos:1.0.0' implementation('com.acmerobotics.dashboard:dashboard:0.4.16') { exclude group: 'org.firstinspires.ftc' } diff --git a/build.gradle b/build.gradle index 4303020f15eb..9337c71c59d9 100644 --- a/build.gradle +++ b/build.gradle @@ -32,3 +32,11 @@ repositories { // maven { url "https://maven.aliyun.com/repository/central" } mavenCentral() } + +repositories { + mavenCentral() + // 你可以添加其他需要的仓库，例如： + maven { url "https://jitpack.io" } + maven { url "https://repo.sparkfun.com/artifactory/sparkfun-repo" } // 假设这是SparkFun的仓库URL +} + From f30cf00af58f247d832965b8db47df053763e9ac Mon Sep 17 00:00:00 2001 From: Ehicy Date: Wed, 4 Dec 2024 19:30:59 +0800 Subject: [PATCH 66/93] add the new function --- .../ftc/teamcode/test/VisionCameraAngleTest.java | 2 +- .../ftc/teamcode/test/VisionCameraColorTest.java | 6 +++--- .../ftc/teamcode/test/VisionCameraDistanceTest.java | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java index c8d5a701fb29..cad1a4ba3e5b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java @@ -2,7 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.hardware.RobotVisionAngle; +import org.firstinspires.ftc.teamcode.hardware.RobotVision.RobotVisionAngle; @TeleOp diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraColorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraColorTest.java index bede089d1077..db56f168d3ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraColorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraColorTest.java @@ -1,15 +1,15 @@ package org.firstinspires.ftc.teamcode.test;//测试摄像头颜色识别功能 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.hardware.RobotVision; +import org.firstinspires.ftc.teamcode.hardware.RobotVision.RobotVisionColor; @TeleOp public class VisionCameraColorTest extends LinearOpMode { - private RobotVision robotVisionColor; + private RobotVisionColor robotVisionColor; @Override public void runOpMode() { - robotVisionColor = new RobotVision(); + robotVisionColor = new RobotVisionColor(); robotVisionColor.initialize(hardwareMap); // 初始化摄像头 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java index 2293159716f1..20007ecc4774 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java @@ -2,7 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.hardware.RobotCameraDistance; +import org.firstinspires.ftc.teamcode.hardware.RobotVision.RobotCameraDistance; @TeleOp(name = "VisionCameraDistanceTest", group = "Test") public class VisionCameraDistanceTest extends LinearOpMode { From c6c0831e3495affa02eca9732677b49535fc1b4e Mon Sep 17 00:00:00 2001 From: Ehicy Date: Wed, 4 Dec 2024 19:31:17 +0800 Subject: [PATCH 67/93] add the new function --- .../hardware/RobotCameraDistance.java | 129 ------------------ .../ftc/teamcode/hardware/RobotVision.java | 90 ------------ .../teamcode/hardware/RobotVisionAngle.java | 112 --------------- 3 files changed, 331 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotCameraDistance.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotCameraDistance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotCameraDistance.java deleted file mode 100644 index 166a8223f866..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotCameraDistance.java +++ /dev/null @@ -1,129 +0,0 @@ -package org.firstinspires.ftc.teamcode.hardware; - -import com.qualcomm.robotcore.hardware.HardwareMap; -import org.opencv.core.Core; -import org.opencv.core.Mat; -import org.opencv.core.MatOfPoint; -import org.opencv.core.Point; -import org.opencv.core.Rect; -import org.opencv.core.Scalar; -import org.opencv.imgproc.Imgproc; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; -import org.openftc.easyopencv.OpenCvPipeline; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; - -import java.util.ArrayList; -import java.util.List; - -public class RobotCameraDistance { - - // 常量定义 - private static final Scalar LOWER_BOUND_RED = new Scalar(0, 100, 100); - private static final Scalar UPPER_BOUND_RED = new Scalar(10, 255, 255); - private static final Scalar LOWER_BOUND_YELLOW = new Scalar(20, 100, 100); - private static final Scalar UPPER_BOUND_YELLOW = new Scalar(30, 255, 255); - private static final Scalar LOWER_BOUND_BLUE = new Scalar(110, 100, 100); - private static final Scalar UPPER_BOUND_BLUE = new Scalar(130, 255, 255); - - private OpenCvCamera webcam; - private String detectedColor; - private double[] polarCoordinates = new double[2]; // 数组形式，0为距离，1为角度 - private Mat hsv = new Mat(); - private Mat mask = new Mat(); - private Mat hierarchy = new Mat(); - private Scalar lowerBound; - private Scalar upperBound; - - public void initialize(HardwareMap hardwareMap, String color) { - this.detectedColor = color; - setColorBounds(color); - - int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( - "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); - webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); - webcam.setPipeline(new ColorDetectionPipeline()); - - webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - @Override - public void onOpened() { - webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); - } - - @Override - public void onError(int errorCode) { - // 处理摄像头打开错误 - } - }); - } - - public double[] getPolarCoordinates() { - return polarCoordinates; - } - - private void setColorBounds(String color) { - switch (color.toLowerCase()) { - case "red": - lowerBound = LOWER_BOUND_RED; - upperBound = UPPER_BOUND_RED; - break; - case "yellow": - lowerBound = LOWER_BOUND_YELLOW; - upperBound = UPPER_BOUND_YELLOW; - break; - case "blue": - lowerBound = LOWER_BOUND_BLUE; - upperBound = UPPER_BOUND_BLUE; - break; - default: - throw new IllegalArgumentException("Unsupported color: " + color); - } - } - - private class ColorDetectionPipeline extends OpenCvPipeline { - @Override - public Mat processFrame(Mat input) { - // 转换为HSV颜色空间 - Imgproc.cvtColor(input, hsv, Imgproc.COLOR_RGB2HSV); - - // 创建颜色掩码 - Core.inRange(hsv, lowerBound, upperBound, mask); - - // 查找轮廓 - List contours = new ArrayList<>(); - Imgproc.findContours(mask, contours, hierarchy, Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); - - // 查找最大矩形 - double maxArea = 0; - Rect largestRect = null; - for (MatOfPoint contour : contours) { - Rect rect = Imgproc.boundingRect(contour); - double area = rect.area(); - if (area > maxArea) { - maxArea = area; - largestRect = rect; - } - } - - // 计算距离和角度 - if (largestRect != null) { - Point rectCenter = new Point(largestRect.x + largestRect.width / 2.0, largestRect.y + largestRect.height / 2.0); - Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); - double distance = Math.sqrt(Math.pow(rectCenter.x - imageCenter.x, 2) + Math.pow(rectCenter.y - imageCenter.y, 2)); - double angle = Math.toDegrees(Math.atan2(rectCenter.y - imageCenter.y, rectCenter.x - imageCenter.x)); - - polarCoordinates[0] = distance; - polarCoordinates[1] = angle; - - // 在图像上绘制最大矩形和中心点 - Imgproc.rectangle(input, largestRect, new Scalar(0, 255, 0), 2); - Imgproc.circle(input, rectCenter, 5, new Scalar(0, 255, 0), -1); - Imgproc.putText(input, "Distance: " + distance, new Point(10, 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); - Imgproc.putText(input, "Angle: " + angle, new Point(10, 60), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); - } - - return input; - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java deleted file mode 100644 index 283bab7f45de..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision.java +++ /dev/null @@ -1,90 +0,0 @@ -//这是第二版视觉识别,可以识别摄像头中心的颜色 -//robotVision.getDetectedColor()返回的是一个字符串然后这个字符串是四个类型Unknown,Red，Blue，Yellow - - -package org.firstinspires.ftc.teamcode.hardware; - -import com.qualcomm.robotcore.hardware.HardwareMap; -import org.opencv.core.Core; -import org.opencv.core.Mat; -import org.opencv.core.Rect; -import org.opencv.core.Scalar; -import org.opencv.imgproc.Imgproc; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; -import org.openftc.easyopencv.OpenCvPipeline; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; - -public class RobotVision { - private OpenCvCamera webcam; - private String detectedColor = "Unknown"; - - public void initialize(HardwareMap hardwareMap) { - int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( - "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); - webcam = OpenCvCameraFactory - .getInstance() - .createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); - webcam.setPipeline(new CenterColorPipeline()); - - webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - @Override - public void onOpened() { - webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); - } - - @Override - public void onError(int errorCode) { - // 处理摄像头打开错误 - } - }); - } - - public String getDetectedColor() { - return detectedColor; - } - - private class CenterColorPipeline extends OpenCvPipeline { - Mat hsv = new Mat(); - Mat maskRed = new Mat(); - Mat maskBlue = new Mat(); - Mat maskYellow = new Mat(); // 增加黄色检测的mask - Scalar lowerRed = new Scalar(0, 100, 100); - Scalar upperRed = new Scalar(10, 255, 255); - Scalar lowerBlue = new Scalar(110, 100, 100); - Scalar upperBlue = new Scalar(130, 255, 255); - Scalar lowerYellow = new Scalar(20, 100, 100); // 黄色的HSV范围 - Scalar upperYellow = new Scalar(30, 255, 255); - - @Override - public Mat processFrame(Mat input) { - Imgproc.cvtColor(input, hsv, Imgproc.COLOR_RGB2HSV); - - // 获取图像中心区域 - Rect centerRect = new Rect(input.width() / 2 - 20, input.height() / 2 - 20, 40, 40); - Mat centerMat = hsv.submat(centerRect); - - // 检测红色、蓝色和黄色 - Core.inRange(centerMat, lowerRed, upperRed, maskRed); - Core.inRange(centerMat, lowerBlue, upperBlue, maskBlue); - Core.inRange(centerMat, lowerYellow, upperYellow, maskYellow); - - double redValue = Core.mean(maskRed).val[0]; - double blueValue = Core.mean(maskBlue).val[0]; - double yellowValue = Core.mean(maskYellow).val[0]; - - if (redValue > blueValue && redValue > yellowValue) { - detectedColor = "Red"; - } else if (blueValue > redValue && blueValue > yellowValue) { - detectedColor = "Blue"; - } else if (yellowValue > redValue && yellowValue > blueValue) { - detectedColor = "Yellow"; - } else { - detectedColor = "Unknown"; - } - - return input; - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java deleted file mode 100644 index 963bdc2045a9..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVisionAngle.java +++ /dev/null @@ -1,112 +0,0 @@ -package org.firstinspires.ftc.teamcode.hardware; - -import com.qualcomm.robotcore.hardware.HardwareMap; -import org.opencv.core.CvType; -import org.opencv.core.Mat; -import org.opencv.core.Rect; -import org.opencv.core.Size; -import org.opencv.core.Point; -import org.opencv.core.Scalar; -import org.opencv.imgproc.Imgproc; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvPipeline; -import org.openftc.easyopencv.OpenCvCameraRotation; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; - -public class RobotVisionAngle { - private OpenCvCamera webcam; - private double detectedAngle = 0; // 角度变量 - - public void initialize(HardwareMap hardwareMap) { - int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( - "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); - webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); - webcam.setPipeline(new CenterAnglePipeline()); - - webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - @Override - public void onOpened() { - webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); - } - - @Override - public void onError(int errorCode) { - // 处理摄像头打开错误 - } - }); - } - - public double getDetectedAngle() { - return detectedAngle; - } - - private class CenterAnglePipeline extends OpenCvPipeline { - Mat gray = new Mat(); - Mat blurred = new Mat(); - Mat edges = new Mat(); - Mat lines = new Mat(); - Mat sharp = new Mat(); - Mat kernel = new Mat(3, 3, CvType.CV_16SC1); - - @Override - public Mat processFrame(Mat input) { - // 转换为灰度图像 - Imgproc.cvtColor(input, gray, Imgproc.COLOR_RGB2GRAY); - // 高斯模糊，减少噪声 - Imgproc.GaussianBlur(gray, blurred, new Size(5, 5), 0); - // 锐化内核 - kernel.put(0, 0, 0, -1, 0); - kernel.put(1, 0, -1, 5, -1); - kernel.put(2, 0, 0, -1, 0); - // 锐化处理 - Imgproc.filter2D(blurred, sharp, gray.depth(), kernel); - // 获取图像中心区域 (3/4的画面) - Rect centerRect = new Rect(input.width() / 8, input.height() / 8, input.width() * 3 / 4, input.height() * 3 / 4); - Mat centerMat = sharp.submat(centerRect); - // 边缘检测 - Imgproc.Canny(centerMat, edges, 50, 150); - // 霍夫直线变换检测线条 - Imgproc.HoughLinesP(edges, lines, 1, Math.PI / 180, 50, 50, 10); - - double angle = 0; - if (lines.rows() > 0) { - for (int i = 0; i < lines.rows(); i++) { - double[] line = lines.get(i, 0); - double dx = line[2] - line[0]; - double dy = line[3] - line[1]; - angle = Math.toDegrees(Math.atan2(dy, dx)); - if (angle < 0) { - angle += 180; - } - angle = Math.abs(angle - 90); - break; // 考虑简单起见只取第一条检测到的线条 - } - } - - // 四舍五入到最接近的10位数 - detectedAngle = Math.round(angle / 10.0) * 10; - - // 获取图像中心点和矩形中心点 - Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); - Point rectCenter = new Point(centerRect.x + centerRect.width / 2.0, centerRect.y + centerRect.height / 2.0); - - // 计算距离和角度 - double distance = Math.sqrt(Math.pow(rectCenter.x - imageCenter.x, 2) + Math.pow(rectCenter.y - imageCenter.y, 2)); - double rectAngle = Math.toDegrees(Math.atan2(rectCenter.y - imageCenter.y, rectCenter.x - imageCenter.x)); - - // 将角度存储到polarCoordinates中 - double[] polarCoordinates = new double[2]; - polarCoordinates[0] = distance; - polarCoordinates[1] = rectAngle; - - // 在图像上绘制最大矩形和中心点 - Imgproc.rectangle(input, centerRect, new Scalar(0, 255, 0), 2); - Imgproc.circle(input, rectCenter, 5, new Scalar(0, 255, 0), -1); - Imgproc.putText(input, "Distance: " + distance, new Point(10, 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); - Imgproc.putText(input, "Angle: " + rectAngle, new Point(10, 60), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); - - return input; - } - } -} From 88f039e844f51feb914db54fcb8439ffbd9ee9ac Mon Sep 17 00:00:00 2001 From: Ehicy Date: Wed, 4 Dec 2024 19:32:36 +0800 Subject: [PATCH 68/93] add the new function --- .../RobotVision/RobotCameraDistance.java | 175 ++++++++++++++++++ .../RobotVision/RobotVisionAngle.java | 112 +++++++++++ .../RobotVision/RobotVisionColor.java | 90 +++++++++ 3 files changed, 377 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionColor.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java new file mode 100644 index 000000000000..cdaa5ed2dc4c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java @@ -0,0 +1,175 @@ +package org.firstinspires.ftc.teamcode.hardware.RobotVision; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.RotatedRect; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvPipeline; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; + +import java.util.ArrayList; +import java.util.List; + +public class RobotCameraDistance { + + // 定义用于颜色检测的HSV颜色范围常量 + private static final Scalar LOWER_BOUND_RED = new Scalar(0, 100, 100); + private static final Scalar UPPER_BOUND_RED = new Scalar(10, 255, 255); + private static final Scalar LOWER_BOUND_YELLOW = new Scalar(20, 100, 100); + private static final Scalar UPPER_BOUND_YELLOW = new Scalar(30, 255, 255); + private static final Scalar LOWER_BOUND_BLUE = new Scalar(110, 100, 100); + private static final Scalar UPPER_BOUND_BLUE = new Scalar(130, 255, 255); + + private OpenCvCamera webcam; // 用于摄像头操作的OpenCvCamera对象 + private String detectedColor; // 用于存储检测到的颜色 + private double[] polarCoordinates = new double[2]; // 数组形式，0为距离，1为角度 + private Mat hsv = new Mat(); // 用于存储HSV颜色空间图像 + private Mat mask = new Mat(); // 用于存储颜色掩码 + private Mat hierarchy = new Mat(); // 用于存储轮廓层级信息 + private Mat sharpened = new Mat(); // 用于存储锐化后的图像 + private Scalar lowerBound; // 用于存储颜色下界 + private Scalar upperBound; // 用于存储颜色上界 + + /** + * 初始化摄像头并设置颜色检测管道。 + * + * @param hardwareMap 硬件映射对象 + * @param color 要检测的颜色 + */ + public void initialize(HardwareMap hardwareMap, String color) { + this.detectedColor = color; + setColorBounds(color); // 设置颜色范围 + + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + webcam.setPipeline(new ColorDetectionPipeline()); // 设置颜色检测管道 + + webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + // 开始视频流，设置分辨率和旋转角度 + webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + // 处理摄像头打开错误 + } + }); + } + + /** + * 获取极坐标信息，包括距离和角度。 + * + * @return 极坐标数组，0为距离，1为角度 + */ + public double[] getPolarCoordinates() { + return polarCoordinates; + } + + /** + * 根据颜色名称设置颜色范围。 + * + * @param color 颜色名称 + */ + private void setColorBounds(String color) { + switch (color.toLowerCase()) { + case "red": + lowerBound = LOWER_BOUND_RED; + upperBound = UPPER_BOUND_RED; + break; + case "yellow": + lowerBound = LOWER_BOUND_YELLOW; + upperBound = UPPER_BOUND_YELLOW; + break; + case "blue": + lowerBound = LOWER_BOUND_BLUE; + upperBound = UPPER_BOUND_BLUE; + break; + default: + throw new IllegalArgumentException("Unsupported color: " + color); + } + } + + private class ColorDetectionPipeline extends OpenCvPipeline { + @Override + public Mat processFrame(Mat input) { + // 复制输入图像用于锐化处理 + Mat src = input.clone(); + Mat blurImg = new Mat(); + + // 应用高斯模糊减少噪声 + Imgproc.GaussianBlur(src, blurImg, new Size(0, 0), 25); + + // 创建锐化效果 + Core.addWeighted(src, 1.5, blurImg, -0.5, 0, sharpened); + + // 将图像转换为HSV颜色空间 + Imgproc.cvtColor(sharpened, hsv, Imgproc.COLOR_RGB2HSV); + + // 根据颜色范围创建掩码 + Core.inRange(hsv, lowerBound, upperBound, mask); + + // 查找图像中的轮廓 + List contours = new ArrayList<>(); + Imgproc.findContours(mask, contours, hierarchy, Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); + + // 查找最大矩形 + double maxArea = 0; + RotatedRect largestRotatedRect = null; + for (MatOfPoint contour : contours) { + // 将轮廓转换为2f点集 + MatOfPoint2f contour2f = new MatOfPoint2f(contour.toArray()); + // 查找最小面积的包围矩形 + RotatedRect rotatedRect = Imgproc.minAreaRect(contour2f); + double area = rotatedRect.size.area(); + if (area > maxArea) { + maxArea = area; + largestRotatedRect = rotatedRect; + } + } + + // 计算距离和角度 + if (largestRotatedRect != null) { + Point rectCenter = largestRotatedRect.center; + Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); + double distance = Math.sqrt(Math.pow(rectCenter.x - imageCenter.x, 2) + Math.pow(rectCenter.y - imageCenter.y, 2)); + double angle = Math.toDegrees(Math.atan2(rectCenter.y - imageCenter.y, rectCenter.x - imageCenter.x)); + + // 使角度范围在0-360度之间 + if (angle < 0) { + angle += 360; + } + + polarCoordinates[0] = distance; + polarCoordinates[1] = angle; + + // 绘制最大旋转矩形 + Point[] vertices = new Point[4]; + largestRotatedRect.points(vertices); + for (int i = 0; i < 4; i++) { + Imgproc.line(input, vertices[i], vertices[(i + 1) % 4], new Scalar(0, 255, 0), 2); + } + + // 在图像上绘制最大矩形和中心点 + Imgproc.circle(input, rectCenter, 5, new Scalar(0, 255, 0), -1); + Imgproc.putText(input, "Distance: " + distance, new Point(10, 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); + Imgproc.putText(input, "Angle: " + angle, new Point(10, 60), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); + } + + return input; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java new file mode 100644 index 000000000000..c8153cac38d0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java @@ -0,0 +1,112 @@ +package org.firstinspires.ftc.teamcode.hardware.RobotVision; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.Rect; +import org.opencv.core.Size; +import org.opencv.core.Point; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvPipeline; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; + +public class RobotVisionAngle { + private OpenCvCamera webcam; + private double detectedAngle = 0; // 角度变量 + + public void initialize(HardwareMap hardwareMap) { + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + webcam.setPipeline(new CenterAnglePipeline()); + + webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + // 处理摄像头打开错误 + } + }); + } + + public double getDetectedAngle() { + return detectedAngle; + } + + private class CenterAnglePipeline extends OpenCvPipeline { + Mat gray = new Mat(); + Mat blurred = new Mat(); + Mat edges = new Mat(); + Mat lines = new Mat(); + Mat sharp = new Mat(); + Mat kernel = new Mat(3, 3, CvType.CV_16SC1); + + @Override + public Mat processFrame(Mat input) { + // 转换为灰度图像 + Imgproc.cvtColor(input, gray, Imgproc.COLOR_RGB2GRAY); + // 高斯模糊，减少噪声 + Imgproc.GaussianBlur(gray, blurred, new Size(5, 5), 0); + // 锐化内核 + kernel.put(0, 0, 0, -1, 0); + kernel.put(1, 0, -1, 5, -1); + kernel.put(2, 0, 0, -1, 0); + // 锐化处理 + Imgproc.filter2D(blurred, sharp, gray.depth(), kernel); + // 获取图像中心区域 (3/4的画面) + Rect centerRect = new Rect(input.width() / 8, input.height() / 8, input.width() * 3 / 4, input.height() * 3 / 4); + Mat centerMat = sharp.submat(centerRect); + // 边缘检测 + Imgproc.Canny(centerMat, edges, 50, 150); + // 霍夫直线变换检测线条 + Imgproc.HoughLinesP(edges, lines, 1, Math.PI / 180, 50, 50, 10); + + double angle = 0; + if (lines.rows() > 0) { + for (int i = 0; i < lines.rows(); i++) { + double[] line = lines.get(i, 0); + double dx = line[2] - line[0]; + double dy = line[3] - line[1]; + angle = Math.toDegrees(Math.atan2(dy, dx)); + if (angle < 0) { + angle += 180; + } + angle = Math.abs(angle - 90); + break; // 考虑简单起见只取第一条检测到的线条 + } + } + + // 四舍五入到最接近的10位数 + detectedAngle = Math.round(angle / 10.0) * 10; + + // 获取图像中心点和矩形中心点 + Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); + Point rectCenter = new Point(centerRect.x + centerRect.width / 2.0, centerRect.y + centerRect.height / 2.0); + + // 计算距离和角度 + double distance = Math.sqrt(Math.pow(rectCenter.x - imageCenter.x, 2) + Math.pow(rectCenter.y - imageCenter.y, 2)); + double rectAngle = Math.toDegrees(Math.atan2(rectCenter.y - imageCenter.y, rectCenter.x - imageCenter.x)); + + // 将角度存储到polarCoordinates中 + double[] polarCoordinates = new double[2]; + polarCoordinates[0] = distance; + polarCoordinates[1] = rectAngle; + + // 在图像上绘制最大矩形和中心点 + Imgproc.rectangle(input, centerRect, new Scalar(0, 255, 0), 2); + Imgproc.circle(input, rectCenter, 5, new Scalar(0, 255, 0), -1); + Imgproc.putText(input, "Distance: " + distance, new Point(10, 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); + Imgproc.putText(input, "Angle: " + rectAngle, new Point(10, 60), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); + + return input; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionColor.java new file mode 100644 index 000000000000..e5341c15b09f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionColor.java @@ -0,0 +1,90 @@ +//这是第二版视觉识别,可以识别摄像头中心的颜色 +//robotVision.getDetectedColor()返回的是一个字符串然后这个字符串是四个类型Unknown,Red，Blue，Yellow + + +package org.firstinspires.ftc.teamcode.hardware.RobotVision; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvPipeline; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; + +public class RobotVisionColor { + private OpenCvCamera webcam; + private String detectedColor = "Unknown"; + + public void initialize(HardwareMap hardwareMap) { + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( + "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam = OpenCvCameraFactory + .getInstance() + .createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + webcam.setPipeline(new CenterColorPipeline()); + + webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + // 处理摄像头打开错误 + } + }); + } + + public String getDetectedColor() { + return detectedColor; + } + + private class CenterColorPipeline extends OpenCvPipeline { + Mat hsv = new Mat(); + Mat maskRed = new Mat(); + Mat maskBlue = new Mat(); + Mat maskYellow = new Mat(); // 增加黄色检测的mask + Scalar lowerRed = new Scalar(0, 100, 100); + Scalar upperRed = new Scalar(10, 255, 255); + Scalar lowerBlue = new Scalar(110, 100, 100); + Scalar upperBlue = new Scalar(130, 255, 255); + Scalar lowerYellow = new Scalar(20, 100, 100); // 黄色的HSV范围 + Scalar upperYellow = new Scalar(30, 255, 255); + + @Override + public Mat processFrame(Mat input) { + Imgproc.cvtColor(input, hsv, Imgproc.COLOR_RGB2HSV); + + // 获取图像中心区域 + Rect centerRect = new Rect(input.width() / 2 - 20, input.height() / 2 - 20, 40, 40); + Mat centerMat = hsv.submat(centerRect); + + // 检测红色、蓝色和黄色 + Core.inRange(centerMat, lowerRed, upperRed, maskRed); + Core.inRange(centerMat, lowerBlue, upperBlue, maskBlue); + Core.inRange(centerMat, lowerYellow, upperYellow, maskYellow); + + double redValue = Core.mean(maskRed).val[0]; + double blueValue = Core.mean(maskBlue).val[0]; + double yellowValue = Core.mean(maskYellow).val[0]; + + if (redValue > blueValue && redValue > yellowValue) { + detectedColor = "Red"; + } else if (blueValue > redValue && blueValue > yellowValue) { + detectedColor = "Blue"; + } else if (yellowValue > redValue && yellowValue > blueValue) { + detectedColor = "Yellow"; + } else { + detectedColor = "Unknown"; + } + + return input; + } + } +} From 2334c40e48dd802c5eec22cb6c76ef5791b67c4d Mon Sep 17 00:00:00 2001 From: Ehicy Date: Wed, 4 Dec 2024 19:34:01 +0800 Subject: [PATCH 69/93] add the new function --- .../ftc/teamcode/test/ColorTest.java | 34 ------------------- 1 file changed, 34 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ColorTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ColorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ColorTest.java deleted file mode 100644 index 6c28a5bc371b..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ColorTest.java +++ /dev/null @@ -1,34 +0,0 @@ -//测试摄像头颜色识别功能 -package org.firstinspires.ftc.teamcode.test; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import org.firstinspires.ftc.teamcode.hardware.RobotVision; - -@TeleOp - -public class ColorTest extends LinearOpMode { - private RobotVision robotVision; - - @Override - public void runOpMode() { - robotVision = new RobotVision(); - - robotVision.initialize(hardwareMap); // 初始化摄像头 - - telemetry.addData("Status", "Initialized"); - telemetry.update(); - - waitForStart(); - - while (opModeIsActive()) { - // 显示颜色检测结果 - telemetry.addData("Center Color", robotVision.getDetectedColor()); - telemetry.update(); - - sleep(100); - } - } -} - - From f17672a0b953c60cc706464fd9742a1f5ae1f499 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Wed, 4 Dec 2024 20:07:32 +0800 Subject: [PATCH 70/93] Feat:Fixed RR problems.The next step is to make the stability --- .../ftc/teamcode/drive/DriveConstants.java | 24 +++++++++---------- ...parkFunOTOS.java => SparkFunOTOSTest.java} | 9 +++---- 2 files changed, 17 insertions(+), 16 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/{SparkFunOTOS.java => SparkFunOTOSTest.java} (80%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java index 6670b9339909..7db3e42f2ee7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java @@ -21,8 +21,8 @@ public class DriveConstants { /* * These are motor constants that should be listed online for your motors. */ - public static final double TICKS_PER_REV = 1; - public static final double MAX_RPM = 1; + public static final double TICKS_PER_REV = 537.6; + public static final double MAX_RPM = 312.5; /* * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. @@ -32,7 +32,7 @@ public class DriveConstants { * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients * from DriveVelocityPIDTuner. */ - public static final boolean RUN_USING_ENCODER = true; + public static final boolean RUN_USING_ENCODER = false; public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0, getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV)); @@ -44,12 +44,11 @@ public class DriveConstants { * angular distances although most angular parameters are wrapped in Math.toRadians() for * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ - public static double WHEEL_RADIUS = 2; // in - public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 1; // in + public static double WHEEL_RADIUS = 3; // in (轮子半径 单位：英寸) + public static double GEAR_RATIO = 1.0/15.0; // (齿轮比) output (wheel) speed / input (motor) speed + public static double TRACK_WIDTH = 32/2.54; // (轮距 单位：英寸) in - /* - * These are the feedforward parameters used to model the drive motor behavior. If you are using + /* * These are the feedforward parameters used to model the drive motor behavior. If you are using * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive * motor encoders or have elected not to use them for velocity control, these values should be * empirically tuned. @@ -65,10 +64,11 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VEL = 30; - public static double MAX_ACCEL = 30; - public static double MAX_ANG_VEL = Math.toRadians(60); - public static double MAX_ANG_ACCEL = Math.toRadians(60); + //TODO:ADJUST THESE NUMBERS. + public static double MAX_VEL = 392.699075; + public static double MAX_ACCEL = 392.699075; + public static double MAX_ANG_VEL = Math.toRadians(31.170489078125); + public static double MAX_ANG_ACCEL = Math.toRadians(31.170489078125); /* * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOS.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOSTest.java similarity index 80% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOS.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOSTest.java index 81d276b2a1dc..90a4bc074dad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOS.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOSTest.java @@ -1,17 +1,18 @@ package org.firstinspires.ftc.teamcode.test; +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.I2cDevice; -public class SparkFunOTOS { - private I2cDevice otosDevice; +public class SparkFunOTOSTest { + private SparkFunOTOS otosDevice; private double x; private double y; private double heading; - public SparkFunOTOS(HardwareMap hardwareMap) { + public SparkFunOTOSTest(HardwareMap hardwareMap) { // 假设 OTOS 传感器连接到 I2C 端口 - otosDevice = hardwareMap.get(I2cDevice.class, "otos"); + otosDevice = hardwareMap.get(SparkFunOTOS.class, "otos"); // 初始化传感器（具体初始化代码取决于 OTOS 库的实现） this.x = 0.0; this.y = 0.0; From 924ea89c14a3c443aab7e509e9290b2c3a79f4a2 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Wed, 4 Dec 2024 20:07:38 +0800 Subject: [PATCH 71/93] Feat:Fixed RR problems.The next step is to make the stability --- .../ftc/teamcode/test/RRTest.java | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java index b9eb4592648c..fc1329799bbb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -2,22 +2,21 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.test.SparkFunOTOS; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; -@Autonomous(name = "SparkFunRoadRunnerAuto") +@Autonomous(name = "RRTest") + public class RRTest extends LinearOpMode { - private SparkFunOTOS sparkFunOTOS; + private SparkFunOTOSTest sparkFunOTOS; @Override public void runOpMode() throws InterruptedException { // 初始化SparkFun OTOS传感器 - sparkFunOTOS = new SparkFunOTOS(hardwareMap); + sparkFunOTOS = new SparkFunOTOSTest(hardwareMap); // 初始化驱动对象 SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); @@ -27,9 +26,13 @@ public void runOpMode() throws InterruptedException { drive.setPoseEstimate(startPose); // 构建轨迹序列 - TrajectorySequence trajectory = drive.trajectorySequenceBuilder(startPose) - .splineTo(new Vector2d(30, 30), Math.toRadians(45.0)) - .splineTo(new Vector2d(60, 0), Math.toRadians(0.0)) + TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(new Pose2d(43.40, 49.04, Math.toRadians(90.00))) + .splineTo(new Vector2d(47.83, -32.93), Math.toRadians(-86.91)) + .splineTo(new Vector2d(18.43, -53.47), Math.toRadians(214.94)) + .splineTo(new Vector2d(-48.23, -41.99), Math.toRadians(170.23)) + .splineTo(new Vector2d(-46.22, 3.52), Math.toRadians(87.47)) + .splineTo(new Vector2d(-35.55, 47.03), Math.toRadians(76.21)) + .splineTo(new Vector2d(11.98, 52.67), Math.toRadians(6.77)) .build(); // 等待开始命令 @@ -37,7 +40,7 @@ public void runOpMode() throws InterruptedException { // 开始跟踪轨迹 if (opModeIsActive()) { - drive.followTrajectorySequence(trajectory); + drive.followTrajectorySequence(trajectory0); while (opModeIsActive()) { // 更新位置信息 From 4e67253657e2198e33e11e4c6919433c0d8b1091 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Wed, 4 Dec 2024 20:08:24 +0800 Subject: [PATCH 72/93] Feat:Fixed RR problems.The next step is to make the stability --- .../SparkFunOTOS.java} | 12 +++++------- .../org/firstinspires/ftc/teamcode/test/RRTest.java | 5 +++-- 2 files changed, 8 insertions(+), 9 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{test/SparkFunOTOSTest.java => hardware/SparkFunOTOS.java} (73%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOSTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOS.java similarity index 73% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOSTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOS.java index 90a4bc074dad..fced04f2e6b6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/SparkFunOTOSTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOS.java @@ -1,18 +1,16 @@ -package org.firstinspires.ftc.teamcode.test; +package org.firstinspires.ftc.teamcode.hardware; -import com.qualcomm.hardware.sparkfun.SparkFunOTOS; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.I2cDevice; -public class SparkFunOTOSTest { - private SparkFunOTOS otosDevice; +public class SparkFunOTOS { + private com.qualcomm.hardware.sparkfun.SparkFunOTOS otosDevice; private double x; private double y; private double heading; - public SparkFunOTOSTest(HardwareMap hardwareMap) { + public SparkFunOTOS(HardwareMap hardwareMap) { // 假设 OTOS 传感器连接到 I2C 端口 - otosDevice = hardwareMap.get(SparkFunOTOS.class, "otos"); + otosDevice = hardwareMap.get(com.qualcomm.hardware.sparkfun.SparkFunOTOS.class, "otos"); // 初始化传感器（具体初始化代码取决于 OTOS 库的实现） this.x = 0.0; this.y = 0.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java index fc1329799bbb..c1e94b05a4fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -5,18 +5,19 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.hardware.SparkFunOTOS; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; @Autonomous(name = "RRTest") public class RRTest extends LinearOpMode { - private SparkFunOTOSTest sparkFunOTOS; + private SparkFunOTOS sparkFunOTOS; @Override public void runOpMode() throws InterruptedException { // 初始化SparkFun OTOS传感器 - sparkFunOTOS = new SparkFunOTOSTest(hardwareMap); + sparkFunOTOS = new SparkFunOTOS(hardwareMap); // 初始化驱动对象 SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); From 994b6c1faef01bc4022c3b86bc0a9ee7c91dda39 Mon Sep 17 00:00:00 2001 From: Ehicy <102399008+Ehicy@users.noreply.github.com> Date: Wed, 4 Dec 2024 21:05:50 +0800 Subject: [PATCH 73/93] Update RobotCameraDistance.java fuck the android studio and the google --- .../RobotVision/RobotCameraDistance.java | 54 ++++++++++--------- 1 file changed, 29 insertions(+), 25 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java index cdaa5ed2dc4c..00f48d65c6fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java @@ -22,7 +22,6 @@ import java.util.List; public class RobotCameraDistance { - // 定义用于颜色检测的HSV颜色范围常量 private static final Scalar LOWER_BOUND_RED = new Scalar(0, 100, 100); private static final Scalar UPPER_BOUND_RED = new Scalar(10, 255, 255); @@ -31,15 +30,17 @@ public class RobotCameraDistance { private static final Scalar LOWER_BOUND_BLUE = new Scalar(110, 100, 100); private static final Scalar UPPER_BOUND_BLUE = new Scalar(130, 255, 255); - private OpenCvCamera webcam; // 用于摄像头操作的OpenCvCamera对象 - private String detectedColor; // 用于存储检测到的颜色 - private double[] polarCoordinates = new double[2]; // 数组形式，0为距离，1为角度 - private Mat hsv = new Mat(); // 用于存储HSV颜色空间图像 - private Mat mask = new Mat(); // 用于存储颜色掩码 - private Mat hierarchy = new Mat(); // 用于存储轮廓层级信息 - private Mat sharpened = new Mat(); // 用于存储锐化后的图像 - private Scalar lowerBound; // 用于存储颜色下界 - private Scalar upperBound; // 用于存储颜色上界 + private OpenCvCamera webcam; // 用于摄像头操作的OpenCvCamera对象 + private String detectedColor; // 用于存储检测到的颜色 + private double[] polarCoordinates = new double[2]; // 数组形式，0为距离，1为角度 + private Mat hsv = new Mat(); // 用于存储HSV颜色空间图像 + private Mat mask = new Mat(); // 用于存储颜色掩码 + private Mat hierarchy = new Mat(); // 用于存储轮廓层级信息 + private Mat sharpened = new Mat(); // 用于存储锐化后的图像 + private Scalar lowerBound; // 用于存储颜色下界 + private Scalar upperBound; // 用于存储颜色上界 + + private RotatedRect currentRect; // 当前锁定的矩形 /** * 初始化摄像头并设置颜色检测管道。 @@ -49,13 +50,11 @@ public class RobotCameraDistance { */ public void initialize(HardwareMap hardwareMap, String color) { this.detectedColor = color; - setColorBounds(color); // 设置颜色范围 - + setColorBounds(color); // 设置颜色范围 int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); - webcam.setPipeline(new ColorDetectionPipeline()); // 设置颜色检测管道 - + webcam.setPipeline(new ColorDetectionPipeline()); // 设置颜色检测管道 webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { @Override public void onOpened() { @@ -109,19 +108,14 @@ public Mat processFrame(Mat input) { // 复制输入图像用于锐化处理 Mat src = input.clone(); Mat blurImg = new Mat(); - // 应用高斯模糊减少噪声 Imgproc.GaussianBlur(src, blurImg, new Size(0, 0), 25); - // 创建锐化效果 Core.addWeighted(src, 1.5, blurImg, -0.5, 0, sharpened); - // 将图像转换为HSV颜色空间 Imgproc.cvtColor(sharpened, hsv, Imgproc.COLOR_RGB2HSV); - // 根据颜色范围创建掩码 Core.inRange(hsv, lowerBound, upperBound, mask); - // 查找图像中的轮廓 List contours = new ArrayList<>(); Imgproc.findContours(mask, contours, hierarchy, Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); @@ -143,22 +137,24 @@ public Mat processFrame(Mat input) { // 计算距离和角度 if (largestRotatedRect != null) { - Point rectCenter = largestRotatedRect.center; + // 如果当前锁定的矩形存在且变化超过阈值，则更新锁定 + if (currentRect == null || !isSameRect(currentRect, largestRotatedRect)) { + currentRect = largestRotatedRect; + } + // 更新极坐标计算基于currentRect + Point rectCenter = currentRect.center; Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); double distance = Math.sqrt(Math.pow(rectCenter.x - imageCenter.x, 2) + Math.pow(rectCenter.y - imageCenter.y, 2)); double angle = Math.toDegrees(Math.atan2(rectCenter.y - imageCenter.y, rectCenter.x - imageCenter.x)); - - // 使角度范围在0-360度之间 if (angle < 0) { angle += 360; } - polarCoordinates[0] = distance; polarCoordinates[1] = angle; // 绘制最大旋转矩形 Point[] vertices = new Point[4]; - largestRotatedRect.points(vertices); + currentRect.points(vertices); for (int i = 0; i < 4; i++) { Imgproc.line(input, vertices[i], vertices[(i + 1) % 4], new Scalar(0, 255, 0), 2); } @@ -168,8 +164,16 @@ public Mat processFrame(Mat input) { Imgproc.putText(input, "Distance: " + distance, new Point(10, 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); Imgproc.putText(input, "Angle: " + angle, new Point(10, 60), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); } - return input; } } + + private boolean isSameRect(RotatedRect rect1, RotatedRect rect2) { + // 检查两个矩形的中心点是否相近以及面积是否相似 + double distanceThreshold = 10.0; // 位置变化阈值 + double areaThreshold = 0.1; // 面积变化百分比阈值 + double centerDistance = Math.sqrt(Math.pow(rect1.center.x - rect2.center.x, 2) + Math.pow(rect1.center.y - rect2.center.y, 2)); + double areaDifference = Math.abs(rect1.size.area() - rect2.size.area()) / rect1.size.area(); + return centerDistance < distanceThreshold && areaDifference < areaThreshold; + } } From 6c61ad96dc1d56ef2fdb0f6ad79489eab284160e Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Wed, 4 Dec 2024 21:11:14 +0800 Subject: [PATCH 74/93] Feat:Refactored the roadrunner. --- .../EmptySequenceException.java | 4 ++ .../TrajectorySequence.java | 4 +- .../TrajectorySequenceBuilder.java | 10 ++--- .../TrajectorySequenceRunner.java | 16 +++---- .../drive/DriveConstants.java | 12 ++--- .../drive/SampleMecanumDrive.java | 40 ++++++----------- .../drive/SampleTankDrive.java | 44 +++++++------------ .../drive/StandardTrackingWheelLocalizer.java | 4 +- .../opmode/AutomaticFeedforwardTuner.java | 14 +++--- .../drive/opmode/BackAndForth.java | 4 +- .../drive/opmode/DriveVelocityPIDTuner.java | 14 +++--- .../drive/opmode/FollowerPIDTuner.java | 6 +-- .../drive/opmode/LocalizationTest.java | 4 +- .../drive/opmode/ManualFeedforwardTuner.java | 18 ++++---- .../drive/opmode/MaxAngularVeloTuner.java | 4 +- .../drive/opmode/MaxVelocityTuner.java | 6 +-- .../drive/opmode/MotorDirectionDebugger.java | 4 +- .../drive/opmode/SplineTest.java | 4 +- .../drive/opmode/StrafeTest.java | 4 +- .../drive/opmode/StraightTest.java | 4 +- .../drive/opmode/TrackWidthTuner.java | 6 +-- .../TrackingWheelForwardOffsetTuner.java | 6 +-- .../TrackingWheelLateralDistanceTuner.java | 6 +-- .../drive/opmode/TurnTest.java | 4 +- .../sequencesegment/SequenceSegment.java | 2 +- .../sequencesegment/TrajectorySegment.java | 2 +- .../sequencesegment/TurnSegment.java | 2 +- .../sequencesegment/WaitSegment.java | 2 +- .../util/AssetsTrajectoryManager.java | 2 +- .../trajectorysequence}/util/AxesSigns.java | 2 +- .../util/AxisDirection.java | 2 +- .../util/DashboardUtil.java | 2 +- .../trajectorysequence}/util/Encoder.java | 2 +- .../trajectorysequence}/util/LogFiles.java | 10 ++--- .../trajectorysequence}/util/LoggingUtil.java | 2 +- .../util/LynxModuleUtil.java | 2 +- .../util/RegressionUtil.java | 2 +- .../EmptySequenceException.java | 4 -- 38 files changed, 127 insertions(+), 153 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner}/trajectorysequence/TrajectorySequence.java (84%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner}/trajectorysequence/TrajectorySequenceBuilder.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner}/trajectorysequence/TrajectorySequenceRunner.java (93%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/DriveConstants.java (89%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/SampleMecanumDrive.java (86%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/SampleTankDrive.java (85%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/StandardTrackingWheelLocalizer.java (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/AutomaticFeedforwardTuner.java (91%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/BackAndForth.java (92%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/DriveVelocityPIDTuner.java (90%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/FollowerPIDTuner.java (89%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/LocalizationTest.java (90%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/ManualFeedforwardTuner.java (88%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/MaxAngularVeloTuner.java (93%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/MaxVelocityTuner.java (91%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/MotorDirectionDebugger.java (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/SplineTest.java (85%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/StrafeTest.java (89%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/StraightTest.java (89%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/TrackWidthTuner.java (92%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/TrackingWheelForwardOffsetTuner.java (94%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/TrackingWheelLateralDistanceTuner.java (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/drive/opmode/TurnTest.java (77%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner}/trajectorysequence/sequencesegment/SequenceSegment.java (91%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner}/trajectorysequence/sequencesegment/TrajectorySegment.java (87%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner}/trajectorysequence/sequencesegment/TurnSegment.java (92%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner}/trajectorysequence/sequencesegment/WaitSegment.java (79%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/util/AssetsTrajectoryManager.java (96%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/util/AxesSigns.java (93%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/util/AxisDirection.java (63%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/util/DashboardUtil.java (96%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/util/Encoder.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/util/LogFiles.java (96%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/util/LoggingUtil.java (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/util/LynxModuleUtil.java (98%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => RoadRunner/trajectorysequence}/util/RegressionUtil.java (98%) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java new file mode 100644 index 000000000000..d5071362c671 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence; + + +public class EmptySequenceException extends RuntimeException { } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java similarity index 84% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java index 29032e685cc0..64d3df5bd8f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java @@ -1,8 +1,8 @@ -package org.firstinspires.ftc.teamcode.trajectorysequence; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence; import com.acmerobotics.roadrunner.geometry.Pose2d; -import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment; import java.util.Collections; import java.util.List; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java index 8166db30e9b9..0b153bbe7b34 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.trajectorysequence; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; @@ -19,10 +19,10 @@ import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint; import com.acmerobotics.roadrunner.util.Angle; -import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment; -import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TrajectorySegment; -import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TurnSegment; -import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.WaitSegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.TrajectorySegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.TurnSegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.WaitSegment; import java.util.ArrayList; import java.util.Collections; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java index e05af9c5ce4c..bcfa8698a008 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.trajectorysequence; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence; import androidx.annotation.Nullable; @@ -17,13 +17,13 @@ import com.acmerobotics.roadrunner.util.NanoClock; import com.qualcomm.robotcore.hardware.VoltageSensor; -import org.firstinspires.ftc.teamcode.drive.DriveConstants; -import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment; -import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TrajectorySegment; -import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TurnSegment; -import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.WaitSegment; -import org.firstinspires.ftc.teamcode.util.DashboardUtil; -import org.firstinspires.ftc.teamcode.util.LogFiles; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.SequenceSegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.TrajectorySegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.TurnSegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment.WaitSegment; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.DashboardUtil; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.LogFiles; import java.util.ArrayList; import java.util.Collections; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java similarity index 89% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java index 7db3e42f2ee7..4e04f518372a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; @@ -46,7 +46,7 @@ public class DriveConstants { */ public static double WHEEL_RADIUS = 3; // in (轮子半径 单位：英寸) public static double GEAR_RATIO = 1.0/15.0; // (齿轮比) output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 32/2.54; // (轮距 单位：英寸) in + public static double TRACK_WIDTH = 33.9/2.54; // (轮距 单位：英寸) in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive @@ -65,10 +65,10 @@ public class DriveConstants { * inches. */ //TODO:ADJUST THESE NUMBERS. - public static double MAX_VEL = 392.699075; - public static double MAX_ACCEL = 392.699075; - public static double MAX_ANG_VEL = Math.toRadians(31.170489078125); - public static double MAX_ANG_ACCEL = Math.toRadians(31.170489078125); + public static double MAX_VEL = MAX_RPM*GEAR_RATIO*WHEEL_RADIUS*2*Math.PI/60; + public static double MAX_ACCEL = MAX_RPM*GEAR_RATIO*WHEEL_RADIUS*2*Math.PI/60; + public static double MAX_ANG_VEL = Math.toRadians(MAX_VEL/TRACK_WIDTH); + public static double MAX_ANG_ACCEL = Math.toRadians(MAX_VEL/TRACK_WIDTH); /* * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java similarity index 86% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java index 06033ef909cb..4743129ab580 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive; import androidx.annotation.NonNull; @@ -28,27 +28,15 @@ import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner; -import org.firstinspires.ftc.teamcode.util.LynxModuleUtil; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequenceRunner; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.LynxModuleUtil; import java.util.ArrayList; import java.util.Arrays; import java.util.List; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; - /* * Simple mecanum drive hardware implementation for REV hardware. */ @@ -65,8 +53,8 @@ public class SampleMecanumDrive extends MecanumDrive { private TrajectorySequenceRunner trajectorySequenceRunner; - private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH); - private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL); + private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(DriveConstants.MAX_VEL, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH); + private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(DriveConstants.MAX_ACCEL); private TrajectoryFollower follower; @@ -80,7 +68,7 @@ public class SampleMecanumDrive extends MecanumDrive { private List lastEncVels = new ArrayList<>(); public SampleMecanumDrive(HardwareMap hardwareMap) { - super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER); + super(DriveConstants.kV, DriveConstants.kA, DriveConstants.kStatic, DriveConstants.TRACK_WIDTH, DriveConstants.TRACK_WIDTH, LATERAL_MULTIPLIER); follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID, new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5); @@ -112,14 +100,14 @@ public SampleMecanumDrive(HardwareMap hardwareMap) { motor.setMotorType(motorConfigurationType); } - if (RUN_USING_ENCODER) { + if (DriveConstants.RUN_USING_ENCODER) { setMode(DcMotor.RunMode.RUN_USING_ENCODER); } setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) { - setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + if (DriveConstants.RUN_USING_ENCODER && DriveConstants.MOTOR_VELO_PID != null) { + setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID); } // reverse any motors using DcMotor.setDirection() @@ -152,7 +140,7 @@ public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) { return new TrajectorySequenceBuilder( startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT, - MAX_ANG_VEL, MAX_ANG_ACCEL + DriveConstants.MAX_ANG_VEL, DriveConstants.MAX_ANG_ACCEL ); } @@ -262,7 +250,7 @@ public List getWheelPositions() { for (DcMotorEx motor : motors) { int position = motor.getCurrentPosition(); lastEncPositions.add(position); - wheelPositions.add(encoderTicksToInches(position)); + wheelPositions.add(DriveConstants.encoderTicksToInches(position)); } return wheelPositions; } @@ -275,7 +263,7 @@ public List getWheelVelocities() { for (DcMotorEx motor : motors) { int vel = (int) motor.getVelocity(); lastEncVels.add(vel); - wheelVelocities.add(encoderTicksToInches(vel)); + wheelVelocities.add(DriveConstants.encoderTicksToInches(vel)); } return wheelVelocities; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleTankDrive.java similarity index 85% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleTankDrive.java index 55aeb3a52163..a9ee7df54da3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleTankDrive.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive; import androidx.annotation.NonNull; @@ -28,27 +28,15 @@ import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner; -import org.firstinspires.ftc.teamcode.util.LynxModuleUtil; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequenceRunner; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.LynxModuleUtil; import java.util.ArrayList; import java.util.Arrays; import java.util.List; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; - /* * Simple tank drive hardware implementation for REV hardware. */ @@ -63,8 +51,8 @@ public class SampleTankDrive extends TankDrive { private TrajectorySequenceRunner trajectorySequenceRunner; - private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH); - private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(MAX_ACCEL); + private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(DriveConstants.MAX_VEL, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH); + private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(DriveConstants.MAX_ACCEL); private TrajectoryFollower follower; @@ -74,7 +62,7 @@ public class SampleTankDrive extends TankDrive { private VoltageSensor batteryVoltageSensor; public SampleTankDrive(HardwareMap hardwareMap) { - super(kV, kA, kStatic, TRACK_WIDTH); + super(DriveConstants.kV, DriveConstants.kA, DriveConstants.kStatic, DriveConstants.TRACK_WIDTH); follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_PID, new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5); @@ -109,14 +97,14 @@ public SampleTankDrive(HardwareMap hardwareMap) { motor.setMotorType(motorConfigurationType); } - if (RUN_USING_ENCODER) { + if (DriveConstants.RUN_USING_ENCODER) { setMode(DcMotor.RunMode.RUN_USING_ENCODER); } setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) { - setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID); + if (DriveConstants.RUN_USING_ENCODER && DriveConstants.MOTOR_VELO_PID != null) { + setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID); } // TODO: reverse any motors using DcMotor.setDirection() @@ -146,7 +134,7 @@ public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) { return new TrajectorySequenceBuilder( startPose, VEL_CONSTRAINT, accelConstraint, - MAX_ANG_VEL, MAX_ANG_ACCEL + DriveConstants.MAX_ANG_VEL, DriveConstants.MAX_ANG_ACCEL ); } @@ -253,10 +241,10 @@ public void setWeightedDrivePower(Pose2d drivePower) { public List getWheelPositions() { double leftSum = 0, rightSum = 0; for (DcMotorEx leftMotor : leftMotors) { - leftSum += encoderTicksToInches(leftMotor.getCurrentPosition()); + leftSum += DriveConstants.encoderTicksToInches(leftMotor.getCurrentPosition()); } for (DcMotorEx rightMotor : rightMotors) { - rightSum += encoderTicksToInches(rightMotor.getCurrentPosition()); + rightSum += DriveConstants.encoderTicksToInches(rightMotor.getCurrentPosition()); } return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size()); } @@ -264,10 +252,10 @@ public List getWheelPositions() { public List getWheelVelocities() { double leftSum = 0, rightSum = 0; for (DcMotorEx leftMotor : leftMotors) { - leftSum += encoderTicksToInches(leftMotor.getVelocity()); + leftSum += DriveConstants.encoderTicksToInches(leftMotor.getVelocity()); } for (DcMotorEx rightMotor : rightMotors) { - rightSum += encoderTicksToInches(rightMotor.getVelocity()); + rightSum += DriveConstants.encoderTicksToInches(rightMotor.getVelocity()); } return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size()); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/StandardTrackingWheelLocalizer.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/StandardTrackingWheelLocalizer.java index 03cdd6addc11..0b59fd321f5d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/StandardTrackingWheelLocalizer.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive; import androidx.annotation.NonNull; @@ -7,7 +7,7 @@ import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.teamcode.util.Encoder; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.Encoder; import java.util.Arrays; import java.util.List; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java similarity index 91% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java index 8e0c62140fae..198b6bd1ddaa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java @@ -1,8 +1,8 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_RPM; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.rpmToVelocity; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.MAX_RPM; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.rpmToVelocity; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -15,9 +15,9 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.internal.system.Misc; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.util.LoggingUtil; -import org.firstinspires.ftc.teamcode.util.RegressionUtil; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.LoggingUtil; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util.RegressionUtil; import java.util.ArrayList; import java.util.List; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/BackAndForth.java similarity index 92% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/BackAndForth.java index a78ad3796bd7..5571ba48330f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/BackAndForth.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; @@ -6,7 +6,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; /* * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/DriveVelocityPIDTuner.java similarity index 90% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/DriveVelocityPIDTuner.java index 931b742083a9..66abd1a5b4ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/DriveVelocityPIDTuner.java @@ -1,10 +1,10 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.MOTOR_VELO_PID; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.kV; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -20,7 +20,7 @@ import com.qualcomm.robotcore.util.RobotLog; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; import java.util.List; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/FollowerPIDTuner.java similarity index 89% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/FollowerPIDTuner.java index 63f577e16989..c1961726bfd5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/FollowerPIDTuner.java @@ -1,12 +1,12 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence; /* * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/LocalizationTest.java similarity index 90% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/LocalizationTest.java index 8411792bda91..be5aa14ad4cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/LocalizationTest.java @@ -1,11 +1,11 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; /** * This is a simple teleop routine for testing localization. Drive the robot around like a normal diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/ManualFeedforwardTuner.java similarity index 88% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/ManualFeedforwardTuner.java index 0d01bcb96157..9bf139e48b80 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/ManualFeedforwardTuner.java @@ -1,16 +1,15 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic; -import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.MAX_ACCEL; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.MAX_VEL; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.RUN_USING_ENCODER; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.kA; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.kStatic; +import static org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants.kV; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.acmerobotics.roadrunner.drive.DriveSignal; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.kinematics.Kinematics; import com.acmerobotics.roadrunner.profile.MotionProfile; @@ -23,8 +22,7 @@ import com.qualcomm.robotcore.util.RobotLog; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.drive.DriveConstants; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; import java.util.Objects; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxAngularVeloTuner.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxAngularVeloTuner.java index 05d82657803f..54223ab82192 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxAngularVeloTuner.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -10,7 +10,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; import java.util.Objects; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxVelocityTuner.java similarity index 91% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxVelocityTuner.java index ddca6cd1a4c3..ff9211b0c34a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxVelocityTuner.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -11,8 +11,8 @@ import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.drive.DriveConstants; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; import java.util.Objects; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MotorDirectionDebugger.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MotorDirectionDebugger.java index 023cfccc3825..9b73a1de7a75 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MotorDirectionDebugger.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -8,7 +8,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; /** * This is a simple teleop routine for debugging your motor configuration. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/SplineTest.java similarity index 85% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/SplineTest.java index d7316766a18e..e1d36ffa39b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/SplineTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; @@ -6,7 +6,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; /* * This is an example of a more complex path to really test the tuning. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StrafeTest.java similarity index 89% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StrafeTest.java index 992d03aea1db..77df5b71fa4d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StrafeTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -9,7 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; /* * This is a simple routine to test translational drive capabilities. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java similarity index 89% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java index e1b27e1c9a6d..19e6583991ed 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -9,7 +9,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; /* * This is a simple routine to test translational drive capabilities. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackWidthTuner.java similarity index 92% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackWidthTuner.java index ffce0233c67d..d0f301b9596d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackWidthTuner.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -11,8 +11,8 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.internal.system.Misc; -import org.firstinspires.ftc.teamcode.drive.DriveConstants; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; /* * This routine determines the effective track width. The procedure works by executing a point turn diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelForwardOffsetTuner.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelForwardOffsetTuner.java index ebe97f28b0d9..d6c2dc649e90 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelForwardOffsetTuner.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -12,8 +12,8 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.internal.system.Misc; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.StandardTrackingWheelLocalizer; /** * This routine determines the effective forward offset for the lateral tracking wheel. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelLateralDistanceTuner.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelLateralDistanceTuner.java index a11059c45986..e0b22b087a96 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelLateralDistanceTuner.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; @@ -7,8 +7,8 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.RobotLog; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.StandardTrackingWheelLocalizer; /** * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TurnTest.java similarity index 77% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TurnTest.java index 0637d19efea4..38beecf9dac5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TurnTest.java @@ -1,10 +1,10 @@ -package org.firstinspires.ftc.teamcode.drive.opmode; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; /* * This is a simple routine to test turning capabilities. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java similarity index 91% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java index 84bccfe1de19..5c8e74c3d0b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java similarity index 87% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java index 2e735d471ba7..cfbfa49b5188 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment; import com.acmerobotics.roadrunner.trajectory.Trajectory; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java similarity index 92% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java index 11995dba7102..c46c54769b3c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.profile.MotionProfile; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java similarity index 79% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java index 62d2ba46ee8d..18337d05e416 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.sequencesegment; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AssetsTrajectoryManager.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AssetsTrajectoryManager.java index 1b34a958d5aa..8d2d4b2469fa 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AssetsTrajectoryManager.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.util; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; import androidx.annotation.Nullable; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxesSigns.java similarity index 93% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxesSigns.java index 42271d65d9bc..a7f3bd409a7f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxesSigns.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.util; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; /** * IMU axes signs in the order XYZ (after remapping). diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxisDirection.java similarity index 63% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxisDirection.java index 5b8279b56566..5a205bda5513 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxisDirection.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.util; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; /** * A direction for an axis to be remapped to diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/DashboardUtil.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/DashboardUtil.java index ce11d8dc4353..17265ab2fc7c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/DashboardUtil.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.util; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; import com.acmerobotics.dashboard.canvas.Canvas; import com.acmerobotics.roadrunner.geometry.Pose2d; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/Encoder.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/Encoder.java index f724c5242ddb..908b90375dc9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/Encoder.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.util; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; import com.acmerobotics.roadrunner.util.NanoClock; import com.qualcomm.robotcore.hardware.DcMotorEx; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LogFiles.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LogFiles.java index 8338eb1c638b..e28fa31ca282 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LogFiles.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.util; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; import android.annotation.SuppressLint; import android.content.Context; @@ -16,10 +16,10 @@ import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar; import org.firstinspires.ftc.robotcore.internal.system.AppUtil; -import org.firstinspires.ftc.teamcode.drive.DriveConstants; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.drive.SampleTankDrive; -import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleTankDrive; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.StandardTrackingWheelLocalizer; import java.io.File; import java.io.FileInputStream; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LoggingUtil.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LoggingUtil.java index 2c558f15ac12..b7776cfcc5f8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LoggingUtil.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.util; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; import org.firstinspires.ftc.robotcore.internal.system.AppUtil; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LynxModuleUtil.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LynxModuleUtil.java index 243dd7eb0e69..b9de338346ae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LynxModuleUtil.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.util; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.hardware.HardwareMap; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/RegressionUtil.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/RegressionUtil.java index c3bf488ee90d..ca227dfda7d7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/RegressionUtil.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.util; +package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.util; import androidx.annotation.Nullable; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java deleted file mode 100644 index 081d69578461..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.trajectorysequence; - - -public class EmptySequenceException extends RuntimeException { } From d21d4ea02ef6342e04eaaaa1f224abc48bd7c764 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Thu, 5 Dec 2024 12:38:52 +0800 Subject: [PATCH 75/93] Manual temporarily completed, which is seemly a piece of sh!t --- .../ftc/teamcode/ManualOpMode.java | 84 ++++++++++++++----- .../ftc/teamcode/hardware/RobotTop.java | 25 ++++++ build.dependencies.gradle | 1 - 3 files changed, 86 insertions(+), 24 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 1a05b07d1d75..9e6963dffa76 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -6,6 +6,7 @@ import org.firstinspires.ftc.teamcode.hardware.RobotChassis; import org.firstinspires.ftc.teamcode.hardware.RobotTop; +import org.firstinspires.ftc.teamcode.hardware.RobotVision.RobotVisionAngle; @TeleOp public class ManualOpMode extends LinearOpMode { @@ -14,8 +15,8 @@ public class ManualOpMode extends LinearOpMode { // servoName(port): positionRange[a, b]; defaultPos // control hub: - // top(3): - // container(4): [0.35-open, 1-close]; default: 1 + // top(3): [0-idle, 0.62-out] up: 0.05; default: 0 + // container(2): [0.35-open, 1-close]; default: 1 // lift(5): [0.2-open, 0.6-close];default: 0.6 // -------------------------------------------- // expansion hub: @@ -30,15 +31,11 @@ public class ManualOpMode extends LinearOpMode { final double STRETCH_BACK_POSITION = 0.03; final double STRETCH_OUT_POSITION = 0.3; final double SPIN_X_DEFAULT_POSITION = 0.5; - //TODO: test spinX hovering position final double SPIN_X_HOVERING_POSITION = 0.53; - //TODO: test the spinX down position final double SPIN_X_DOWN_POSITION = 0.58; final double SPIN_Y_DEFAULT_POSITION = 0.1; final double TURN_BACK_POSITION = 0.38; - //TODO: test the hovering position final double TURN_OUT_HOVERING_POSITION = 0.64; - //TODO: test the grabbing down position final double TURN_OUT_DOWN_POSITION = 0.71; final double GRAB_OPEN_POSITION = 0.2; final double GRAB_CLOSE_POSITION = 0.9; @@ -47,6 +44,8 @@ public class ManualOpMode extends LinearOpMode { public void runOpMode() { robotChassis = new RobotChassis(this); robotTop = new RobotTop(this); + RobotVisionAngle robotVisionAngle = new RobotVisionAngle(); + robotVisionAngle.initialize(hardwareMap); // 初始化摄像头 Gamepad previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); @@ -56,17 +55,22 @@ public void runOpMode() { double armSpinXPos = SPIN_X_DEFAULT_POSITION; double armSpinYPos = SPIN_Y_DEFAULT_POSITION; boolean armGrabbing = false; + boolean grabbingFlag = false; + boolean topServoState = false; + boolean containerRelease = false; + double recognitionAngle = 0; LiftState liftState = LiftState.BOTTOM; ArmState armState = ArmState.IDLE; waitForStart(); while (opModeIsActive()) { - robotChassis.driveRobot(gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); // robotLift liftPosition = robotTop.getLiftPosition(); if (liftState == LiftState.BOTTOM) { if (gamepad1.y && liftPosition <= LIFT_TOP_POSITION - 150) { + robotTop.setTopServoPosition(0.05); robotTop.setLeftPower(0.7); } else if (liftPosition >= LIFT_TOP_POSITION) { robotTop.setLeftPower(0); @@ -79,11 +83,36 @@ public void runOpMode() { robotTop.setLeftPower(-0.6); } else if (liftPosition <= LIFT_BOTTOM_POSITION) { robotTop.setLeftPower(0); + robotTop.setTopServoPosition(0.05); liftState = LiftState.BOTTOM; } else if (liftPosition <= LIFT_BOTTOM_POSITION + 150) { robotTop.setLeftPower(-0.2); } } + if(gamepad1.y && !previousGamepad1.y && armState == ArmState.IDLE){ + armStretchPos = STRETCH_OUT_POSITION; + armState = ArmState.TURNING_OUT; + robotTop.setArmStretchPosition(armStretchPos); + robotChassis.stopMotor(); + sleep(500); + } + if(gamepad1.left_bumper && !previousGamepad1.left_bumper + && liftState == LiftState.TOP && armState != ArmState.IDLE){ + if(topServoState){ + robotTop.setTopServoPosition(0.05); + }else{ + robotTop.setTopServoPosition(0.6); + } + topServoState = !topServoState; + } + if(gamepad1.right_bumper && !previousGamepad1.right_bumper){ + if(containerRelease){ + robotTop.setContainerServoPosition(1); + }else { + robotTop.setContainerServoPosition(0.35); + } + containerRelease = !containerRelease; + } // robotArm if (gamepad1.x && !previousGamepad1.x) { @@ -91,25 +120,26 @@ public void runOpMode() { armStretchPos = STRETCH_OUT_POSITION; armState = ArmState.STRETCHED; robotTop.setArmStretchPosition(armStretchPos); - } else if (armState == ArmState.STRETCHED) { - armState = ArmState.WITHDRAWING; } +// else if (armState == ArmState.STRETCHED) { +// armState = ArmState.WITHDRAWING; +// } } if (armState == ArmState.WITHDRAWING) { if (armStretchPos <= STRETCH_BACK_POSITION) { armState = ArmState.IDLE; } else { - armStretchPos -= 0.04; + armStretchPos -= 0.03; robotTop.setArmStretchPosition(armStretchPos); } } - if (gamepad1.a && !previousGamepad1.a) { - if (armState == ArmState.STRETCHED) { - armState = ArmState.TURNING_OUT; - } else if (armState == ArmState.TURNED) { - armState = ArmState.TURNING_BACK; - } - } +// if (gamepad1.a && !previousGamepad1.a) { +// if (armState == ArmState.STRETCHED) { +// armState = ArmState.TURNING_OUT; +// } else if (armState == ArmState.TURNED) { +// armState = ArmState.TURNING_BACK; +// } +// } if (armState == ArmState.TURNING_OUT) { if (armTurnPos >= TURN_OUT_HOVERING_POSITION - 0.05) { armTurnPos = TURN_OUT_HOVERING_POSITION; @@ -125,7 +155,7 @@ public void runOpMode() { if (armTurnPos <= TURN_BACK_POSITION + 0.05) { armTurnPos = TURN_BACK_POSITION; robotTop.setArmTurnPosition(armTurnPos); - armState = ArmState.STRETCHED; + armState = ArmState.WITHDRAWING; } else { armTurnPos -= 0.03; robotTop.setArmTurnPosition(armTurnPos); @@ -134,20 +164,22 @@ public void runOpMode() { if (gamepad1.b && !previousGamepad1.b) { robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); sleep(500); - if (!armGrabbing) { + if (!armGrabbing && armState == ArmState.TURNED) { robotTop.setArmTurnPosition(TURN_OUT_DOWN_POSITION); robotTop.setArmSpinXPosition(SPIN_X_DOWN_POSITION); sleep(200); robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); - sleep(500); - robotTop.setArmTurnPosition(TURN_OUT_HOVERING_POSITION); - armGrabbing = true; - telemetry.addData("grab", 1); + grabbingFlag = true; } else { robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); armGrabbing = false; } } + if(grabbingFlag && !gamepad1.b && previousGamepad1.b){ + robotTop.setArmTurnPosition(TURN_OUT_HOVERING_POSITION); + armGrabbing = true; + grabbingFlag = false; + } if (gamepad1.dpad_up) { armSpinXPos = Math.min(1, armSpinXPos + 0.02); robotTop.setArmSpinXPosition(armSpinXPos); @@ -163,6 +195,11 @@ public void runOpMode() { robotTop.setArmSpinYPosition(armSpinYPos); } + //vision + if(armState == ArmState.TURNED){ + recognitionAngle = robotVisionAngle.getDetectedAngle(); + } + // telemetry telemetry.addData("liftPos", liftPosition); telemetry.addData("state", liftState); @@ -170,6 +207,7 @@ public void runOpMode() { telemetry.addData("XPos", armSpinXPos); telemetry.addData("YPos", armSpinYPos); telemetry.addData("grab", armGrabbing); + telemetry.addData("angle", recognitionAngle); telemetry.update(); previousGamepad1.copy(gamepad1); sleep(50); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java index 4a9cf83b80aa..10056906ba28 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java @@ -22,6 +22,8 @@ public class RobotTop { private Servo armSpinYServo; private Servo armGrabServo; private Servo liftServo; + private Servo topServo; + private Servo containerServo; public RobotTop(LinearOpMode opMode) { this.opMode = opMode; @@ -39,6 +41,8 @@ public void init() { armSpinYServo = hardwareMap.get(Servo.class, "armSpinY"); armGrabServo = hardwareMap.get(Servo.class, "armGrab"); liftServo = hardwareMap.get(Servo.class, "liftServo"); + topServo = hardwareMap.get(Servo.class, "liftTop"); + containerServo = hardwareMap.get(Servo.class, "containerServo"); leftLiftMotor.setDirection(DcMotor.Direction.FORWARD); rightLiftMotor.setDirection(DcMotor.Direction.REVERSE); @@ -100,4 +104,25 @@ public void setArmGrabPosition(double position) { public double getArmGrabPosition() { return armGrabServo.getPosition(); } + public void setLiftServoPosition(double position) { + liftServo.setPosition(position); + } + + public double getLiftServoPosition() { + return liftServo.getPosition(); + } + public void setTopServoPosition(double position) { + topServo.setPosition(position); + } + + public double getTopServoPosition() { + return topServo.getPosition(); + } + public void setContainerServoPosition(double position) { + containerServo.setPosition(position); + } + + public double getContainerServoPosition() { + return containerServo.getPosition(); + } } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 72a1d3e2fe11..1ec9869b3ed6 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -15,7 +15,6 @@ dependencies { implementation 'org.firstinspires.ftc:Vision:10.1.0' implementation 'androidx.appcompat:appcompat:1.2.0' implementation 'com.acmerobotics.roadrunner:core:0.5.2' -// implementation 'com.sparkfun:otos:1.0.0' implementation('com.acmerobotics.dashboard:dashboard:0.4.16') { exclude group: 'org.firstinspires.ftc' } From 1146afe86637c4d5d66f0437daadf9eb464e2e04 Mon Sep 17 00:00:00 2001 From: Ehicy <102399008+Ehicy@users.noreply.github.com> Date: Thu, 5 Dec 2024 12:49:37 +0800 Subject: [PATCH 76/93] Update RobotVisionAngle.java add the minus --- .../RobotVision/RobotVisionAngle.java | 25 +++++-------------- 1 file changed, 6 insertions(+), 19 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java index c8153cac38d0..40aa2acad299 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java @@ -16,7 +16,7 @@ public class RobotVisionAngle { private OpenCvCamera webcam; - private double detectedAngle = 0; // 角度变量 + private double detectedAngle = 0; public void initialize(HardwareMap hardwareMap) { int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( @@ -32,7 +32,6 @@ public void onOpened() { @Override public void onError(int errorCode) { - // 处理摄像头打开错误 } }); } @@ -51,22 +50,15 @@ private class CenterAnglePipeline extends OpenCvPipeline { @Override public Mat processFrame(Mat input) { - // 转换为灰度图像 Imgproc.cvtColor(input, gray, Imgproc.COLOR_RGB2GRAY); - // 高斯模糊，减少噪声 Imgproc.GaussianBlur(gray, blurred, new Size(5, 5), 0); - // 锐化内核 kernel.put(0, 0, 0, -1, 0); kernel.put(1, 0, -1, 5, -1); kernel.put(2, 0, 0, -1, 0); - // 锐化处理 Imgproc.filter2D(blurred, sharp, gray.depth(), kernel); - // 获取图像中心区域 (3/4的画面) Rect centerRect = new Rect(input.width() / 8, input.height() / 8, input.width() * 3 / 4, input.height() * 3 / 4); Mat centerMat = sharp.submat(centerRect); - // 边缘检测 Imgproc.Canny(centerMat, edges, 50, 150); - // 霍夫直线变换检测线条 Imgproc.HoughLinesP(edges, lines, 1, Math.PI / 180, 50, 50, 10); double angle = 0; @@ -76,31 +68,26 @@ public Mat processFrame(Mat input) { double dx = line[2] - line[0]; double dy = line[3] - line[1]; angle = Math.toDegrees(Math.atan2(dy, dx)); - if (angle < 0) { - angle += 180; - } - angle = Math.abs(angle - 90); - break; // 考虑简单起见只取第一条检测到的线条 + break; } } - // 四舍五入到最接近的10位数 detectedAngle = Math.round(angle / 10.0) * 10; - // 获取图像中心点和矩形中心点 Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); Point rectCenter = new Point(centerRect.x + centerRect.width / 2.0, centerRect.y + centerRect.height / 2.0); - // 计算距离和角度 double distance = Math.sqrt(Math.pow(rectCenter.x - imageCenter.x, 2) + Math.pow(rectCenter.y - imageCenter.y, 2)); double rectAngle = Math.toDegrees(Math.atan2(rectCenter.y - imageCenter.y, rectCenter.x - imageCenter.x)); - // 将角度存储到polarCoordinates中 + if (rectAngle > 180) { + rectAngle -= 360; + } + double[] polarCoordinates = new double[2]; polarCoordinates[0] = distance; polarCoordinates[1] = rectAngle; - // 在图像上绘制最大矩形和中心点 Imgproc.rectangle(input, centerRect, new Scalar(0, 255, 0), 2); Imgproc.circle(input, rectCenter, 5, new Scalar(0, 255, 0), -1); Imgproc.putText(input, "Distance: " + distance, new Point(10, 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); From 49e54b77f35dfcb2480e4ccbf2d3579955a78cf6 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Thu, 5 Dec 2024 12:53:02 +0800 Subject: [PATCH 77/93] Feat:FIXED. --- .../java/com/example/meepmeep/MeepTurbo.java | 6 +- .../drive/DriveConstants.java | 17 ++--- .../ftc/teamcode/hardware/SparkFunOTOS.java | 6 ++ .../ftc/teamcode/test/RRTest.java | 68 ++++++++----------- 4 files changed, 46 insertions(+), 51 deletions(-) diff --git a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java index cf56cc860d78..89f600e9b272 100644 --- a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java +++ b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java @@ -16,11 +16,11 @@ public static void main(String[] args) { RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep) // Required: Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width - .setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15) + .setConstraints(500, 60, Math.toRadians(1000), Math.toRadians(1000), 339/2.54) // Option: Set theme. Default = ColorSchemeRedDark() .setColorScheme(new ColorSchemeRedDark()) .followTrajectorySequence(drive -> - drive.trajectorySequenceBuilder(new Pose2d(0, 0, 0)) + drive.trajectorySequenceBuilder(new Pose2d(0, 0, 90)) .forward(30) .turn(Math.toRadians(90)) .forward(30) @@ -31,7 +31,7 @@ public static void main(String[] args) { // bot.wobbleArm.lower() }) .turn(Math.toRadians(90)) - .splineTo(new Vector2d(10, 15), 0) + .splineTo(new Vector2d(50, 15), 0) .turn(Math.toRadians(90)) .build() ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java index 4e04f518372a..8f25e98f0656 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java @@ -21,8 +21,8 @@ public class DriveConstants { /* * These are motor constants that should be listed online for your motors. */ - public static final double TICKS_PER_REV = 537.6; - public static final double MAX_RPM = 312.5; + public static final double TICKS_PER_REV = 500; + public static final double MAX_RPM = 300; /* * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. @@ -45,8 +45,8 @@ public class DriveConstants { * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ public static double WHEEL_RADIUS = 3; // in (轮子半径 单位：英寸) - public static double GEAR_RATIO = 1.0/15.0; // (齿轮比) output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 33.9/2.54; // (轮距 单位：英寸) in + public static double GEAR_RATIO = 15.0; // (齿轮比) output (wheel) speed / input (motor) speed + public static double TRACK_WIDTH = 339/2.54; // (轮距 单位：英寸) in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive @@ -54,8 +54,8 @@ public class DriveConstants { * empirically tuned. */ public static double kV = 1.0 / rpmToVelocity(MAX_RPM); - public static double kA = 0; - public static double kStatic = 0; + public static double kA = 0.01; + public static double kStatic = 0.05; /* * These values are used to generate the trajectories for you robot. To ensure proper operation, @@ -65,8 +65,8 @@ public class DriveConstants { * inches. */ //TODO:ADJUST THESE NUMBERS. - public static double MAX_VEL = MAX_RPM*GEAR_RATIO*WHEEL_RADIUS*2*Math.PI/60; - public static double MAX_ACCEL = MAX_RPM*GEAR_RATIO*WHEEL_RADIUS*2*Math.PI/60; + public static double MAX_VEL = 30; + public static double MAX_ACCEL = 30; public static double MAX_ANG_VEL = Math.toRadians(MAX_VEL/TRACK_WIDTH); public static double MAX_ANG_ACCEL = Math.toRadians(MAX_VEL/TRACK_WIDTH); @@ -92,3 +92,4 @@ public static double getMotorVelocityF(double ticksPerSecond) { return 32767 / ticksPerSecond; } } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOS.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOS.java index fced04f2e6b6..aa67dffda117 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOS.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOS.java @@ -38,4 +38,10 @@ public void updateData(double newX, double newY, double newHeading) { this.y = newY; this.heading = newHeading; } + + public double getDistance() { + + return 0; + } } + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java index c1e94b05a4fb..6ae13f86210f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -2,59 +2,47 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.hardware.SparkFunOTOS; -import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; -@Autonomous(name = "RRTest") +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; +import org.firstinspires.ftc.teamcode.hardware.SparkFunOTOS; +@Autonomous public class RRTest extends LinearOpMode { - private SparkFunOTOS sparkFunOTOS; + private SampleMecanumDrive drive; + private SparkFunOTOS otosSensor; @Override public void runOpMode() throws InterruptedException { - // 初始化SparkFun OTOS传感器 - sparkFunOTOS = new SparkFunOTOS(hardwareMap); - - // 初始化驱动对象 - SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); - - // 定义机器人起始位置 - Pose2d startPose = new Pose2d(0, 0, Math.toRadians(90.0)); - drive.setPoseEstimate(startPose); - - // 构建轨迹序列 - TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(new Pose2d(43.40, 49.04, Math.toRadians(90.00))) - .splineTo(new Vector2d(47.83, -32.93), Math.toRadians(-86.91)) - .splineTo(new Vector2d(18.43, -53.47), Math.toRadians(214.94)) - .splineTo(new Vector2d(-48.23, -41.99), Math.toRadians(170.23)) - .splineTo(new Vector2d(-46.22, 3.52), Math.toRadians(87.47)) - .splineTo(new Vector2d(-35.55, 47.03), Math.toRadians(76.21)) - .splineTo(new Vector2d(11.98, 52.67), Math.toRadians(6.77)) + // 初始化驱动器和传感器 + drive = new SampleMecanumDrive(hardwareMap); + otosSensor = hardwareMap.get(SparkFunOTOS.class, "otos"); + + // 定义目标位置 + Pose2d targetPosition = new Pose2d(30, 30, Math.toRadians(0)); // 示例目标位置 + + // 创建轨迹 + Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) + .lineTo(new Vector2d(targetPosition.getX(), targetPosition.getY())) .build(); - // 等待开始命令 + // 等待开始指令 waitForStart(); - // 开始跟踪轨迹 - if (opModeIsActive()) { - drive.followTrajectorySequence(trajectory0); - - while (opModeIsActive()) { - // 更新位置信息 - drive.update(); - - // 输出当前位置信息到Telemetry - telemetry.addData("Current Pose", drive.getPoseEstimate()); - telemetry.addData("SparkFun OTOS X", sparkFunOTOS.getX()); - telemetry.addData("SparkFun OTOS Y", sparkFunOTOS.getY()); - telemetry.addData("SparkFun OTOS Heading", sparkFunOTOS.getHeading()); - telemetry.update(); - } + if (isStopRequested()) return; + + // 执行轨迹移动 + drive.followTrajectory(trajectory); + + while (opModeIsActive()) { + double distance = otosSensor.getDistance(); + telemetry.addData("Distance", distance); + telemetry.update(); + // 实时调整逻辑 } + } } - From 999e279017463fa352ba75e85e2088700e04e356 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Thu, 5 Dec 2024 20:56:29 +0800 Subject: [PATCH 78/93] Feat:FIXED RRTest. --- .../java/com/example/meepmeep/MeepTurbo.java | 39 ++++++++++++------- .../drive/SampleMecanumDrive.java | 6 +-- .../ftc/teamcode/test/RRTest.java | 28 +++++++++---- 3 files changed, 47 insertions(+), 26 deletions(-) diff --git a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java index 89f600e9b272..8a2c2da45e97 100644 --- a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java +++ b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java @@ -20,21 +20,30 @@ public static void main(String[] args) { // Option: Set theme. Default = ColorSchemeRedDark() .setColorScheme(new ColorSchemeRedDark()) .followTrajectorySequence(drive -> - drive.trajectorySequenceBuilder(new Pose2d(0, 0, 90)) - .forward(30) - .turn(Math.toRadians(90)) - .forward(30) - .addDisplacementMarker(() -> { - /* Everything in the marker callback should be commented out */ - - // bot.shooter.shoot() - // bot.wobbleArm.lower() - }) - .turn(Math.toRadians(90)) - .splineTo(new Vector2d(50, 15), 0) - .turn(Math.toRadians(90)) - .build() - ); + drive.trajectorySequenceBuilder(new Pose2d(-33.53, -65.35, Math.toRadians(15.81))) +// .forward(50) +// .turn(Math.toRadians(90)) +// .forward(50) +// .addDisplacementMarker(() -> { +// /* Everything in the marker callback should be commented out */ +// +// // bot.shooter.shoot() +// // bot.wobbleArm.lower() +// }) +// .turn(Math.toRadians(90)) +// .splineTo(new Vector2d(50, 15), 0) +// .turn(Math.toRadians(90)) +// .build() + //TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(new Pose2d(13.33, -60.10, Math.toRadians(24.60))) + //TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(new Pose2d(-33.53, -65.35, Math.toRadians(15.81))) + .splineTo(new Vector2d(46.62, -45.21), Math.toRadians(56.31)) + .splineTo(new Vector2d(56.29, 38.97), Math.toRadians(104.04)) + .splineTo(new Vector2d(-25.07, 57.90), Math.toRadians(186.77)) + .build()); + + + + // Set field image meepMeep.setBackground(MeepMeep.Background.FIELD_FREIGHTFRENZY_ADI_DARK) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java index 4743129ab580..6f5244983f6f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java @@ -42,10 +42,10 @@ */ @Config public class SampleMecanumDrive extends MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0.1, 0, 0.1); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(0.1, 0, 0.1); - public static double LATERAL_MULTIPLIER = 1; + public static double LATERAL_MULTIPLIER = 1.2; public static double VX_WEIGHT = 1; public static double VY_WEIGHT = 1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java index 6ae13f86210f..0eec8eee8bcf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.hardware.SparkFunOTOS; @@ -20,13 +21,24 @@ public void runOpMode() throws InterruptedException { // 初始化驱动器和传感器 drive = new SampleMecanumDrive(hardwareMap); otosSensor = hardwareMap.get(SparkFunOTOS.class, "otos"); - - // 定义目标位置 - Pose2d targetPosition = new Pose2d(30, 30, Math.toRadians(0)); // 示例目标位置 - - // 创建轨迹 - Trajectory trajectory = drive.trajectoryBuilder(new Pose2d()) - .lineTo(new Vector2d(targetPosition.getX(), targetPosition.getY())) + // 创建轨迹序列 +// TrajectorySequence trajectorySequence = drive.trajectorySequenceBuilder(new Pose2d(0, 0, 90)) +// .forward(100) +// .turn(Math.toRadians(90)) +// .forward(100) +// .addDisplacementMarker(() -> { +// // 在这里添加要执行的操作，如发射器射击或手臂放下 +// // bot.shooter.shoot(); +// // bot.wobbleArm.lower(); +// }) +// .turn(Math.toRadians(90)) +// .splineTo(new Vector2d(50, 15), Math.toRadians(0)) +// .turn(Math.toRadians(90)) +// .build(); + TrajectorySequence trajectorySequence = drive.trajectorySequenceBuilder(new Pose2d(-33.53, -65.35, Math.toRadians(15.81))) + .splineTo(new Vector2d(46.62, -45.21), Math.toRadians(56.31)) + .splineTo(new Vector2d(56.29, 38.97), Math.toRadians(104.04)) + .splineTo(new Vector2d(-25.07, 57.90), Math.toRadians(186.77)) .build(); // 等待开始指令 @@ -35,7 +47,7 @@ public void runOpMode() throws InterruptedException { if (isStopRequested()) return; // 执行轨迹移动 - drive.followTrajectory(trajectory); + drive.followTrajectorySequence(trajectorySequence); while (opModeIsActive()) { double distance = otosSensor.getDistance(); From b9ee3216733449ffe592169eec98623d6daf0662 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Thu, 5 Dec 2024 21:19:47 +0800 Subject: [PATCH 79/93] Manual completed --- .../ftc/teamcode/ManualOpMode.java | 83 +++++++++++++------ .../advancedManual/AdvancedManual.java | 27 ++++++ .../teamcode/advancedManual/ServoManager.java | 78 +++++++++++++++++ .../teamcode/advancedManual/ServoTask.java | 35 ++++++++ .../advancedManual/TimedServoTest.java | 33 ++++++++ .../ftc/teamcode/hardware/RobotChassis.java | 8 +- .../ftc/teamcode/hardware/RobotTop.java | 21 +++-- 7 files changed, 243 insertions(+), 42 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoManager.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoTask.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TimedServoTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 9e6963dffa76..cc8003c176ca 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -30,7 +30,7 @@ public class ManualOpMode extends LinearOpMode { final int LIFT_BOTTOM_POSITION = 50; final double STRETCH_BACK_POSITION = 0.03; final double STRETCH_OUT_POSITION = 0.3; - final double SPIN_X_DEFAULT_POSITION = 0.5; + final double SPIN_X_DEFAULT_POSITION = 0.22; final double SPIN_X_HOVERING_POSITION = 0.53; final double SPIN_X_DOWN_POSITION = 0.58; final double SPIN_Y_DEFAULT_POSITION = 0.1; @@ -45,7 +45,7 @@ public void runOpMode() { robotChassis = new RobotChassis(this); robotTop = new RobotTop(this); RobotVisionAngle robotVisionAngle = new RobotVisionAngle(); - robotVisionAngle.initialize(hardwareMap); // 初始化摄像头 + robotVisionAngle.initialize(hardwareMap); Gamepad previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); @@ -63,6 +63,8 @@ public void runOpMode() { ArmState armState = ArmState.IDLE; waitForStart(); + robotTop.setArmStretchPosition(armStretchPos); + robotTop.setArmTurnPosition(armTurnPos); while (opModeIsActive()) { robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); @@ -72,7 +74,10 @@ public void runOpMode() { if (gamepad1.y && liftPosition <= LIFT_TOP_POSITION - 150) { robotTop.setTopServoPosition(0.05); robotTop.setLeftPower(0.7); - } else if (liftPosition >= LIFT_TOP_POSITION) { + liftState = LiftState.GOING_UP; + } + } else if (liftState == LiftState.GOING_UP) { + if (liftPosition >= LIFT_TOP_POSITION) { robotTop.setLeftPower(0); liftState = LiftState.TOP; } else if (liftPosition >= LIFT_TOP_POSITION - 150) { @@ -81,7 +86,13 @@ public void runOpMode() { } else if (liftState == LiftState.TOP) { if (gamepad1.y && liftPosition >= LIFT_BOTTOM_POSITION + 150) { robotTop.setLeftPower(-0.6); - } else if (liftPosition <= LIFT_BOTTOM_POSITION) { + liftState = LiftState.GOING_DOWN; + } + if (liftPosition <= LIFT_TOP_POSITION) { + robotTop.setLeftPower(0.3); + } + }else if(liftState == LiftState.GOING_DOWN){ + if (liftPosition <= LIFT_BOTTOM_POSITION) { robotTop.setLeftPower(0); robotTop.setTopServoPosition(0.05); liftState = LiftState.BOTTOM; @@ -89,12 +100,20 @@ public void runOpMode() { robotTop.setLeftPower(-0.2); } } - if(gamepad1.y && !previousGamepad1.y && armState == ArmState.IDLE){ - armStretchPos = STRETCH_OUT_POSITION; - armState = ArmState.TURNING_OUT; - robotTop.setArmStretchPosition(armStretchPos); - robotChassis.stopMotor(); - sleep(500); + if(gamepad1.y && !previousGamepad1.y){ + if(armState == ArmState.IDLE){ + robotChassis.stopMotor(); + armStretchPos = STRETCH_OUT_POSITION; + robotTop.setArmStretchPosition(armStretchPos); + robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION + 0.2); + sleep(1000); + armState = ArmState.LOCKED; + }else if(armState == ArmState.LOCKED){ + robotChassis.stopMotor(); + sleep(1000); + armStretchPos = STRETCH_BACK_POSITION; + armState = ArmState.WITHDRAWING; + } } if(gamepad1.left_bumper && !previousGamepad1.left_bumper && liftState == LiftState.TOP && armState != ArmState.IDLE){ @@ -118,28 +137,23 @@ public void runOpMode() { if (gamepad1.x && !previousGamepad1.x) { if (armState == ArmState.IDLE) { armStretchPos = STRETCH_OUT_POSITION; - armState = ArmState.STRETCHED; + armState = ArmState.TURNING_OUT; robotTop.setArmStretchPosition(armStretchPos); } -// else if (armState == ArmState.STRETCHED) { -// armState = ArmState.WITHDRAWING; -// } + else if (armState == ArmState.TURNED) { + armState = ArmState.TURNING_BACK; + robotTop.setArmSpinYPosition(SPIN_Y_DEFAULT_POSITION); + } } if (armState == ArmState.WITHDRAWING) { if (armStretchPos <= STRETCH_BACK_POSITION) { armState = ArmState.IDLE; + robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION); } else { armStretchPos -= 0.03; robotTop.setArmStretchPosition(armStretchPos); } } -// if (gamepad1.a && !previousGamepad1.a) { -// if (armState == ArmState.STRETCHED) { -// armState = ArmState.TURNING_OUT; -// } else if (armState == ArmState.TURNED) { -// armState = ArmState.TURNING_BACK; -// } -// } if (armState == ArmState.TURNING_OUT) { if (armTurnPos >= TURN_OUT_HOVERING_POSITION - 0.05) { armTurnPos = TURN_OUT_HOVERING_POSITION; @@ -167,19 +181,21 @@ public void runOpMode() { if (!armGrabbing && armState == ArmState.TURNED) { robotTop.setArmTurnPosition(TURN_OUT_DOWN_POSITION); robotTop.setArmSpinXPosition(SPIN_X_DOWN_POSITION); - sleep(200); - robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); grabbingFlag = true; } else { robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); armGrabbing = false; } } - if(grabbingFlag && !gamepad1.b && previousGamepad1.b){ + if(grabbingFlag && !gamepad1.b){ + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + sleep(500); robotTop.setArmTurnPosition(TURN_OUT_HOVERING_POSITION); + robotTop.setArmSpinXPosition(SPIN_X_HOVERING_POSITION); armGrabbing = true; grabbingFlag = false; } + robotTop.setArmStretchPosition(armStretchPos); if (gamepad1.dpad_up) { armSpinXPos = Math.min(1, armSpinXPos + 0.02); robotTop.setArmSpinXPosition(armSpinXPos); @@ -196,8 +212,9 @@ public void runOpMode() { } //vision - if(armState == ArmState.TURNED){ + if(gamepad1.a && !previousGamepad1.a){ recognitionAngle = robotVisionAngle.getDetectedAngle(); + robotTop.setArmSpinYPosition(calculateSpinY(recognitionAngle)); } // telemetry @@ -207,18 +224,30 @@ public void runOpMode() { telemetry.addData("XPos", armSpinXPos); telemetry.addData("YPos", armSpinYPos); telemetry.addData("grab", armGrabbing); + telemetry.addData("grabFlag", grabbingFlag); + telemetry.addData("stretch", armStretchPos); telemetry.addData("angle", recognitionAngle); telemetry.update(); previousGamepad1.copy(gamepad1); sleep(50); } } + + protected double calculateSpinY(double angle){ + if(angle <= 0){ + angle = -angle/360 + 0.1; + } + else if(angle >= 0){ + angle = 0.6 - angle/360; + } + return Math.min(angle,1); + } } enum LiftState { - BOTTOM, TOP + BOTTOM,GOING_UP, TOP, GOING_DOWN } enum ArmState { - IDLE, WITHDRAWING, STRETCHED, TURNING_OUT, TURNED, TURNING_BACK + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java new file mode 100644 index 000000000000..be607f7c6983 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.advancedManual; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +// The ManualOp currently used is a piece of shit. +// I'll write a better one here (if the time is enough). +public class AdvancedManual extends LinearOpMode { + @Override + public void runOpMode(){ + ServoManager servoMgr = new ServoManager(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + //code + + servoMgr.updateServos(); + previousGamepad1.copy(gamepad1); + sleep(50); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoManager.java new file mode 100644 index 000000000000..b45c6d5058eb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoManager.java @@ -0,0 +1,78 @@ +package org.firstinspires.ftc.teamcode.advancedManual; + +import android.util.ArrayMap; + +import com.qualcomm.robotcore.hardware.Servo; + +import java.util.List; +import java.util.Map; +import java.util.stream.Collectors; + +public class ServoManager { + protected Map tasks; + + public ServoManager() { + tasks = new ArrayMap<>(); + } + + public void updateServos(){ + for (Map.Entry taskEntry : tasks.entrySet()) { + int taskId = taskEntry.getKey(); + ServoTask task = taskEntry.getValue(); + if (!task.hasNext()) { + task.finish(); + tasks.remove(taskId, task); + continue; + } + task.iterate(); + } + } + + /** + * Sets the current position of the servo in a fixed time, expressed as a fraction of + * its available range. If PWM power is enabled for the servo, the servo will attempt + * to move to the indicated position. + * + * @param position the position to which the servo should move, a value in the range [0.0, 1.0] + * @param timeMs the time that the servo will take to move. + * @return a Task object + */ + private ServoTask timedSetPosition(Servo servo, double position, long timeMs) { + final double deltaPos = position - servo.getPosition(); + final long iterationCount = timeMs / ServoTask.TICK_MS; + final double positionPerIteration = deltaPos / iterationCount; + return new ServoTask() { + long remainingIterations = iterationCount; + final double targetPosition = position; + + @Override + public void iterate() { + servo.setPosition(servo.getPosition() + positionPerIteration); + remainingIterations--; + } + + @Override + public void finish() { + servo.setPosition(targetPosition); + } + + @Override + public boolean hasNext() { + return remainingIterations > 0; + } + }; + } + public void setTimedServoPosition(Servo servo, double position, long timeMs){ + ServoTask task = timedSetPosition(servo, position, timeMs); + int taskId = findMinFreeTaskId(); + tasks.put(taskId, task); + } + private int findMinFreeTaskId() { + List taskIdList = tasks.keySet().stream().sorted().collect(Collectors.toList()); + int maxTaskId = taskIdList.get(taskIdList.size() - 1); + for (int i = 0; i <= maxTaskId; i++) { + if (taskIdList.contains(i)) return 1; + } + return maxTaskId + 1; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoTask.java new file mode 100644 index 000000000000..9cd9a6e45b96 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoTask.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode.advancedManual; + +public interface ServoTask { + int TICK_MS = 50; + + /** + * This method is called in the main loop of OpMode + * It should contain the code of EACH TICK, instead of a loop in it! + */ + void iterate(); + + /** + * To judge whether the task needs to continue. + * @return Return true when it need to continue, otherwise false. + */ + boolean hasNext(); + + /** + * This method is called once when the task starts looping in the TaskOpMode. + */ + default void init() { + } + + /** + * The method is called when the task is finished. + */ + default void finish() { + } + + /** + * The method is called when the task is canceled. + */ + default void cancel() { + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TimedServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TimedServoTest.java new file mode 100644 index 000000000000..11c3149593cb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TimedServoTest.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.advancedManual; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +// The ManualOp currently used is a piece of shit. +// I'll write a better one here (if the time is enough). +public class TimedServoTest extends LinearOpMode { + @Override + public void runOpMode(){ + Servo servo = hardwareMap.get(Servo.class, "servo"); + ServoManager servoMgr = new ServoManager(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + if(gamepad1.a && !previousGamepad1.a){ + servoMgr.setTimedServoPosition(servo, 1,2000); + } + if(gamepad1.b && !previousGamepad1.b){ + servoMgr.setTimedServoPosition(servo, 0,2000); + } + + servoMgr.updateServos(); + previousGamepad1.copy(gamepad1); + sleep(50); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java index 27766929b96f..f99814a82b44 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java @@ -12,10 +12,10 @@ public class RobotChassis { LinearOpMode opMode; HardwareMap hardwareMap; Telemetry telemetry; - private DcMotor leftFrontDrive; - private DcMotor leftBackDrive; - private DcMotor rightFrontDrive; - private DcMotor rightBackDrive; + protected DcMotor leftFrontDrive; + protected DcMotor leftBackDrive; + protected DcMotor rightFrontDrive; + protected DcMotor rightBackDrive; public RobotChassis(LinearOpMode opMode) { this.opMode = opMode; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java index 10056906ba28..480af2c51b82 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotTop.java @@ -14,17 +14,16 @@ public class RobotTop { OpMode opMode; HardwareMap hardwareMap; Telemetry telemetry; - private DcMotor leftLiftMotor; - private DcMotor rightLiftMotor; - private Servo armStretchServo; - private Servo armTurnServo; - private Servo armSpinXServo; - private Servo armSpinYServo; - private Servo armGrabServo; - private Servo liftServo; - private Servo topServo; - private Servo containerServo; - + protected DcMotor leftLiftMotor; + protected DcMotor rightLiftMotor; + protected Servo armStretchServo; + protected Servo armTurnServo; + protected Servo armSpinXServo; + protected Servo armSpinYServo; + protected Servo armGrabServo; + protected Servo liftServo; + protected Servo topServo; + protected Servo containerServo; public RobotTop(LinearOpMode opMode) { this.opMode = opMode; hardwareMap = opMode.hardwareMap; From 3a331c1e982f1d265e52b67108d6bf8d54c319c0 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Thu, 5 Dec 2024 21:41:38 +0800 Subject: [PATCH 80/93] Manual completed --- .../java/org/firstinspires/ftc/teamcode/ManualOpMode.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index cc8003c176ca..be9800c04c8a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -106,11 +106,11 @@ public void runOpMode() { armStretchPos = STRETCH_OUT_POSITION; robotTop.setArmStretchPosition(armStretchPos); robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION + 0.2); - sleep(1000); + sleep(1500); armState = ArmState.LOCKED; }else if(armState == ArmState.LOCKED){ robotChassis.stopMotor(); - sleep(1000); + sleep(1500); armStretchPos = STRETCH_BACK_POSITION; armState = ArmState.WITHDRAWING; } From 7b09c61f514f150701bc26ef7f4ee113a4fd2acf Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Fri, 6 Dec 2024 05:57:33 +0800 Subject: [PATCH 81/93] Feat:FIXED RRTest. --- .../trajectorysequence/drive/DriveConstants.java | 10 +++++----- .../{SparkFunOTOS.java => SparkFunOTOSSensor.java} | 9 +++++---- 2 files changed, 10 insertions(+), 9 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/{SparkFunOTOS.java => SparkFunOTOSSensor.java} (80%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java index 8f25e98f0656..951450904a8a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java @@ -45,7 +45,7 @@ public class DriveConstants { * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ public static double WHEEL_RADIUS = 3; // in (轮子半径 单位：英寸) - public static double GEAR_RATIO = 15.0; // (齿轮比) output (wheel) speed / input (motor) speed + public static double GEAR_RATIO = 1.0/15.0; // (齿轮比) output (wheel) speed / input (motor) speed public static double TRACK_WIDTH = 339/2.54; // (轮距 单位：英寸) in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using @@ -65,10 +65,10 @@ public class DriveConstants { * inches. */ //TODO:ADJUST THESE NUMBERS. - public static double MAX_VEL = 30; - public static double MAX_ACCEL = 30; - public static double MAX_ANG_VEL = Math.toRadians(MAX_VEL/TRACK_WIDTH); - public static double MAX_ANG_ACCEL = Math.toRadians(MAX_VEL/TRACK_WIDTH); + public static double MAX_VEL = 60; + public static double MAX_ACCEL = 60; + public static double MAX_ANG_VEL = Math.toRadians(180); + public static double MAX_ANG_ACCEL = Math.toRadians(180); /* * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOS.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java similarity index 80% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOS.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java index aa67dffda117..0f2f59952d1d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOS.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java @@ -1,16 +1,17 @@ package org.firstinspires.ftc.teamcode.hardware; +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; import com.qualcomm.robotcore.hardware.HardwareMap; -public class SparkFunOTOS { - private com.qualcomm.hardware.sparkfun.SparkFunOTOS otosDevice; +public class SparkFunOTOSSensor { + private SparkFunOTOS otosDevice; private double x; private double y; private double heading; - public SparkFunOTOS(HardwareMap hardwareMap) { + public SparkFunOTOSSensor(HardwareMap hardwareMap) { // 假设 OTOS 传感器连接到 I2C 端口 - otosDevice = hardwareMap.get(com.qualcomm.hardware.sparkfun.SparkFunOTOS.class, "otos"); + otosDevice = hardwareMap.get(SparkFunOTOS.class, "otos"); // 初始化传感器（具体初始化代码取决于 OTOS 库的实现） this.x = 0.0; this.y = 0.0; From 1c4895303589d3f330f626e10bc1df868d1efa6b Mon Sep 17 00:00:00 2001 From: Ehicy <102399008+Ehicy@users.noreply.github.com> Date: Fri, 6 Dec 2024 06:35:36 +0800 Subject: [PATCH 82/93] Update ManualOpMode.java correct the calculations --- .../ftc/teamcode/ManualOpMode.java | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index be9800c04c8a..da4acff9e1e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -233,21 +233,26 @@ else if (armState == ArmState.TURNED) { } } - protected double calculateSpinY(double angle){ - if(angle <= 0){ - angle = -angle/360 + 0.1; - } - else if(angle >= 0){ - angle = 0.6 - angle/360; - } - return Math.min(angle,1); +protected double calculateSpinY(double angle) { + if (angle < -180 || angle > 180) { + throw new IllegalArgumentException("Angle must be between -180 and 180 degrees"); + } + + double adjustedAngle; + if (angle <= 0) { + adjustedAngle = -angle / 360.0 + 0.1; + } else { + adjustedAngle = 0.6 - angle / 360.0; } + + return Math.max(0, Math.min(adjustedAngle, 1)); // 确保值在0到1之间 } + enum LiftState { BOTTOM,GOING_UP, TOP, GOING_DOWN } enum ArmState { IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED -} \ No newline at end of file +} From 2cbc4779f9977859b99054252ee5dd18fbe49a3b Mon Sep 17 00:00:00 2001 From: Ehicy Date: Wed, 4 Dec 2024 21:33:15 +0800 Subject: [PATCH 83/93] fuck the sdk --- .../RobotVision/RobotCameraDistance.java | 116 ++++++------------ .../test/VisionCameraDistanceTest.java | 10 +- 2 files changed, 46 insertions(+), 80 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java index 00f48d65c6fb..eced74138094 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotCameraDistance.java @@ -2,15 +2,12 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.opencv.core.Core; -import org.opencv.core.CvType; import org.opencv.core.Mat; import org.opencv.core.MatOfPoint; import org.opencv.core.MatOfPoint2f; import org.opencv.core.Point; -import org.opencv.core.Rect; import org.opencv.core.RotatedRect; import org.opencv.core.Scalar; -import org.opencv.core.Size; import org.opencv.imgproc.Imgproc; import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; @@ -22,7 +19,7 @@ import java.util.List; public class RobotCameraDistance { - // 定义用于颜色检测的HSV颜色范围常量 + private static final Scalar LOWER_BOUND_RED = new Scalar(0, 100, 100); private static final Scalar UPPER_BOUND_RED = new Scalar(10, 255, 255); private static final Scalar LOWER_BOUND_YELLOW = new Scalar(20, 100, 100); @@ -30,59 +27,39 @@ public class RobotCameraDistance { private static final Scalar LOWER_BOUND_BLUE = new Scalar(110, 100, 100); private static final Scalar UPPER_BOUND_BLUE = new Scalar(130, 255, 255); - private OpenCvCamera webcam; // 用于摄像头操作的OpenCvCamera对象 - private String detectedColor; // 用于存储检测到的颜色 - private double[] polarCoordinates = new double[2]; // 数组形式，0为距离，1为角度 - private Mat hsv = new Mat(); // 用于存储HSV颜色空间图像 - private Mat mask = new Mat(); // 用于存储颜色掩码 - private Mat hierarchy = new Mat(); // 用于存储轮廓层级信息 - private Mat sharpened = new Mat(); // 用于存储锐化后的图像 - private Scalar lowerBound; // 用于存储颜色下界 - private Scalar upperBound; // 用于存储颜色上界 - - private RotatedRect currentRect; // 当前锁定的矩形 - - /** - * 初始化摄像头并设置颜色检测管道。 - * - * @param hardwareMap 硬件映射对象 - * @param color 要检测的颜色 - */ + private OpenCvCamera webcam; + private String detectedColor; + private double[] polarCoordinates = new double[2]; + private Mat hsv = new Mat(); + private Mat mask = new Mat(); + private Mat hierarchy = new Mat(); + private Scalar lowerBound; + private Scalar upperBound; + private RotatedRect currentRect; + public void initialize(HardwareMap hardwareMap, String color) { this.detectedColor = color; - setColorBounds(color); // 设置颜色范围 + setColorBounds(color); int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier( "cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); - webcam.setPipeline(new ColorDetectionPipeline()); // 设置颜色检测管道 + webcam.setPipeline(new ColorDetectionPipeline()); webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { @Override public void onOpened() { - // 开始视频流，设置分辨率和旋转角度 webcam.startStreaming(320, 240, OpenCvCameraRotation.UPRIGHT); } @Override public void onError(int errorCode) { - // 处理摄像头打开错误 } }); } - /** - * 获取极坐标信息，包括距离和角度。 - * - * @return 极坐标数组，0为距离，1为角度 - */ public double[] getPolarCoordinates() { return polarCoordinates; } - /** - * 根据颜色名称设置颜色范围。 - * - * @param color 颜色名称 - */ private void setColorBounds(String color) { switch (color.toLowerCase()) { case "red": @@ -102,46 +79,36 @@ private void setColorBounds(String color) { } } + public void updateCurrentRectExcludingCurrent() { + List contours = new ArrayList<>(); + Imgproc.findContours(mask, contours, hierarchy, Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); + + RotatedRect largestRotatedRect = null; + double maxArea = 0; + for (MatOfPoint contour : contours) { + MatOfPoint2f contour2f = new MatOfPoint2f(contour.toArray()); + RotatedRect rotatedRect = Imgproc.minAreaRect(contour2f); + double area = rotatedRect.size.area(); + if (area > maxArea && (currentRect == null || !isSameRect(rotatedRect, currentRect))) { + maxArea = area; + largestRotatedRect = rotatedRect; + } + } + + if (largestRotatedRect != null) { + currentRect = largestRotatedRect; + } + } + private class ColorDetectionPipeline extends OpenCvPipeline { @Override public Mat processFrame(Mat input) { - // 复制输入图像用于锐化处理 - Mat src = input.clone(); - Mat blurImg = new Mat(); - // 应用高斯模糊减少噪声 - Imgproc.GaussianBlur(src, blurImg, new Size(0, 0), 25); - // 创建锐化效果 - Core.addWeighted(src, 1.5, blurImg, -0.5, 0, sharpened); - // 将图像转换为HSV颜色空间 - Imgproc.cvtColor(sharpened, hsv, Imgproc.COLOR_RGB2HSV); - // 根据颜色范围创建掩码 + Imgproc.cvtColor(input, hsv, Imgproc.COLOR_RGB2HSV); Core.inRange(hsv, lowerBound, upperBound, mask); - // 查找图像中的轮廓 - List contours = new ArrayList<>(); - Imgproc.findContours(mask, contours, hierarchy, Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); - - // 查找最大矩形 - double maxArea = 0; - RotatedRect largestRotatedRect = null; - for (MatOfPoint contour : contours) { - // 将轮廓转换为2f点集 - MatOfPoint2f contour2f = new MatOfPoint2f(contour.toArray()); - // 查找最小面积的包围矩形 - RotatedRect rotatedRect = Imgproc.minAreaRect(contour2f); - double area = rotatedRect.size.area(); - if (area > maxArea) { - maxArea = area; - largestRotatedRect = rotatedRect; - } - } - // 计算距离和角度 - if (largestRotatedRect != null) { - // 如果当前锁定的矩形存在且变化超过阈值，则更新锁定 - if (currentRect == null || !isSameRect(currentRect, largestRotatedRect)) { - currentRect = largestRotatedRect; - } - // 更新极坐标计算基于currentRect + updateCurrentRectExcludingCurrent(); + + if (currentRect != null) { Point rectCenter = currentRect.center; Point imageCenter = new Point(input.width() / 2.0, input.height() / 2.0); double distance = Math.sqrt(Math.pow(rectCenter.x - imageCenter.x, 2) + Math.pow(rectCenter.y - imageCenter.y, 2)); @@ -152,14 +119,12 @@ public Mat processFrame(Mat input) { polarCoordinates[0] = distance; polarCoordinates[1] = angle; - // 绘制最大旋转矩形 Point[] vertices = new Point[4]; currentRect.points(vertices); for (int i = 0; i < 4; i++) { Imgproc.line(input, vertices[i], vertices[(i + 1) % 4], new Scalar(0, 255, 0), 2); } - // 在图像上绘制最大矩形和中心点 Imgproc.circle(input, rectCenter, 5, new Scalar(0, 255, 0), -1); Imgproc.putText(input, "Distance: " + distance, new Point(10, 30), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); Imgproc.putText(input, "Angle: " + angle, new Point(10, 60), Imgproc.FONT_HERSHEY_SIMPLEX, 0.8, new Scalar(255, 255, 255), 2); @@ -169,9 +134,8 @@ public Mat processFrame(Mat input) { } private boolean isSameRect(RotatedRect rect1, RotatedRect rect2) { - // 检查两个矩形的中心点是否相近以及面积是否相似 - double distanceThreshold = 10.0; // 位置变化阈值 - double areaThreshold = 0.1; // 面积变化百分比阈值 + double distanceThreshold = 10.0; + double areaThreshold = 0.1; double centerDistance = Math.sqrt(Math.pow(rect1.center.x - rect2.center.x, 2) + Math.pow(rect1.center.y - rect2.center.y, 2)); double areaDifference = Math.abs(rect1.size.area() - rect2.size.area()) / rect1.size.area(); return centerDistance < distanceThreshold && areaDifference < areaThreshold; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java index 20007ecc4774..3daaccecaa45 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.test;//测试摄像头距离和角度识别功能 +package org.firstinspires.ftc.teamcode.test; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -12,7 +12,7 @@ public class VisionCameraDistanceTest extends LinearOpMode { public void runOpMode() { robotCameraDistance = new RobotCameraDistance(); - robotCameraDistance.initialize(hardwareMap, "blue"); // 初始化摄像头，并设置颜色参数为红色 + robotCameraDistance.initialize(hardwareMap, "blue"); telemetry.addData("Status", "Initialized"); telemetry.update(); @@ -20,10 +20,12 @@ public void runOpMode() { waitForStart(); while (opModeIsActive()) { - // 获取极坐标 double[] polarCoordinates = robotCameraDistance.getPolarCoordinates(); - // 显示距离和角度检测结果 + if (gamepad1.a) { + robotCameraDistance.updateCurrentRectExcludingCurrent(); + } + telemetry.addData("Distance from Center", polarCoordinates[0]); telemetry.addData("Angle from Center", polarCoordinates[1]); telemetry.update(); From 0285bb1549be4b2c3d2590c200ff7d7a95c728b8 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Fri, 6 Dec 2024 06:38:35 +0800 Subject: [PATCH 84/93] Feat:FIXED RRTest. --- .../java/com/example/meepmeep/MeepTurbo.java | 14 ++++++------ .../drive/SampleMecanumDrive.java | 6 ++--- .../ftc/teamcode/test/RRTest.java | 22 ++++++------------- 3 files changed, 17 insertions(+), 25 deletions(-) diff --git a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java index 8a2c2da45e97..fa154a2f2fc7 100644 --- a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java +++ b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java @@ -20,7 +20,7 @@ public static void main(String[] args) { // Option: Set theme. Default = ColorSchemeRedDark() .setColorScheme(new ColorSchemeRedDark()) .followTrajectorySequence(drive -> - drive.trajectorySequenceBuilder(new Pose2d(-33.53, -65.35, Math.toRadians(15.81))) + drive.trajectorySequenceBuilder(new Pose2d(36.35, -62.84, Math.toRadians(47.22))) // .forward(50) // .turn(Math.toRadians(90)) // .forward(50) @@ -34,12 +34,12 @@ public static void main(String[] args) { // .splineTo(new Vector2d(50, 15), 0) // .turn(Math.toRadians(90)) // .build() - //TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(new Pose2d(13.33, -60.10, Math.toRadians(24.60))) - //TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(new Pose2d(-33.53, -65.35, Math.toRadians(15.81))) - .splineTo(new Vector2d(46.62, -45.21), Math.toRadians(56.31)) - .splineTo(new Vector2d(56.29, 38.97), Math.toRadians(104.04)) - .splineTo(new Vector2d(-25.07, 57.90), Math.toRadians(186.77)) - .build()); + //TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(new Pose2d(36.35, -62.84, Math.toRadians(47.22))) + .splineTo(new Vector2d(60.65, -35.75), Math.toRadians(90.00)) + .splineTo(new Vector2d(61.24, 37.15), Math.toRadians(90.00)) + .splineTo(new Vector2d(35.95, 60.05), Math.toRadians(180.00)) + .build()); + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java index 6f5244983f6f..2ce99bf1251f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java @@ -42,14 +42,14 @@ */ @Config public class SampleMecanumDrive extends MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0.1, 0, 0.1); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0.2, 0.01, 0.15); public static PIDCoefficients HEADING_PID = new PIDCoefficients(0.1, 0, 0.1); - public static double LATERAL_MULTIPLIER = 1.2; + public static double LATERAL_MULTIPLIER = 1.5; public static double VX_WEIGHT = 1; public static double VY_WEIGHT = 1; - public static double OMEGA_WEIGHT = 1; + public static double OMEGA_WEIGHT = 1.3; private TrajectorySequenceRunner trajectorySequenceRunner; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java index 0eec8eee8bcf..bb7327e9bedc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -2,25 +2,24 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.hardware.SparkFunOTOS; +import org.firstinspires.ftc.teamcode.hardware.SparkFunOTOSSensor; @Autonomous public class RRTest extends LinearOpMode { private SampleMecanumDrive drive; - private SparkFunOTOS otosSensor; + private SparkFunOTOSSensor otosSensor; @Override public void runOpMode() throws InterruptedException { // 初始化驱动器和传感器 drive = new SampleMecanumDrive(hardwareMap); - otosSensor = hardwareMap.get(SparkFunOTOS.class, "otos"); // 创建轨迹序列 // TrajectorySequence trajectorySequence = drive.trajectorySequenceBuilder(new Pose2d(0, 0, 90)) // .forward(100) @@ -35,12 +34,11 @@ public void runOpMode() throws InterruptedException { // .splineTo(new Vector2d(50, 15), Math.toRadians(0)) // .turn(Math.toRadians(90)) // .build(); - TrajectorySequence trajectorySequence = drive.trajectorySequenceBuilder(new Pose2d(-33.53, -65.35, Math.toRadians(15.81))) - .splineTo(new Vector2d(46.62, -45.21), Math.toRadians(56.31)) - .splineTo(new Vector2d(56.29, 38.97), Math.toRadians(104.04)) - .splineTo(new Vector2d(-25.07, 57.90), Math.toRadians(186.77)) + TrajectorySequence trajectorySequence = drive.trajectorySequenceBuilder(new Pose2d(36.35, -62.84, Math.toRadians(47.22))) + .splineTo(new Vector2d(60.65, -35.75), Math.toRadians(90.00)) + .splineTo(new Vector2d(61.24, 37.15), Math.toRadians(90.00)) + .splineTo(new Vector2d(35.95, 60.05), Math.toRadians(180.00)) .build(); - // 等待开始指令 waitForStart(); @@ -49,12 +47,6 @@ public void runOpMode() throws InterruptedException { // 执行轨迹移动 drive.followTrajectorySequence(trajectorySequence); - while (opModeIsActive()) { - double distance = otosSensor.getDistance(); - telemetry.addData("Distance", distance); - telemetry.update(); - // 实时调整逻辑 - } } } From 6ecdd736327a716a274c22db4c257fe22a4bf78e Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 6 Dec 2024 23:52:22 +0800 Subject: [PATCH 85/93] the day before the first qualification race --- .../ftc/teamcode/ManualOpMode.java | 106 +++++++---- .../opmode/AutomaticFeedforwardTuner.java | 2 +- .../drive/opmode/StraightTest.java | 2 +- .../advancedManual/AdvancedManual.java | 2 + .../advancedManual/ArmStateMachine.java | 80 +++++++++ .../teamcode/advancedManual/ServoManager.java | 5 +- .../advancedManual/test/ServoMgrTest.java | 100 +++++++++++ .../{ => test}/TimedServoTest.java | 5 +- .../ftc/teamcode/hardware/RobotAuto.java | 170 ++++++++++++++---- .../ftc/teamcode/hardware/RobotChassis.java | 19 +- .../ftc/teamcode/test/AUTOOpModeTest.java | 4 +- .../ftc/teamcode/test/LeftTest.java | 4 +- .../ftc/teamcode/test/MecanumTeleOp.java | 4 +- .../ftc/teamcode/test/RRTest.java | 4 +- .../ftc/teamcode/test/ServoTest.java | 4 +- .../ftc/teamcode/test/ServoTest2.java | 4 +- .../teamcode/test/VisionCameraAngleTest.java | 4 +- .../teamcode/test/VisionCameraColorTest.java | 4 +- .../test/VisionCameraDistanceTest.java | 2 + .../ftc/teamcode/test/WheelOffset.java | 5 +- 20 files changed, 447 insertions(+), 83 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/{ => test}/TimedServoTest.java (84%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index da4acff9e1e8..08048b114374 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -57,14 +57,18 @@ public void runOpMode() { boolean armGrabbing = false; boolean grabbingFlag = false; boolean topServoState = false; - boolean containerRelease = false; + boolean containerRelease = true; + boolean backGrabOpen = false; double recognitionAngle = 0; + double timePoint = 0; LiftState liftState = LiftState.BOTTOM; ArmState armState = ArmState.IDLE; waitForStart(); robotTop.setArmStretchPosition(armStretchPos); robotTop.setArmTurnPosition(armTurnPos); + robotTop.setContainerServoPosition(0.35); + robotTop.setLiftServoPosition(0.2); while (opModeIsActive()) { robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); @@ -91,7 +95,7 @@ public void runOpMode() { if (liftPosition <= LIFT_TOP_POSITION) { robotTop.setLeftPower(0.3); } - }else if(liftState == LiftState.GOING_DOWN){ + } else if (liftState == LiftState.GOING_DOWN) { if (liftPosition <= LIFT_BOTTOM_POSITION) { robotTop.setLeftPower(0); robotTop.setTopServoPosition(0.05); @@ -100,34 +104,56 @@ public void runOpMode() { robotTop.setLeftPower(-0.2); } } - if(gamepad1.y && !previousGamepad1.y){ - if(armState == ArmState.IDLE){ + if (gamepad1.y && !previousGamepad1.y) { + if (armState == ArmState.IDLE) { robotChassis.stopMotor(); armStretchPos = STRETCH_OUT_POSITION; robotTop.setArmStretchPosition(armStretchPos); robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION + 0.2); sleep(1500); armState = ArmState.LOCKED; - }else if(armState == ArmState.LOCKED){ + robotTop.setLeftPower(0.3); + } else if (armState == ArmState.LOCKED) { robotChassis.stopMotor(); sleep(1500); armStretchPos = STRETCH_BACK_POSITION; armState = ArmState.WITHDRAWING; } } - if(gamepad1.left_bumper && !previousGamepad1.left_bumper - && liftState == LiftState.TOP && armState != ArmState.IDLE){ - if(topServoState){ - robotTop.setTopServoPosition(0.05); + if(liftState == LiftState.BOTTOM && armState == ArmState.IDLE){ + if(gamepad1.left_trigger != 0){ + armStretchPos = STRETCH_OUT_POSITION; + robotTop.setArmStretchPosition(armStretchPos); + armState = ArmState.STRETCHED; + sleep(1500); + }if(gamepad1.right_trigger != 0){ + armStretchPos = STRETCH_OUT_POSITION; + robotTop.setArmStretchPosition(armStretchPos); + armState = ArmState.STRETCHED; + sleep(1500); + } + }else{ + if(gamepad1.left_trigger != 0){ + robotTop.setLeftPower(-0.5); + } else if (gamepad1.right_trigger != 0) { + robotTop.setLeftPower(0.5); }else{ + robotTop.setLeftPower(0); + } + } + if (gamepad1.left_bumper && !previousGamepad1.left_bumper + && liftState == LiftState.TOP && armState != ArmState.IDLE) { + if (topServoState) { + robotTop.setTopServoPosition(0.05); + } else { robotTop.setTopServoPosition(0.6); } topServoState = !topServoState; } - if(gamepad1.right_bumper && !previousGamepad1.right_bumper){ - if(containerRelease){ + if (gamepad1.right_bumper && !previousGamepad1.right_bumper) { + if (containerRelease) { robotTop.setContainerServoPosition(1); - }else { + } else { robotTop.setContainerServoPosition(0.35); } containerRelease = !containerRelease; @@ -139,8 +165,7 @@ public void runOpMode() { armStretchPos = STRETCH_OUT_POSITION; armState = ArmState.TURNING_OUT; robotTop.setArmStretchPosition(armStretchPos); - } - else if (armState == ArmState.TURNED) { + } else if (armState == ArmState.TURNED) { armState = ArmState.TURNING_BACK; robotTop.setArmSpinYPosition(SPIN_Y_DEFAULT_POSITION); } @@ -175,6 +200,11 @@ else if (armState == ArmState.TURNED) { robotTop.setArmTurnPosition(armTurnPos); } } + if(armState == ArmState.STRETCHED){ + if(gamepad1.x && !previousGamepad1.x){ + armState = ArmState.WITHDRAWING; + } + } if (gamepad1.b && !previousGamepad1.b) { robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); sleep(500); @@ -187,7 +217,7 @@ else if (armState == ArmState.TURNED) { armGrabbing = false; } } - if(grabbingFlag && !gamepad1.b){ + if (grabbingFlag && !gamepad1.b) { robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); sleep(500); robotTop.setArmTurnPosition(TURN_OUT_HOVERING_POSITION); @@ -210,9 +240,18 @@ else if (armState == ArmState.TURNED) { armSpinYPos = Math.max(0, armSpinYPos - 0.05); robotTop.setArmSpinYPosition(armSpinYPos); } + if(gamepad1.a && !previousGamepad1.a){ + if(backGrabOpen){ + robotTop.setLiftServoPosition(0.2); + }else{ + robotTop.setLiftServoPosition(0.6); + } + backGrabOpen = !backGrabOpen; + } //vision - if(gamepad1.a && !previousGamepad1.a){ + if (getRuntime() - timePoint >= 1) { + timePoint = getRuntime(); recognitionAngle = robotVisionAngle.getDetectedAngle(); robotTop.setArmSpinYPosition(calculateSpinY(recognitionAngle)); } @@ -233,26 +272,27 @@ else if (armState == ArmState.TURNED) { } } -protected double calculateSpinY(double angle) { - if (angle < -180 || angle > 180) { - throw new IllegalArgumentException("Angle must be between -180 and 180 degrees"); - } + protected double calculateSpinY(double angle) { + if (angle < -180 || angle > 180) { + return 0; + } - double adjustedAngle; - if (angle <= 0) { - adjustedAngle = -angle / 360.0 + 0.1; - } else { - adjustedAngle = 0.6 - angle / 360.0; - } + double adjustedAngle; + if (angle <= 0) { + adjustedAngle = -angle / 360.0 + 0.1; + } else { + adjustedAngle = 0.6 - angle / 360.0; + } - return Math.max(0, Math.min(adjustedAngle, 1)); // 确保值在0到1之间 -} + return Math.max(0, Math.min(adjustedAngle, 1)); // 确保值在0到1之间 + } -enum LiftState { - BOTTOM,GOING_UP, TOP, GOING_DOWN -} + enum LiftState { + BOTTOM, GOING_UP, TOP, GOING_DOWN + } -enum ArmState { - IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED + enum ArmState { + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED, STRETCHED + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java index 198b6bd1ddaa..b3a0ec77e2e9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java @@ -32,7 +32,7 @@ * 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear * regression. */ -@Config + @Autonomous(group = "drive") public class AutomaticFeedforwardTuner extends LinearOpMode { public static double MAX_POWER = 0.7; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java index 19e6583991ed..28254cbb5bd6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java @@ -17,7 +17,7 @@ @Config @Autonomous(group = "drive") public class StraightTest extends LinearOpMode { - public static double DISTANCE = 60; // in + public static double DISTANCE = 20; // in @Override public void runOpMode() throws InterruptedException { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java index be607f7c6983..fe0da6a83c64 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java @@ -1,9 +1,11 @@ package org.firstinspires.ftc.teamcode.advancedManual; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; +@Disabled // The ManualOp currently used is a piece of shit. // I'll write a better one here (if the time is enough). public class AdvancedManual extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java new file mode 100644 index 000000000000..ce63e58f36c5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java @@ -0,0 +1,80 @@ +package org.firstinspires.ftc.teamcode.advancedManual; + +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.teamcode.ManualOpMode; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + +public class ArmStateMachine { + enum ArmState{ + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK + } + RobotTop robotTop; + ArmState armState; + Gamepad gamepad1; + Gamepad previousGamepad1; + ServoManager servoMgr; + + final double STRETCH_BACK_POSITION = 0.03; + final double STRETCH_OUT_POSITION = 0.3; + final double SPIN_X_DEFAULT_POSITION = 0.22; + final double SPIN_X_HOVERING_POSITION = 0.53; + final double SPIN_X_DOWN_POSITION = 0.58; + final double SPIN_Y_DEFAULT_POSITION = 0.1; + final double TURN_BACK_POSITION = 0.38; + final double TURN_OUT_HOVERING_POSITION = 0.64; + final double TURN_OUT_DOWN_POSITION = 0.71; + final double GRAB_OPEN_POSITION = 0.2; + final double GRAB_CLOSE_POSITION = 0.9; + + double armStretchPos; + double armTurnPos; + double armSpinXPos; + double armSpinYPos; + public ArmStateMachine(RobotTop robotTop){ + this.robotTop = robotTop; + this.armState = ArmState.IDLE; + this.servoMgr = new ServoManager(); + } + protected void init(){ + int liftPosition; + double armStretchPos = STRETCH_BACK_POSITION; + double armTurnPos = TURN_BACK_POSITION; + double armSpinXPos = SPIN_X_DEFAULT_POSITION; + double armSpinYPos = SPIN_Y_DEFAULT_POSITION; + boolean armGrabbing = false; + boolean grabbingFlag = false; + boolean topServoState = false; + boolean containerRelease = false; + double recognitionAngle = 0; + } + public void receiveGamepad(Gamepad gamepad1, Gamepad previousGamepad1){ + this.gamepad1 = gamepad1; + this.previousGamepad1 = previousGamepad1; + } + + public void update(){ + switch (armState){ + case IDLE: + handleIdleState(); + case TURNING_OUT: + handleTurningOutState(); + case TURNED: + handleTurnedState(); + case TURNING_BACK: + handleTurningBackState(); + case WITHDRAWING: + handleWithdrawingState(); + } + } + protected void handleIdleState(){ + if(gamepad1.x && !previousGamepad1.x){ + robotTop.setArmStretchPosition(armStretchPos); + + } + } + protected void handleTurningOutState(){} + protected void handleTurnedState(){} + protected void handleTurningBackState(){} + protected void handleWithdrawingState(){} +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoManager.java index b45c6d5058eb..8626ea2875fb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoManager.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoManager.java @@ -10,6 +10,7 @@ public class ServoManager { protected Map tasks; + protected Map taskState; public ServoManager() { tasks = new ArrayMap<>(); @@ -21,6 +22,7 @@ public void updateServos(){ ServoTask task = taskEntry.getValue(); if (!task.hasNext()) { task.finish(); + taskState.replace(taskId, false); tasks.remove(taskId, task); continue; } @@ -62,10 +64,11 @@ public boolean hasNext() { } }; } - public void setTimedServoPosition(Servo servo, double position, long timeMs){ + public int setTimedServoPosition(Servo servo, double position, long timeMs){ ServoTask task = timedSetPosition(servo, position, timeMs); int taskId = findMinFreeTaskId(); tasks.put(taskId, task); + return taskId; } private int findMinFreeTaskId() { List taskIdList = tasks.keySet().stream().sorted().collect(Collectors.toList()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java new file mode 100644 index 000000000000..fcc7de77fb6a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java @@ -0,0 +1,100 @@ +package org.firstinspires.ftc.teamcode.advancedManual.test; + +import android.util.ArrayMap; + +import org.firstinspires.ftc.teamcode.advancedManual.ServoTask; + +import java.util.List; +import java.util.Map; +import java.util.stream.Collectors; + +class ServoStimulator{ + double position = 0; + + public void setPosition(double position) { + this.position = position; + System.out.println(this.position); + } + + public double getPosition() { + return position; + } +} + +class ServoMgrTest { + protected Map tasks; + protected Map taskState; + + public ServoMgrTest() { + tasks = new ArrayMap<>(); + } + + public void updateServos(){ + for (Map.Entry taskEntry : tasks.entrySet()) { + int taskId = taskEntry.getKey(); + ServoTask task = taskEntry.getValue(); + if (!task.hasNext()) { + task.finish(); + taskState.replace(taskId, false); + tasks.remove(taskId, task); + continue; + } + task.iterate(); + } + } + + private ServoTask timedSetPosition(ServoStimulator servo, double position, long timeMs) { + final double deltaPos = position - servo.getPosition(); + final long iterationCount = timeMs / ServoTask.TICK_MS; + final double positionPerIteration = deltaPos / iterationCount; + return new ServoTask() { + long remainingIterations = iterationCount; + final double targetPosition = position; + + @Override + public void iterate() { + servo.setPosition(servo.getPosition() + positionPerIteration); + remainingIterations--; + } + + @Override + public void finish() { + servo.setPosition(targetPosition); + } + + @Override + public boolean hasNext() { + return remainingIterations > 0; + } + }; + } + public int setTimedServoPosition(ServoStimulator servo, double position, long timeMs){ + ServoTask task = timedSetPosition(servo, position, timeMs); + int taskId = findMinFreeTaskId(); + tasks.put(taskId, task); + return taskId; + } + private int findMinFreeTaskId() { + List taskIdList = tasks.keySet().stream().sorted().collect(Collectors.toList()); + int maxTaskId = taskIdList.get(taskIdList.size() - 1); + for (int i = 0; i <= maxTaskId; i++) { + if (taskIdList.contains(i)) return 1; + } + return maxTaskId + 1; + } +} + +class Main{ + public static void main(String[] args) { + ServoMgrTest servoMgr = new ServoMgrTest(); + ServoStimulator servo1 = new ServoStimulator(); + ServoStimulator servo2 = new ServoStimulator(); + ServoStimulator servo3 = new ServoStimulator(); + servoMgr.setTimedServoPosition(servo1, 0.7, 1000); + servoMgr.setTimedServoPosition(servo2, 0.2, 5000); + servoMgr.setTimedServoPosition(servo3, 1, 10000); + while (true){ + servoMgr.updateServos(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TimedServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java similarity index 84% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TimedServoTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java index 11c3149593cb..0cc11664a475 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TimedServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java @@ -1,9 +1,12 @@ -package org.firstinspires.ftc.teamcode.advancedManual; +package org.firstinspires.ftc.teamcode.advancedManual.test; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.teamcode.advancedManual.ServoManager; +@Disabled // The ManualOp currently used is a piece of shit. // I'll write a better one here (if the time is enough). public class TimedServoTest extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java index 77a3ea67e4d2..8ff049d8ca1c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java @@ -30,17 +30,51 @@ public RobotAuto(LinearOpMode opMode) { final double COUNTS_PER_INCH = 0; final double P_DRIVE_GAIN = 0; + public double getSteeringCorrection(double desiredHeading, double proportionalGain) { + + // Determine the heading current error + double headingError = desiredHeading - getHeading(); + + // Normalize the error to be within +/- 180 degrees + while (headingError > 180) headingError -= 360; + while (headingError <= -180) headingError += 360; + + // Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0 + return Range.clip(headingError * proportionalGain, -1, 1); + } + /** + * Read the robot heading directly from the IMU. + * + * @return The heading of the robot in degrees. + */ + public double getHeading() { + return getHeading(AngleUnit.DEGREES); + } + /** + * read the Robot heading directly from the IMU + * + * @param unit The desired angle unit (degrees or radians) + * @return The heading of the robot in desired units. + */ + public double getHeading(AngleUnit unit) { + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + telemetry.addData("Yaw/Pitch/Roll", orientation.toString()); + telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); + telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); + telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); + return orientation.getYaw(unit); + } + public RobotAuto driveStraight(double maxDriveSpeed, double distance, double heading) { // Ensure that the OpMode is still active if (opMode.opModeIsActive()) { - double turnSpeed; // Determine new target position, and pass to motor controller int moveCounts = (int) (distance * COUNTS_PER_INCH); - robotChassis.setTargetPosition(moveCounts); + setStraightTargetPosition(moveCounts); robotChassis.setRunMode(DcMotor.RunMode.RUN_TO_POSITION); @@ -53,7 +87,7 @@ public RobotAuto driveStraight(double maxDriveSpeed, while (opMode.opModeIsActive() && robotChassis.isAllBusy()) { // Determine required steering to keep on heading - turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN); + double turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN); // if driving in reverse, the motor correction also needs to be reversed if (distance < 0) @@ -71,39 +105,113 @@ public RobotAuto driveStraight(double maxDriveSpeed, } return this; } - public double getSteeringCorrection(double desiredHeading, double proportionalGain) { - // Determine the heading current error - double headingError = desiredHeading - getHeading(); + public RobotAuto driveStrafe(double maxDriveSpeed, + double distance, + double heading + ) { - // Normalize the error to be within +/- 180 degrees - while (headingError > 180) headingError -= 360; - while (headingError <= -180) headingError += 360; + // Ensure that the OpMode is still active + if (opMode.opModeIsActive()) { - // Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0 - return Range.clip(headingError * proportionalGain, -1, 1); + // Determine new target position, and pass to motor controller + int moveCounts = (int) (distance * COUNTS_PER_INCH); + setStrafeTargetPosition(moveCounts); + + robotChassis.setRunMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set the required driving speed (must be positive for RUN_TO_POSITION) + // Start driving straight, and then enter the control loop + maxDriveSpeed = Math.abs(maxDriveSpeed); + robotChassis.driveRobot(0, maxDriveSpeed, 0); + + // keep looping while we are still active, and BOTH motors are running. + while (opMode.opModeIsActive() && robotChassis.isAllBusy()) { + + // Determine required steering to keep on heading + double turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN); + + // if driving in reverse, the motor correction also needs to be reversed + if (distance < 0) + turnSpeed *= -1.0; + + // Apply the turning correction to the current driving speed. + robotChassis.driveRobot(0, maxDriveSpeed, -turnSpeed); + // telemetry.addData("x","%4.2f, %4.2f, %4.2f, %4.2f, %4d",maxDriveSpeed,distance,heading,turnSpeed,moveCounts); + telemetry.update(); + } + + // Stop all motion & Turn off RUN_TO_POSITION + robotChassis.stopMotor(); + robotChassis.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + return this; } - /** - * Read the robot heading directly from the IMU. - * - * @return The heading of the robot in degrees. - */ - public double getHeading() { - return getHeading(AngleUnit.DEGREES); + public void setStraightTargetPosition(int moveCounts) { + int[] motorPos = robotChassis.getTargetPosition(); + for (int i = 0; i < motorPos.length; i++) { + motorPos[i] += moveCounts; + } + robotChassis.setTargetPosition(motorPos); } - /** - * read the Robot heading directly from the IMU - * - * @param unit The desired angle unit (degrees or radians) - * @return The heading of the robot in desired units. - */ - public double getHeading(AngleUnit unit) { - YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); - telemetry.addData("Yaw/Pitch/Roll", orientation.toString()); - telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); - telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); - telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); - return orientation.getYaw(unit); + public void setStrafeTargetPosition(int moveCounts) { + int[] motorPos = robotChassis.getTargetPosition(); + motorPos[0] = motorPos[0] + moveCounts; + motorPos[1] = motorPos[1] - moveCounts; + motorPos[2] = motorPos[2] - moveCounts; + motorPos[3] = motorPos[3] + moveCounts; + robotChassis.setTargetPosition(motorPos); + } + public RobotAuto strafeToPosition(double targetX, double targetY, double maxSpeed, double heading) { + // Ensure the OpMode is still active + if (opMode.opModeIsActive()) { + // 获取当前坐标（需要提供机器人当前的位置，可以由传感器或编码器计算得到） + double currentX = getCurrentX(); + double currentY = getCurrentY(); + + // 计算目标位置与当前坐标的差值 + double deltaX = targetX - currentX; + double deltaY = targetY - currentY; + + // 计算移动的总距离 + double distance = Math.hypot(deltaX, deltaY); + + // 计算移动的方向角（相对于正前方） + double angle = Math.atan2(deltaY, deltaX); + + // 转换方向角为机器人坐标系的方向（可能需要调整角度） + double robotAngle = angle - Math.toRadians(getHeading()); + + // 将方向和速度分解为麦轮需要的前后左右分量 + double strafeX = Math.cos(robotAngle) * maxSpeed; + double strafeY = Math.sin(robotAngle) * maxSpeed; + + // 设置目标位置并启动移动 + setStraightTargetPosition((int) (distance * COUNTS_PER_INCH)); + robotChassis.setRunMode(DcMotor.RunMode.RUN_TO_POSITION); + + while (opMode.opModeIsActive() && robotChassis.isAllBusy()) { + // 调整麦轮的速度分量并修正方向 + double turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN); + robotChassis.driveRobot(strafeY, strafeX, -turnSpeed); + telemetry.addData("Target", "X: %.2f, Y: %.2f", targetX, targetY); + telemetry.addData("Current", "X: %.2f, Y: %.2f", currentX, currentY); + telemetry.update(); + } + + // 停止所有运动并重置模式 + robotChassis.stopMotor(); + robotChassis.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + return this; + } + public double getCurrentX() { + // 根据编码器的读数计算当前X坐标 + return 0; // 示例返回值，需要实际实现 } + public double getCurrentY() { + // 根据编码器的读数计算当前Y坐标 + return 0; // 示例返回值，需要实际实现 + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java index f99814a82b44..34892e780196 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java @@ -130,12 +130,21 @@ public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior behavior) { * * @param moveCounts The desired increment/decrement. */ - public void setTargetPosition(int moveCounts) { + public void setTargetPosition(int[] moveCounts) { // Set Target FIRST, then turn on RUN_TO_POSITION - leftFrontDrive.setTargetPosition(leftFrontDrive.getCurrentPosition() + moveCounts); - rightFrontDrive.setTargetPosition(rightFrontDrive.getCurrentPosition() + moveCounts); - leftBackDrive.setTargetPosition(leftBackDrive.getCurrentPosition() + moveCounts); - rightBackDrive.setTargetPosition(rightBackDrive.getCurrentPosition() + moveCounts); + leftFrontDrive.setTargetPosition(moveCounts[0]); + rightFrontDrive.setTargetPosition(moveCounts[1]); + leftBackDrive.setTargetPosition(moveCounts[2]); + rightBackDrive.setTargetPosition(moveCounts[3]); + } + + public int[] getTargetPosition() { + // Set Target FIRST, then turn on RUN_TO_POSITION + int lf = leftFrontDrive.getTargetPosition(); + int rf = rightFrontDrive.getTargetPosition(); + int lb = leftBackDrive.getTargetPosition(); + int rb = rightBackDrive.getTargetPosition(); + return new int[]{lf, lb, lb, rb}; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AUTOOpModeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AUTOOpModeTest.java index c714fecda2bf..e4b49a376e20 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AUTOOpModeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AUTOOpModeTest.java @@ -1,10 +1,12 @@ package org.firstinspires.ftc.teamcode.test; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; -@Autonomous +@Disabled +@Autonomous(group = "Test") public class AUTOOpModeTest extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/LeftTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/LeftTest.java index a87ec37baa26..3ad5f2cc462f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/LeftTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/LeftTest.java @@ -1,11 +1,13 @@ package org.firstinspires.ftc.teamcode.test; +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.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; -@TeleOp +@Disabled +@TeleOp(group = "Test") public class LeftTest extends LinearOpMode { @Override public void runOpMode(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MecanumTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MecanumTeleOp.java index 927af3702ebf..424872db219a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MecanumTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MecanumTeleOp.java @@ -1,11 +1,13 @@ package org.firstinspires.ftc.teamcode.test; +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.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; -@TeleOp +@Disabled +@TeleOp(group = "Test") public class MecanumTeleOp extends LinearOpMode { @Override public void runOpMode() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java index bb7327e9bedc..29363650776d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -4,13 +4,15 @@ import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.hardware.sparkfun.SparkFunOTOS; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; import org.firstinspires.ftc.teamcode.hardware.SparkFunOTOSSensor; -@Autonomous +@Disabled +@Autonomous(group = "Test") public class RRTest extends LinearOpMode { private SampleMecanumDrive drive; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java index bd05f52f59ba..6f0a70a0a1a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest.java @@ -1,11 +1,13 @@ package org.firstinspires.ftc.teamcode.test; +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.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; -@TeleOp +@Disabled +@TeleOp(group = "Test") public class ServoTest extends LinearOpMode { @Override public void runOpMode(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest2.java index bf5e67b81871..1a3cfab2d361 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ServoTest2.java @@ -1,11 +1,13 @@ package org.firstinspires.ftc.teamcode.test; +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.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; -@TeleOp +@Disabled +@TeleOp(group = "Test") public class ServoTest2 extends LinearOpMode { @Override public void runOpMode(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java index cad1a4ba3e5b..29f4065763a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraAngleTest.java @@ -1,10 +1,12 @@ package org.firstinspires.ftc.teamcode.test;//测试摄像头角度识别功能 +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.hardware.RobotVision.RobotVisionAngle; -@TeleOp +@Disabled +@TeleOp(group = "Test") public class VisionCameraAngleTest extends LinearOpMode { private RobotVisionAngle robotVisionAngle; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraColorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraColorTest.java index db56f168d3ef..22a21a4a9348 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraColorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraColorTest.java @@ -1,9 +1,11 @@ package org.firstinspires.ftc.teamcode.test;//测试摄像头颜色识别功能 +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.hardware.RobotVision.RobotVisionColor; -@TeleOp +@Disabled +@TeleOp(group = "Test") public class VisionCameraColorTest extends LinearOpMode { private RobotVisionColor robotVisionColor; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java index 20007ecc4774..703210351570 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/VisionCameraDistanceTest.java @@ -1,9 +1,11 @@ package org.firstinspires.ftc.teamcode.test;//测试摄像头距离和角度识别功能 +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.hardware.RobotVision.RobotCameraDistance; +@Disabled @TeleOp(name = "VisionCameraDistanceTest", group = "Test") public class VisionCameraDistanceTest extends LinearOpMode { private RobotCameraDistance robotCameraDistance; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java index 554b5e081814..b9a022769ea6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java @@ -1,11 +1,12 @@ package org.firstinspires.ftc.teamcode.test; +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.hardware.DcMotor; - -@TeleOp +@Disabled +@TeleOp(group = "Test") public class WheelOffset extends LinearOpMode { @Override public void runOpMode(){ From 73b9606e058d4e0944cc473c0a52c09e8e611e31 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sat, 7 Dec 2024 00:52:55 +0800 Subject: [PATCH 86/93] the day before the first qualification race --- .../ftc/teamcode/ManualOpMode.java | 8 +- .../ftc/teamcode/hardware/RobotAuto.java | 176 +++++++++++++++++- 2 files changed, 180 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 08048b114374..0da6492be2d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -75,9 +75,8 @@ public void runOpMode() { // robotLift liftPosition = robotTop.getLiftPosition(); if (liftState == LiftState.BOTTOM) { - if (gamepad1.y && liftPosition <= LIFT_TOP_POSITION - 150) { + if (gamepad1.y) { robotTop.setTopServoPosition(0.05); - robotTop.setLeftPower(0.7); liftState = LiftState.GOING_UP; } } else if (liftState == LiftState.GOING_UP) { @@ -86,6 +85,9 @@ public void runOpMode() { liftState = LiftState.TOP; } else if (liftPosition >= LIFT_TOP_POSITION - 150) { robotTop.setLeftPower(0.3); + } else if (liftPosition <= LIFT_TOP_POSITION -150) { + //TODO + robotTop.setLeftPower(0.5); } } else if (liftState == LiftState.TOP) { if (gamepad1.y && liftPosition >= LIFT_BOTTOM_POSITION + 150) { @@ -112,7 +114,7 @@ public void runOpMode() { robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION + 0.2); sleep(1500); armState = ArmState.LOCKED; - robotTop.setLeftPower(0.3); + liftState = LiftState.GOING_UP; } else if (armState == ArmState.LOCKED) { robotChassis.stopMotor(); sleep(1500); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java index 8ff049d8ca1c..d50d546c1e50 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.hardware; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; @@ -7,6 +9,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import com.qualcomm.robotcore.util.Range; @@ -17,23 +20,40 @@ public class RobotAuto { Telemetry telemetry; RobotChassis robotChassis; RobotTop robotTop; + private double headingError = 0; + @SuppressWarnings("FieldCanBeLocal") + private double targetHeading = 0; + @SuppressWarnings("FieldMayBeFinal") + private double driveSpeed = 0; + private double turnSpeed = 0; IMU imu; + SparkFunOTOS otos; + RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.FORWARD; + RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.LEFT; + RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection); public RobotAuto(LinearOpMode opMode) { this.opMode = opMode; hardwareMap = opMode.hardwareMap; telemetry = opMode.telemetry; this.robotChassis = new RobotChassis(opMode); this.robotTop = new RobotTop(opMode); + otos = opMode.hardwareMap.get(SparkFunOTOS.class, "otos"); + configureOtos(); + imu = opMode.hardwareMap.get(IMU.class, "imu"); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + imu.resetYaw(); } final double COUNTS_PER_INCH = 0; final double P_DRIVE_GAIN = 0; + static final double HEADING_THRESHOLD = 0.5; + static final double P_TURN_GAIN = 0.1; public double getSteeringCorrection(double desiredHeading, double proportionalGain) { // Determine the heading current error - double headingError = desiredHeading - getHeading(); + headingError = desiredHeading - getHeading(); // Normalize the error to be within +/- 180 degrees while (headingError > 180) headingError -= 360; @@ -162,6 +182,160 @@ public void setStrafeTargetPosition(int moveCounts) { motorPos[3] = motorPos[3] + moveCounts; robotChassis.setTargetPosition(motorPos); } + public RobotAuto turnToHeading(double maxTurnSpeed, double heading) { + + // Run getSteeringCorrection() once to pre-calculate the current error + getSteeringCorrection(heading, P_DRIVE_GAIN); + + // keep looping while we are still active, and not on heading. + //绝对值的问题？ + while (opMode.opModeIsActive() && (Math.abs(headingError) > HEADING_THRESHOLD)) { + + // Determine required steering to keep on heading + turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN); + + // Clip the speed to the maximum permitted value. + turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed); + + // Pivot in place by applying the turning correction + robotChassis.driveRobot(0, 0, -turnSpeed); + // telemetry.addData("x","%4.2f, %4.2f, %4.2f, %4.2f",maxTurnSpeed,turnSpeed,heading,getHeading()); + telemetry.update(); + } + + // Stop all motion; + robotChassis.stopMotor(); + return this; + } + + private void configureOtos() { + otos.setLinearUnit(DistanceUnit.INCH); + otos.setAngularUnit(AngleUnit.DEGREES); + SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0, 0, 0); + otos.setOffset(offset); + + otos.setLinearScalar(1.0); + otos.setAngularScalar(1.0); + + otos.calibrateImu(); + + otos.resetTracking(); + + SparkFunOTOS.Pose2D currentPosition = new SparkFunOTOS.Pose2D(0, 0, 0); + otos.setPosition(currentPosition); + + // Get the hardware and firmware version + SparkFunOTOS.Version hwVersion = new SparkFunOTOS.Version(); + SparkFunOTOS.Version fwVersion = new SparkFunOTOS.Version(); + otos.getVersionInfo(hwVersion, fwVersion); + + telemetry.addLine("OTOS configured! Press start to get position data!"); + telemetry.addLine(); + telemetry.addLine(String.format("OTOS Hardware Version: v%d.%d", hwVersion.major, hwVersion.minor)); + telemetry.addLine(String.format("OTOS Firmware Version: v%d.%d", fwVersion.major, fwVersion.minor)); + telemetry.update(); + } + + public SparkFunOTOS.Pose2D getPosition() { + return otos.getPosition(); + } + + public RobotAuto forward(double d) { + return driveStraight(0.6, d, getHeading()); + } + + public RobotAuto fastForward(double d) { + return driveStraight(0.9, d, getHeading()); + } + + + public RobotAuto backward(double d) { + return driveStraight(0.6, -d, getHeading()); + } + + public RobotAuto fastBackward(double d) { + return driveStraight(0.9, -d, getHeading()); + } + + public RobotAuto rightShift(double d) { + return driveStrafe(0.9, d, getHeading()); + } + + public RobotAuto leftShift(double d) { + return driveStrafe(0.9, -d, getHeading()); + } + + public RobotAuto spin(double h) { + return turnToHeading(0.6, h); + } + + public RobotAuto fastSpin(double h) { + return turnToHeading(0.7, h); + } + + public RobotAuto sleep(long milliseconds) { + opMode.sleep(milliseconds); + return this; + } + + protected double[] SpinVector(double[] vector, double angle) { + double x = vector[0] * Math.cos(Math.toRadians(angle)) - vector[1] * Math.sin(Math.toRadians(angle)); + double y = vector[0] * Math.sin(Math.toRadians(angle)) + vector[1] * Math.cos(Math.toRadians(angle)); + return new double[]{x, y}; + } + protected double[] getDisplacement(double[] CurrentPos, double[] DesiredPos) { + double CurrentX = CurrentPos[0]; + double CurrentY = CurrentPos[1]; + double CurrentHeading = CurrentPos[2]; + double DesiredX = DesiredPos[0]; + double DesiredY = DesiredPos[1]; + double DesiredHeading = DesiredPos[2]; + double[] Displacement = {DesiredX - CurrentX, DesiredY - CurrentY, CurrentHeading}; + Displacement = SpinVector(Displacement, -CurrentHeading); + return Displacement; + } + /** + * Go to the position given (track only include left/right and forward/backward) + * Go forward/backward first,then move left/right. + * + * @param CurrentPos The current position.(position{axial,lateral,heading}) + * @param DesiredPos The desired position.(position{axial,lateral,heading}) + * @return RobotHardware class. + */ + public RobotAuto gotoPosition(double[] CurrentPos, double[] DesiredPos) { + double[] Displacement = getDisplacement(CurrentPos, DesiredPos); + double DesiredHeading = DesiredPos[2]; + return fastForward(-Displacement[0]) + .leftShift(-Displacement[1]) + .fastSpin(DesiredHeading); + } + public RobotAuto gotoPosition(double x, double y, double h) { + SparkFunOTOS.Pose2D CurrentPos = getPosition(); + double[] DesiredPos = {x, y, h}; + return gotoPosition(new double[]{CurrentPos.x, CurrentPos.y, CurrentPos.h}, DesiredPos); + } + + /** + * Go to the position given (track only include left/right and forward/backward) + * Move left/right first,then go forward/backward. + * + * @param CurrentPos The current position.(position{axial,lateral,heading}) + * @param DesiredPos The desired position.(position{axial,lateral,heading}) + * @return RobotHardware class. + */ + public RobotAuto gotoPosition2(double[] CurrentPos, double[] DesiredPos) { + double[] Displacement = getDisplacement(CurrentPos, DesiredPos); + double DesiredHeading = DesiredPos[2]; + return leftShift(-Displacement[1]) + .fastForward(-Displacement[0]) + .fastSpin(DesiredHeading); + } + public RobotAuto gotoPosition2(double x, double y, double h) { + SparkFunOTOS.Pose2D CurrentPos = getPosition(); + double[] DesiredPos = {x, y, h}; + return gotoPosition2(new double[]{CurrentPos.x, CurrentPos.y, CurrentPos.h}, DesiredPos); + } + public RobotAuto strafeToPosition(double targetX, double targetY, double maxSpeed, double heading) { // Ensure the OpMode is still active if (opMode.opModeIsActive()) { From 72b1cd01208274ff38a0db68c4f72b5cba78f79b Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sat, 7 Dec 2024 03:41:28 +0800 Subject: [PATCH 87/93] Feat:Unified --- .../java/com/example/meepmeep/MeepTurbo.java | 10 +- .../ftc/teamcode/ManualOpMode.java | 113 ++++++------------ .../drive/DriveConstants.java | 8 +- .../drive/SampleMecanumDrive.java | 4 +- .../ftc/teamcode/test/RRTest.java | 12 +- 5 files changed, 55 insertions(+), 92 deletions(-) diff --git a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java index fa154a2f2fc7..e851a14aeeb6 100644 --- a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java +++ b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java @@ -20,7 +20,7 @@ public static void main(String[] args) { // Option: Set theme. Default = ColorSchemeRedDark() .setColorScheme(new ColorSchemeRedDark()) .followTrajectorySequence(drive -> - drive.trajectorySequenceBuilder(new Pose2d(36.35, -62.84, Math.toRadians(47.22))) + drive.trajectorySequenceBuilder(new Pose2d(59.71, 36.96, Math.toRadians(153.15))) // .forward(50) // .turn(Math.toRadians(90)) // .forward(50) @@ -34,17 +34,15 @@ public static void main(String[] args) { // .splineTo(new Vector2d(50, 15), 0) // .turn(Math.toRadians(90)) // .build() - //TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(new Pose2d(36.35, -62.84, Math.toRadians(47.22))) - .splineTo(new Vector2d(60.65, -35.75), Math.toRadians(90.00)) - .splineTo(new Vector2d(61.24, 37.15), Math.toRadians(90.00)) - .splineTo(new Vector2d(35.95, 60.05), Math.toRadians(180.00)) + //TrajectorySequence trajectory0 = drive.trajectorySequenceBuilder(new Pose2d(59.71, 36.96, Math.toRadians(153.15))) + .splineTo(new Vector2d(10.98, 59.92), Math.toRadians(180.00)) + .splineTo(new Vector2d(-36.35, 37.36), Math.toRadians(223.03)) .build()); - // Set field image meepMeep.setBackground(MeepMeep.Background.FIELD_FREIGHTFRENZY_ADI_DARK) .setDarkMode(true) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 0da6492be2d1..9e5baad49f76 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -57,26 +57,23 @@ public void runOpMode() { boolean armGrabbing = false; boolean grabbingFlag = false; boolean topServoState = false; - boolean containerRelease = true; - boolean backGrabOpen = false; + boolean containerRelease = false; double recognitionAngle = 0; - double timePoint = 0; LiftState liftState = LiftState.BOTTOM; ArmState armState = ArmState.IDLE; waitForStart(); robotTop.setArmStretchPosition(armStretchPos); robotTop.setArmTurnPosition(armTurnPos); - robotTop.setContainerServoPosition(0.35); - robotTop.setLiftServoPosition(0.2); while (opModeIsActive()) { robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); // robotLift liftPosition = robotTop.getLiftPosition(); if (liftState == LiftState.BOTTOM) { - if (gamepad1.y) { + if (gamepad1.y && liftPosition <= LIFT_TOP_POSITION - 150) { robotTop.setTopServoPosition(0.05); + robotTop.setLeftPower(0.7); liftState = LiftState.GOING_UP; } } else if (liftState == LiftState.GOING_UP) { @@ -85,9 +82,6 @@ public void runOpMode() { liftState = LiftState.TOP; } else if (liftPosition >= LIFT_TOP_POSITION - 150) { robotTop.setLeftPower(0.3); - } else if (liftPosition <= LIFT_TOP_POSITION -150) { - //TODO - robotTop.setLeftPower(0.5); } } else if (liftState == LiftState.TOP) { if (gamepad1.y && liftPosition >= LIFT_BOTTOM_POSITION + 150) { @@ -97,7 +91,7 @@ public void runOpMode() { if (liftPosition <= LIFT_TOP_POSITION) { robotTop.setLeftPower(0.3); } - } else if (liftState == LiftState.GOING_DOWN) { + }else if(liftState == LiftState.GOING_DOWN){ if (liftPosition <= LIFT_BOTTOM_POSITION) { robotTop.setLeftPower(0); robotTop.setTopServoPosition(0.05); @@ -106,56 +100,34 @@ public void runOpMode() { robotTop.setLeftPower(-0.2); } } - if (gamepad1.y && !previousGamepad1.y) { - if (armState == ArmState.IDLE) { + if(gamepad1.y && !previousGamepad1.y){ + if(armState == ArmState.IDLE){ robotChassis.stopMotor(); armStretchPos = STRETCH_OUT_POSITION; robotTop.setArmStretchPosition(armStretchPos); robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION + 0.2); sleep(1500); armState = ArmState.LOCKED; - liftState = LiftState.GOING_UP; - } else if (armState == ArmState.LOCKED) { + }else if(armState == ArmState.LOCKED){ robotChassis.stopMotor(); sleep(1500); armStretchPos = STRETCH_BACK_POSITION; armState = ArmState.WITHDRAWING; } } - if(liftState == LiftState.BOTTOM && armState == ArmState.IDLE){ - if(gamepad1.left_trigger != 0){ - armStretchPos = STRETCH_OUT_POSITION; - robotTop.setArmStretchPosition(armStretchPos); - armState = ArmState.STRETCHED; - sleep(1500); - }if(gamepad1.right_trigger != 0){ - armStretchPos = STRETCH_OUT_POSITION; - robotTop.setArmStretchPosition(armStretchPos); - armState = ArmState.STRETCHED; - sleep(1500); - } - }else{ - if(gamepad1.left_trigger != 0){ - robotTop.setLeftPower(-0.5); - } else if (gamepad1.right_trigger != 0) { - robotTop.setLeftPower(0.5); - }else{ - robotTop.setLeftPower(0); - } - } - if (gamepad1.left_bumper && !previousGamepad1.left_bumper - && liftState == LiftState.TOP && armState != ArmState.IDLE) { - if (topServoState) { + if(gamepad1.left_bumper && !previousGamepad1.left_bumper + && liftState == LiftState.TOP && armState != ArmState.IDLE){ + if(topServoState){ robotTop.setTopServoPosition(0.05); - } else { + }else{ robotTop.setTopServoPosition(0.6); } topServoState = !topServoState; } - if (gamepad1.right_bumper && !previousGamepad1.right_bumper) { - if (containerRelease) { + if(gamepad1.right_bumper && !previousGamepad1.right_bumper){ + if(containerRelease){ robotTop.setContainerServoPosition(1); - } else { + }else { robotTop.setContainerServoPosition(0.35); } containerRelease = !containerRelease; @@ -167,7 +139,8 @@ public void runOpMode() { armStretchPos = STRETCH_OUT_POSITION; armState = ArmState.TURNING_OUT; robotTop.setArmStretchPosition(armStretchPos); - } else if (armState == ArmState.TURNED) { + } + else if (armState == ArmState.TURNED) { armState = ArmState.TURNING_BACK; robotTop.setArmSpinYPosition(SPIN_Y_DEFAULT_POSITION); } @@ -202,11 +175,6 @@ public void runOpMode() { robotTop.setArmTurnPosition(armTurnPos); } } - if(armState == ArmState.STRETCHED){ - if(gamepad1.x && !previousGamepad1.x){ - armState = ArmState.WITHDRAWING; - } - } if (gamepad1.b && !previousGamepad1.b) { robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); sleep(500); @@ -219,7 +187,7 @@ public void runOpMode() { armGrabbing = false; } } - if (grabbingFlag && !gamepad1.b) { + if(grabbingFlag && !gamepad1.b){ robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); sleep(500); robotTop.setArmTurnPosition(TURN_OUT_HOVERING_POSITION); @@ -242,18 +210,9 @@ public void runOpMode() { armSpinYPos = Math.max(0, armSpinYPos - 0.05); robotTop.setArmSpinYPosition(armSpinYPos); } - if(gamepad1.a && !previousGamepad1.a){ - if(backGrabOpen){ - robotTop.setLiftServoPosition(0.2); - }else{ - robotTop.setLiftServoPosition(0.6); - } - backGrabOpen = !backGrabOpen; - } //vision - if (getRuntime() - timePoint >= 1) { - timePoint = getRuntime(); + if(gamepad1.a && !previousGamepad1.a){ recognitionAngle = robotVisionAngle.getDetectedAngle(); robotTop.setArmSpinYPosition(calculateSpinY(recognitionAngle)); } @@ -274,27 +233,27 @@ public void runOpMode() { } } - protected double calculateSpinY(double angle) { - if (angle < -180 || angle > 180) { - return 0; - } - - double adjustedAngle; - if (angle <= 0) { - adjustedAngle = -angle / 360.0 + 0.1; - } else { - adjustedAngle = 0.6 - angle / 360.0; - } +protected double calculateSpinY(double angle) { + if (angle < -180 || angle > 180) { + throw new IllegalArgumentException("Angle must be between -180 and 180 degrees"); + } - return Math.max(0, Math.min(adjustedAngle, 1)); // 确保值在0到1之间 + double adjustedAngle; + if (angle <= 0) { + adjustedAngle = -angle / 360.0 + 0.1; + } else { + adjustedAngle = 0.6 - angle / 360.0; } + return Math.max(0, Math.min(adjustedAngle, 1)); // 确保值在0到1之间 +} - enum LiftState { - BOTTOM, GOING_UP, TOP, GOING_DOWN - } - enum ArmState { - IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED, STRETCHED - } +enum LiftState { + BOTTOM,GOING_UP, TOP, GOING_DOWN +} + +enum ArmState { + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED +} } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java index 951450904a8a..5608ac17e836 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java @@ -45,7 +45,9 @@ public class DriveConstants { * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ public static double WHEEL_RADIUS = 3; // in (轮子半径 单位：英寸) - public static double GEAR_RATIO = 1.0/15.0; // (齿轮比) output (wheel) speed / input (motor) speed + public static double GEAR_RATIO = 1.0/17.515 + + ; // (齿轮比) output (wheel) speed / input (motor) speed public static double TRACK_WIDTH = 339/2.54; // (轮距 单位：英寸) in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using @@ -65,8 +67,8 @@ public class DriveConstants { * inches. */ //TODO:ADJUST THESE NUMBERS. - public static double MAX_VEL = 60; - public static double MAX_ACCEL = 60; + public static double MAX_VEL = 50; + public static double MAX_ACCEL = 30; public static double MAX_ANG_VEL = Math.toRadians(180); public static double MAX_ANG_ACCEL = Math.toRadians(180); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java index 2ce99bf1251f..67ce1abaf000 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java @@ -42,10 +42,10 @@ */ @Config public class SampleMecanumDrive extends MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0.2, 0.01, 0.15); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0.1, 0.01, 0.1); public static PIDCoefficients HEADING_PID = new PIDCoefficients(0.1, 0, 0.1); - public static double LATERAL_MULTIPLIER = 1.5; + public static double LATERAL_MULTIPLIER = 0.357124; public static double VX_WEIGHT = 1; public static double VY_WEIGHT = 1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java index 29363650776d..084bfec67f0e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java @@ -36,11 +36,15 @@ public void runOpMode() throws InterruptedException { // .splineTo(new Vector2d(50, 15), Math.toRadians(0)) // .turn(Math.toRadians(90)) // .build(); - TrajectorySequence trajectorySequence = drive.trajectorySequenceBuilder(new Pose2d(36.35, -62.84, Math.toRadians(47.22))) - .splineTo(new Vector2d(60.65, -35.75), Math.toRadians(90.00)) - .splineTo(new Vector2d(61.24, 37.15), Math.toRadians(90.00)) - .splineTo(new Vector2d(35.95, 60.05), Math.toRadians(180.00)) + TrajectorySequence trajectorySequence = drive.trajectorySequenceBuilder(new Pose2d(35.75, -58.91, Math.toRadians(56.65))) + .splineTo(new Vector2d(61.33, -19.84), Math.toRadians(91.97)) + .splineTo(new Vector2d(54.08, 43.80), Math.toRadians(116.57)) + .splineTo(new Vector2d(-6.95, 53.87), Math.toRadians(189.13)) + .splineTo(new Vector2d(-49.44, 28.70), Math.toRadians(243.43)) + .splineTo(new Vector2d(-45.62, -36.15), Math.toRadians(-58.39)) .build(); + + // 等待开始指令 waitForStart(); From 1c32dbf07250edcb9fe71c835ff1dda17def7053 Mon Sep 17 00:00:00 2001 From: Ehicy Date: Sat, 7 Dec 2024 04:08:17 +0800 Subject: [PATCH 88/93] fix the strange screen presentation --- .../hardware/RobotVision/RobotVisionAngle.java | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java index 40aa2acad299..fc51c352b946 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotVision/RobotVisionAngle.java @@ -51,13 +51,22 @@ private class CenterAnglePipeline extends OpenCvPipeline { @Override public Mat processFrame(Mat input) { Imgproc.cvtColor(input, gray, Imgproc.COLOR_RGB2GRAY); + + // 应用高斯模糊减少噪声 Imgproc.GaussianBlur(gray, blurred, new Size(5, 5), 0); + + // 增强图像锐度 kernel.put(0, 0, 0, -1, 0); kernel.put(1, 0, -1, 5, -1); kernel.put(2, 0, 0, -1, 0); Imgproc.filter2D(blurred, sharp, gray.depth(), kernel); + + // 优化亮度和对比度 + Mat brightened = new Mat(); + sharp.convertTo(brightened, -1, 1.2, 10); + Rect centerRect = new Rect(input.width() / 8, input.height() / 8, input.width() * 3 / 4, input.height() * 3 / 4); - Mat centerMat = sharp.submat(centerRect); + Mat centerMat = brightened.submat(centerRect); Imgproc.Canny(centerMat, edges, 50, 150); Imgproc.HoughLinesP(edges, lines, 1, Math.PI / 180, 50, 50, 10); From 6ff60957c975723521a976268123d41670f9979b Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sat, 7 Dec 2024 04:19:43 +0800 Subject: [PATCH 89/93] try to solve the mis-record of a missing '}' in Github --- .../ftc/teamcode/ManualOpMode.java | 38 ++++++------ .../ftc/teamcode/hardware/RobotAuto.java | 60 ++++--------------- 2 files changed, 31 insertions(+), 67 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 0da6492be2d1..84bc1e56095d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -48,6 +48,8 @@ public void runOpMode() { robotVisionAngle.initialize(hardwareMap); Gamepad previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); + Gamepad previousGamepad2 = new Gamepad(); + previousGamepad2.copy(gamepad2); int liftPosition; double armStretchPos = STRETCH_BACK_POSITION; @@ -60,7 +62,6 @@ public void runOpMode() { boolean containerRelease = true; boolean backGrabOpen = false; double recognitionAngle = 0; - double timePoint = 0; LiftState liftState = LiftState.BOTTOM; ArmState armState = ArmState.IDLE; @@ -85,9 +86,9 @@ public void runOpMode() { liftState = LiftState.TOP; } else if (liftPosition >= LIFT_TOP_POSITION - 150) { robotTop.setLeftPower(0.3); - } else if (liftPosition <= LIFT_TOP_POSITION -150) { + } else if (liftPosition <= LIFT_TOP_POSITION - 150) { //TODO - robotTop.setLeftPower(0.5); + robotTop.setLeftPower(0.3); } } else if (liftState == LiftState.TOP) { if (gamepad1.y && liftPosition >= LIFT_BOTTOM_POSITION + 150) { @@ -122,24 +123,25 @@ public void runOpMode() { armState = ArmState.WITHDRAWING; } } - if(liftState == LiftState.BOTTOM && armState == ArmState.IDLE){ - if(gamepad1.left_trigger != 0){ + if (liftState == LiftState.BOTTOM && armState == ArmState.IDLE) { + if (gamepad1.left_trigger != 0) { armStretchPos = STRETCH_OUT_POSITION; robotTop.setArmStretchPosition(armStretchPos); armState = ArmState.STRETCHED; sleep(1500); - }if(gamepad1.right_trigger != 0){ + } + if (gamepad1.right_trigger != 0) { armStretchPos = STRETCH_OUT_POSITION; robotTop.setArmStretchPosition(armStretchPos); armState = ArmState.STRETCHED; sleep(1500); } - }else{ - if(gamepad1.left_trigger != 0){ + } else { + if (gamepad1.left_trigger != 0) { robotTop.setLeftPower(-0.5); } else if (gamepad1.right_trigger != 0) { robotTop.setLeftPower(0.5); - }else{ + } else { robotTop.setLeftPower(0); } } @@ -202,8 +204,8 @@ public void runOpMode() { robotTop.setArmTurnPosition(armTurnPos); } } - if(armState == ArmState.STRETCHED){ - if(gamepad1.x && !previousGamepad1.x){ + if (armState == ArmState.STRETCHED) { + if (gamepad1.x && !previousGamepad1.x) { armState = ArmState.WITHDRAWING; } } @@ -242,18 +244,18 @@ public void runOpMode() { armSpinYPos = Math.max(0, armSpinYPos - 0.05); robotTop.setArmSpinYPosition(armSpinYPos); } - if(gamepad1.a && !previousGamepad1.a){ - if(backGrabOpen){ + if (gamepad1.a && !previousGamepad1.a) { + if (backGrabOpen) { robotTop.setLiftServoPosition(0.2); - }else{ + } else { robotTop.setLiftServoPosition(0.6); } backGrabOpen = !backGrabOpen; } //vision - if (getRuntime() - timePoint >= 1) { - timePoint = getRuntime(); + if ((gamepad2.left_trigger != 0 && previousGamepad2.left_trigger == 0 + ) || (gamepad2.right_trigger != 0 && previousGamepad2.right_trigger == 0)) { recognitionAngle = robotVisionAngle.getDetectedAngle(); robotTop.setArmSpinYPosition(calculateSpinY(recognitionAngle)); } @@ -264,12 +266,10 @@ public void runOpMode() { telemetry.addData("ArmState", armState); telemetry.addData("XPos", armSpinXPos); telemetry.addData("YPos", armSpinYPos); - telemetry.addData("grab", armGrabbing); - telemetry.addData("grabFlag", grabbingFlag); - telemetry.addData("stretch", armStretchPos); telemetry.addData("angle", recognitionAngle); telemetry.update(); previousGamepad1.copy(gamepad1); + previousGamepad2.copy(gamepad2); sleep(50); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java index d50d546c1e50..05ae07059cef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java @@ -336,56 +336,20 @@ public RobotAuto gotoPosition2(double x, double y, double h) { return gotoPosition2(new double[]{CurrentPos.x, CurrentPos.y, CurrentPos.h}, DesiredPos); } - public RobotAuto strafeToPosition(double targetX, double targetY, double maxSpeed, double heading) { - // Ensure the OpMode is still active - if (opMode.opModeIsActive()) { - // 获取当前坐标（需要提供机器人当前的位置，可以由传感器或编码器计算得到） - double currentX = getCurrentX(); - double currentY = getCurrentY(); - - // 计算目标位置与当前坐标的差值 - double deltaX = targetX - currentX; - double deltaY = targetY - currentY; - - // 计算移动的总距离 - double distance = Math.hypot(deltaX, deltaY); - - // 计算移动的方向角（相对于正前方） - double angle = Math.atan2(deltaY, deltaX); - - // 转换方向角为机器人坐标系的方向（可能需要调整角度） - double robotAngle = angle - Math.toRadians(getHeading()); - - // 将方向和速度分解为麦轮需要的前后左右分量 - double strafeX = Math.cos(robotAngle) * maxSpeed; - double strafeY = Math.sin(robotAngle) * maxSpeed; - - // 设置目标位置并启动移动 - setStraightTargetPosition((int) (distance * COUNTS_PER_INCH)); - robotChassis.setRunMode(DcMotor.RunMode.RUN_TO_POSITION); - - while (opMode.opModeIsActive() && robotChassis.isAllBusy()) { - // 调整麦轮的速度分量并修正方向 - double turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN); - robotChassis.driveRobot(strafeY, strafeX, -turnSpeed); - telemetry.addData("Target", "X: %.2f, Y: %.2f", targetX, targetY); - telemetry.addData("Current", "X: %.2f, Y: %.2f", currentX, currentY); - telemetry.update(); - } - - // 停止所有运动并重置模式 - robotChassis.stopMotor(); - robotChassis.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER); - } + public RobotAuto stretchArm(){ + robotTop.setArmStretchPosition(0.3); return this; } - public double getCurrentX() { - // 根据编码器的读数计算当前X坐标 - return 0; // 示例返回值，需要实际实现 + public RobotAuto setLiftPower(double power){ + robotTop.setLeftPower(power); + return this; } - - public double getCurrentY() { - // 根据编码器的读数计算当前Y坐标 - return 0; // 示例返回值，需要实际实现 + public RobotAuto grab(){ + robotTop.setLiftServoPosition(0.6); + return this; + } + public RobotAuto release(){ + robotTop.setLiftServoPosition(0.2); + return this; } } From 270cfd0ac4fbed137324197562e750eb56e8188c Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sat, 7 Dec 2024 09:55:28 +0800 Subject: [PATCH 90/93] f~~k the robot --- .../ftc/teamcode/ManualOpMode.java | 115 +++++++++--------- .../ftc/teamcode/RedLeftAuto.java | 27 ++++ .../ftc/teamcode/hardware/RobotAuto.java | 19 ++- 3 files changed, 102 insertions(+), 59 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 84bc1e56095d..499286d9aae2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -75,78 +75,79 @@ public void runOpMode() { // robotLift liftPosition = robotTop.getLiftPosition(); - if (liftState == LiftState.BOTTOM) { - if (gamepad1.y) { - robotTop.setTopServoPosition(0.05); - liftState = LiftState.GOING_UP; - } - } else if (liftState == LiftState.GOING_UP) { - if (liftPosition >= LIFT_TOP_POSITION) { - robotTop.setLeftPower(0); - liftState = LiftState.TOP; - } else if (liftPosition >= LIFT_TOP_POSITION - 150) { - robotTop.setLeftPower(0.3); - } else if (liftPosition <= LIFT_TOP_POSITION - 150) { - //TODO - robotTop.setLeftPower(0.3); - } - } else if (liftState == LiftState.TOP) { - if (gamepad1.y && liftPosition >= LIFT_BOTTOM_POSITION + 150) { - robotTop.setLeftPower(-0.6); - liftState = LiftState.GOING_DOWN; - } - if (liftPosition <= LIFT_TOP_POSITION) { - robotTop.setLeftPower(0.3); - } - } else if (liftState == LiftState.GOING_DOWN) { - if (liftPosition <= LIFT_BOTTOM_POSITION) { - robotTop.setLeftPower(0); - robotTop.setTopServoPosition(0.05); - liftState = LiftState.BOTTOM; - } else if (liftPosition <= LIFT_BOTTOM_POSITION + 150) { - robotTop.setLeftPower(-0.2); - } - } - if (gamepad1.y && !previousGamepad1.y) { - if (armState == ArmState.IDLE) { - robotChassis.stopMotor(); - armStretchPos = STRETCH_OUT_POSITION; - robotTop.setArmStretchPosition(armStretchPos); - robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION + 0.2); - sleep(1500); - armState = ArmState.LOCKED; - liftState = LiftState.GOING_UP; - } else if (armState == ArmState.LOCKED) { - robotChassis.stopMotor(); - sleep(1500); - armStretchPos = STRETCH_BACK_POSITION; - armState = ArmState.WITHDRAWING; - } - } +// if (liftState == LiftState.BOTTOM) { +// if (gamepad1.y) { +// liftState = LiftState.GOING_UP; +// } +// } else if (liftState == LiftState.GOING_UP) { +// robotTop.setTopServoPosition(0.05); +// if (liftPosition >= LIFT_TOP_POSITION) { +// robotTop.setLeftPower(0); +// liftState = LiftState.TOP; +// } else if (liftPosition >= LIFT_TOP_POSITION - 150) { +// robotTop.setLeftPower(0.3); +// } else if (liftPosition <= LIFT_TOP_POSITION - 150) { +// robotTop.setLeftPower(0.7); +// } +// } else if (liftState == LiftState.TOP) { +// if (gamepad1.y && liftPosition >= LIFT_BOTTOM_POSITION + 150) { +// robotTop.setLeftPower(-0.6); +// liftState = LiftState.GOING_DOWN; +// } +// if (liftPosition <= LIFT_TOP_POSITION) { +// robotTop.setLeftPower(0.3); +// } +// } else if (liftState == LiftState.GOING_DOWN) { +// if (liftPosition <= LIFT_BOTTOM_POSITION) { +// robotTop.setLeftPower(0); +// robotTop.setTopServoPosition(0.05); +// sleep(500); +// liftState = LiftState.BOTTOM; +// } else if (liftPosition <= LIFT_BOTTOM_POSITION + 150) { +// robotTop.setLeftPower(-0.2); +// } +// } +// if (gamepad1.y && !previousGamepad1.y) { +// if (armState == ArmState.IDLE) { +// robotChassis.stopMotor(); +// armStretchPos = STRETCH_OUT_POSITION; +// robotTop.setArmStretchPosition(armStretchPos); +// robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION + 0.2); +// sleep(1000); +// armState = ArmState.LOCKED; +// liftState = LiftState.GOING_UP; +// } else if (armState == ArmState.LOCKED) { +// robotChassis.stopMotor(); +// sleep(1500); +// armStretchPos = STRETCH_BACK_POSITION; +// armState = ArmState.WITHDRAWING; +// } +// } if (liftState == LiftState.BOTTOM && armState == ArmState.IDLE) { if (gamepad1.left_trigger != 0) { armStretchPos = STRETCH_OUT_POSITION; robotTop.setArmStretchPosition(armStretchPos); armState = ArmState.STRETCHED; - sleep(1500); + sleep(1000); } if (gamepad1.right_trigger != 0) { armStretchPos = STRETCH_OUT_POSITION; robotTop.setArmStretchPosition(armStretchPos); armState = ArmState.STRETCHED; - sleep(1500); + sleep(1000); } } else { if (gamepad1.left_trigger != 0) { + robotTop.setTopServoPosition(0.05); robotTop.setLeftPower(-0.5); } else if (gamepad1.right_trigger != 0) { robotTop.setLeftPower(0.5); + robotTop.setTopServoPosition(0.05); } else { robotTop.setLeftPower(0); } } - if (gamepad1.left_bumper && !previousGamepad1.left_bumper - && liftState == LiftState.TOP && armState != ArmState.IDLE) { + if (gamepad1.left_bumper && !previousGamepad1.left_bumper && liftPosition > 1100) { if (topServoState) { robotTop.setTopServoPosition(0.05); } else { @@ -166,6 +167,7 @@ public void runOpMode() { // robotArm if (gamepad1.x && !previousGamepad1.x) { if (armState == ArmState.IDLE) { + robotTop.setTopServoPosition(0); armStretchPos = STRETCH_OUT_POSITION; armState = ArmState.TURNING_OUT; robotTop.setArmStretchPosition(armStretchPos); @@ -177,7 +179,6 @@ public void runOpMode() { if (armState == ArmState.WITHDRAWING) { if (armStretchPos <= STRETCH_BACK_POSITION) { armState = ArmState.IDLE; - robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION); } else { armStretchPos -= 0.03; robotTop.setArmStretchPosition(armStretchPos); @@ -198,6 +199,7 @@ public void runOpMode() { if (armTurnPos <= TURN_BACK_POSITION + 0.05) { armTurnPos = TURN_BACK_POSITION; robotTop.setArmTurnPosition(armTurnPos); + robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION); armState = ArmState.WITHDRAWING; } else { armTurnPos -= 0.03; @@ -206,6 +208,7 @@ public void runOpMode() { } if (armState == ArmState.STRETCHED) { if (gamepad1.x && !previousGamepad1.x) { + robotTop.setTopServoPosition(0); armState = ArmState.WITHDRAWING; } } @@ -246,7 +249,7 @@ public void runOpMode() { } if (gamepad1.a && !previousGamepad1.a) { if (backGrabOpen) { - robotTop.setLiftServoPosition(0.2); + robotTop.setLiftServoPosition(0.1); } else { robotTop.setLiftServoPosition(0.6); } @@ -281,9 +284,9 @@ protected double calculateSpinY(double angle) { double adjustedAngle; if (angle <= 0) { - adjustedAngle = -angle / 360.0 + 0.1; + adjustedAngle = -angle / 180 + 0.1; } else { - adjustedAngle = 0.6 - angle / 360.0; + adjustedAngle = 0.6 - angle / 180; } return Math.max(0, Math.min(adjustedAngle, 1)); // 确保值在0到1之间 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java new file mode 100644 index 000000000000..92451099560b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.RobotAuto; + +@Autonomous +public class RedLeftAuto extends LinearOpMode { + RobotAuto robotAuto; + @Override + public void runOpMode(){ + robotAuto = new RobotAuto(this); + waitForStart(); + resetRuntime(); + robotAuto.fastForward(96) + .sleep(1000) + .leftShift(-12) + .sleep(30000); + telemetry.addData("x",robotAuto.getPosition().x); + telemetry.addData("y",robotAuto.getPosition().y); + telemetry.addData("h",robotAuto.getPosition().h); + telemetry.update(); + robotAuto.grab().setLiftPower(0.5).sleep(400).fastForward(24).rightShift(48).fastSpin(180) + .setLiftPower(0.3).sleep(500).backward(10).setLiftPower(-0.5).sleep(200).setLiftPower(0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java index 05ae07059cef..5adddad4212d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java @@ -3,6 +3,7 @@ import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.hardware.sparkfun.SparkFunOTOS; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.DcMotor; @@ -45,10 +46,22 @@ public RobotAuto(LinearOpMode opMode) { imu.resetYaw(); } - final double COUNTS_PER_INCH = 0; - final double P_DRIVE_GAIN = 0; + static final double COUNTS_PER_MOTOR_REV = 560; + + // gearing UP (will affect the direction of wheel rotation) < 1 < gearing DOWN + // eg. for a 12-tooth driving a 24-tooth, the value is 24/12=2.0 + static final double DRIVE_GEAR_REDUCTION = 1.0; + + // wheel diameter in inches + static final double WHEEL_DIAMETER_INCHES = 5.31; + + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1415); + + static final double P_DRIVE_GAIN = 0.03; // Larger is more responsive, but also less stable + static final double P_STRAFE_GAIN = 0.03; // Strafe Speed Control "Gain". + static final double P_TURN_GAIN = 0.1; // Larger is more responsive, but also less stable static final double HEADING_THRESHOLD = 0.5; - static final double P_TURN_GAIN = 0.1; public double getSteeringCorrection(double desiredHeading, double proportionalGain) { From e0b369a83a3d312fe1533aca99dcaa67fe4c6a0c Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sat, 7 Dec 2024 11:05:07 +0800 Subject: [PATCH 91/93] A good ManualOpMode --- .../ftc/teamcode/ManualOpMode.java | 22 +++++++++++++------ 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index 499286d9aae2..f68560c74393 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -36,7 +36,7 @@ public class ManualOpMode extends LinearOpMode { final double SPIN_Y_DEFAULT_POSITION = 0.1; final double TURN_BACK_POSITION = 0.38; final double TURN_OUT_HOVERING_POSITION = 0.64; - final double TURN_OUT_DOWN_POSITION = 0.71; + final double TURN_OUT_DOWN_POSITION = 0.68; final double GRAB_OPEN_POSITION = 0.2; final double GRAB_CLOSE_POSITION = 0.9; @@ -58,7 +58,7 @@ public void runOpMode() { double armSpinYPos = SPIN_Y_DEFAULT_POSITION; boolean armGrabbing = false; boolean grabbingFlag = false; - boolean topServoState = false; + boolean topServoOut = false; boolean containerRelease = true; boolean backGrabOpen = false; double recognitionAngle = 0; @@ -138,22 +138,30 @@ public void runOpMode() { } } else { if (gamepad1.left_trigger != 0) { - robotTop.setTopServoPosition(0.05); + if(!topServoOut){ + robotTop.setTopServoPosition(0.05); + } robotTop.setLeftPower(-0.5); } else if (gamepad1.right_trigger != 0) { + if(!topServoOut){ + robotTop.setTopServoPosition(0.05); + } robotTop.setLeftPower(0.5); - robotTop.setTopServoPosition(0.05); } else { - robotTop.setLeftPower(0); + if(LIFT_TOP_POSITION - 150 <= liftPosition && liftPosition <= LIFT_TOP_POSITION){ + robotTop.setLeftPower(0.2); + }else{ + robotTop.setLeftPower(0); + } } } if (gamepad1.left_bumper && !previousGamepad1.left_bumper && liftPosition > 1100) { - if (topServoState) { + if (topServoOut) { robotTop.setTopServoPosition(0.05); } else { robotTop.setTopServoPosition(0.6); } - topServoState = !topServoState; + topServoOut = !topServoOut; } if (gamepad1.right_bumper && !previousGamepad1.right_bumper) { if (containerRelease) { From 3f5acc5d11922bf4db443b992e6a4b9e423f3336 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sat, 7 Dec 2024 20:44:52 +0800 Subject: [PATCH 92/93] ??? --- .../ftc/teamcode/ManualOpMode.java | 10 +- .../ftc/teamcode/RedLeftAuto.java | 25 +++-- .../advancedManual/AdvancedManual.java | 3 +- .../advancedManual/ArmStateMachine.java | 8 +- .../teamcode/advancedManual/ServoManager.java | 81 ---------------- .../{ServoTask.java => Task.java} | 2 +- .../teamcode/advancedManual/TaskManager.java | 92 +++++++++++++++++++ .../advancedManual/test/ServoMgrTest.java | 16 ++-- .../advancedManual/test/TimedServoTest.java | 4 +- .../ftc/teamcode/hardware/RobotAuto.java | 14 +-- .../ftc/teamcode/test/AutoTest.java | 35 +++++++ .../ftc/teamcode/test/WheelOffset.java | 3 +- 12 files changed, 172 insertions(+), 121 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoManager.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/{ServoTask.java => Task.java} (96%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TaskManager.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index f68560c74393..d451e8c0971c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -27,6 +27,7 @@ public class ManualOpMode extends LinearOpMode { // grab(5): [0.2-open, 0.83-close]; default: 0.2 final int LIFT_TOP_POSITION = 1250; + final int LIFT_HANGING_POSITION = 925; final int LIFT_BOTTOM_POSITION = 50; final double STRETCH_BACK_POSITION = 0.03; final double STRETCH_OUT_POSITION = 0.3; @@ -69,7 +70,7 @@ public void runOpMode() { robotTop.setArmStretchPosition(armStretchPos); robotTop.setArmTurnPosition(armTurnPos); robotTop.setContainerServoPosition(0.35); - robotTop.setLiftServoPosition(0.2); + robotTop.setLiftServoPosition(0.6); while (opModeIsActive()) { robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); @@ -141,15 +142,15 @@ public void runOpMode() { if(!topServoOut){ robotTop.setTopServoPosition(0.05); } - robotTop.setLeftPower(-0.5); + robotTop.setLeftPower(-0.3); } else if (gamepad1.right_trigger != 0) { if(!topServoOut){ robotTop.setTopServoPosition(0.05); } robotTop.setLeftPower(0.5); } else { - if(LIFT_TOP_POSITION - 150 <= liftPosition && liftPosition <= LIFT_TOP_POSITION){ - robotTop.setLeftPower(0.2); + if(LIFT_TOP_POSITION - 100 <= liftPosition && liftPosition <= LIFT_TOP_POSITION){ + robotTop.setLeftPower(0.03); }else{ robotTop.setLeftPower(0); } @@ -267,6 +268,7 @@ public void runOpMode() { //vision if ((gamepad2.left_trigger != 0 && previousGamepad2.left_trigger == 0 ) || (gamepad2.right_trigger != 0 && previousGamepad2.right_trigger == 0)) { + //robotTop.setArmSpinYPosition(SPIN_Y_DEFAULT_POSITION); recognitionAngle = robotVisionAngle.getDetectedAngle(); robotTop.setArmSpinYPosition(calculateSpinY(recognitionAngle)); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java index 92451099560b..efb7ac891d29 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java @@ -13,15 +13,20 @@ public void runOpMode(){ robotAuto = new RobotAuto(this); waitForStart(); resetRuntime(); - robotAuto.fastForward(96) - .sleep(1000) - .leftShift(-12) - .sleep(30000); - telemetry.addData("x",robotAuto.getPosition().x); - telemetry.addData("y",robotAuto.getPosition().y); - telemetry.addData("h",robotAuto.getPosition().h); - telemetry.update(); - robotAuto.grab().setLiftPower(0.5).sleep(400).fastForward(24).rightShift(48).fastSpin(180) - .setLiftPower(0.3).sleep(500).backward(10).setLiftPower(-0.5).sleep(200).setLiftPower(0); + + robotAuto.grab() + .setLiftPower(0.5) + .sleep(400) + .setLiftPower(0) + .fastBackward(24) + .leftShift(24) + .fastSpin(180) + .setLiftPower(0.3) + .sleep(800) + .fastBackward(10) + .setLiftPower(-0.5) + .sleep(200) + .setLiftPower(0) + .release(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java index fe0da6a83c64..b7726e8508b9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java @@ -3,7 +3,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.Gamepad; -import com.qualcomm.robotcore.hardware.Servo; @Disabled // The ManualOp currently used is a piece of shit. @@ -11,7 +10,7 @@ public class AdvancedManual extends LinearOpMode { @Override public void runOpMode(){ - ServoManager servoMgr = new ServoManager(); + TaskManager servoMgr = new TaskManager(); Gamepad previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); waitForStart(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java index ce63e58f36c5..2941740faa0f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java @@ -2,7 +2,6 @@ import com.qualcomm.robotcore.hardware.Gamepad; -import org.firstinspires.ftc.teamcode.ManualOpMode; import org.firstinspires.ftc.teamcode.hardware.RobotTop; public class ArmStateMachine { @@ -13,7 +12,7 @@ enum ArmState{ ArmState armState; Gamepad gamepad1; Gamepad previousGamepad1; - ServoManager servoMgr; + TaskManager taskMgr; final double STRETCH_BACK_POSITION = 0.03; final double STRETCH_OUT_POSITION = 0.3; @@ -34,7 +33,7 @@ enum ArmState{ public ArmStateMachine(RobotTop robotTop){ this.robotTop = robotTop; this.armState = ArmState.IDLE; - this.servoMgr = new ServoManager(); + this.taskMgr = new TaskManager(); } protected void init(){ int liftPosition; @@ -69,8 +68,9 @@ public void update(){ } protected void handleIdleState(){ if(gamepad1.x && !previousGamepad1.x){ + armStretchPos = STRETCH_OUT_POSITION; robotTop.setArmStretchPosition(armStretchPos); - + robotTop.setTopServoPosition(0); } } protected void handleTurningOutState(){} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoManager.java deleted file mode 100644 index 8626ea2875fb..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoManager.java +++ /dev/null @@ -1,81 +0,0 @@ -package org.firstinspires.ftc.teamcode.advancedManual; - -import android.util.ArrayMap; - -import com.qualcomm.robotcore.hardware.Servo; - -import java.util.List; -import java.util.Map; -import java.util.stream.Collectors; - -public class ServoManager { - protected Map tasks; - protected Map taskState; - - public ServoManager() { - tasks = new ArrayMap<>(); - } - - public void updateServos(){ - for (Map.Entry taskEntry : tasks.entrySet()) { - int taskId = taskEntry.getKey(); - ServoTask task = taskEntry.getValue(); - if (!task.hasNext()) { - task.finish(); - taskState.replace(taskId, false); - tasks.remove(taskId, task); - continue; - } - task.iterate(); - } - } - - /** - * Sets the current position of the servo in a fixed time, expressed as a fraction of - * its available range. If PWM power is enabled for the servo, the servo will attempt - * to move to the indicated position. - * - * @param position the position to which the servo should move, a value in the range [0.0, 1.0] - * @param timeMs the time that the servo will take to move. - * @return a Task object - */ - private ServoTask timedSetPosition(Servo servo, double position, long timeMs) { - final double deltaPos = position - servo.getPosition(); - final long iterationCount = timeMs / ServoTask.TICK_MS; - final double positionPerIteration = deltaPos / iterationCount; - return new ServoTask() { - long remainingIterations = iterationCount; - final double targetPosition = position; - - @Override - public void iterate() { - servo.setPosition(servo.getPosition() + positionPerIteration); - remainingIterations--; - } - - @Override - public void finish() { - servo.setPosition(targetPosition); - } - - @Override - public boolean hasNext() { - return remainingIterations > 0; - } - }; - } - public int setTimedServoPosition(Servo servo, double position, long timeMs){ - ServoTask task = timedSetPosition(servo, position, timeMs); - int taskId = findMinFreeTaskId(); - tasks.put(taskId, task); - return taskId; - } - private int findMinFreeTaskId() { - List taskIdList = tasks.keySet().stream().sorted().collect(Collectors.toList()); - int maxTaskId = taskIdList.get(taskIdList.size() - 1); - for (int i = 0; i <= maxTaskId; i++) { - if (taskIdList.contains(i)) return 1; - } - return maxTaskId + 1; - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/Task.java similarity index 96% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoTask.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/Task.java index 9cd9a6e45b96..f72d16f4cf59 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ServoTask.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/Task.java @@ -1,6 +1,6 @@ package org.firstinspires.ftc.teamcode.advancedManual; -public interface ServoTask { +public interface Task { int TICK_MS = 50; /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TaskManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TaskManager.java new file mode 100644 index 000000000000..30566ecd3e6d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TaskManager.java @@ -0,0 +1,92 @@ +package org.firstinspires.ftc.teamcode.advancedManual; + + +import com.qualcomm.robotcore.hardware.Servo; + +import java.util.HashMap; +import java.util.HashSet; +import java.util.Set; + +public class TaskManager { + protected int currentMaxId = 0; + protected HashMap tasks; + protected HashMap taskState; + + public TaskManager() { + tasks = new HashMap<>(); + taskState = new HashMap<>(); + } + + public void updateServos(){ + Set tasksToRemove = new HashSet<>(); + for (HashMap.Entry taskEntry : tasks.entrySet()) { + int taskId = taskEntry.getKey(); + Task task = taskEntry.getValue(); + if (!task.hasNext()) { + task.finish(); + taskState.replace(taskId, false); + tasksToRemove.add(taskId); + continue; + } + task.iterate(); + } + for (int taskId : tasksToRemove) { + tasks.remove(taskId); + } + } + + public int setTimedServoPosition(Servo servo, double position, long timeMs) { + final double deltaPos = position - servo.getPosition(); + final long iterationCount = timeMs / Task.TICK_MS; + final double positionPerIteration = deltaPos / iterationCount; + Task task = new Task() { + long remainingIterations = iterationCount; + final double targetPosition = position; + + @Override + public void iterate() { + servo.setPosition(servo.getPosition() + positionPerIteration); + remainingIterations--; + } + + @Override + public void finish() { + servo.setPosition(targetPosition); + } + + @Override + public boolean hasNext() { + return remainingIterations > 0; + } + }; + return registerTask(task); + } + public int await(long timeMs){ + Task task = new Task() { + long iteration = timeMs / 50; + @Override + public void iterate() { + iteration--; + } + + @Override + public boolean hasNext() { + return iteration > 0; + } + }; + return registerTask(task); + } + public boolean getTaskState(int taskId){ + return Boolean.TRUE.equals(taskState.get(taskId)); + } + private int registerTask(Task task){ + int taskId = findMinFreeTaskId(); + tasks.put(taskId, task); + taskState.put(taskId, true); + return taskId; + } + private int findMinFreeTaskId() { + currentMaxId += 1; + return currentMaxId; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java index fcc7de77fb6a..7a4333668fad 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java @@ -2,7 +2,7 @@ import android.util.ArrayMap; -import org.firstinspires.ftc.teamcode.advancedManual.ServoTask; +import org.firstinspires.ftc.teamcode.advancedManual.Task; import java.util.List; import java.util.Map; @@ -22,7 +22,7 @@ public double getPosition() { } class ServoMgrTest { - protected Map tasks; + protected Map tasks; protected Map taskState; public ServoMgrTest() { @@ -30,9 +30,9 @@ public ServoMgrTest() { } public void updateServos(){ - for (Map.Entry taskEntry : tasks.entrySet()) { + for (Map.Entry taskEntry : tasks.entrySet()) { int taskId = taskEntry.getKey(); - ServoTask task = taskEntry.getValue(); + Task task = taskEntry.getValue(); if (!task.hasNext()) { task.finish(); taskState.replace(taskId, false); @@ -43,11 +43,11 @@ public void updateServos(){ } } - private ServoTask timedSetPosition(ServoStimulator servo, double position, long timeMs) { + private Task timedSetPosition(ServoStimulator servo, double position, long timeMs) { final double deltaPos = position - servo.getPosition(); - final long iterationCount = timeMs / ServoTask.TICK_MS; + final long iterationCount = timeMs / Task.TICK_MS; final double positionPerIteration = deltaPos / iterationCount; - return new ServoTask() { + return new Task() { long remainingIterations = iterationCount; final double targetPosition = position; @@ -69,7 +69,7 @@ public boolean hasNext() { }; } public int setTimedServoPosition(ServoStimulator servo, double position, long timeMs){ - ServoTask task = timedSetPosition(servo, position, timeMs); + Task task = timedSetPosition(servo, position, timeMs); int taskId = findMinFreeTaskId(); tasks.put(taskId, task); return taskId; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java index 0cc11664a475..24d386da3c73 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java @@ -5,7 +5,7 @@ import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; -import org.firstinspires.ftc.teamcode.advancedManual.ServoManager; +import org.firstinspires.ftc.teamcode.advancedManual.TaskManager; @Disabled // The ManualOp currently used is a piece of shit. // I'll write a better one here (if the time is enough). @@ -13,7 +13,7 @@ public class TimedServoTest extends LinearOpMode { @Override public void runOpMode(){ Servo servo = hardwareMap.get(Servo.class, "servo"); - ServoManager servoMgr = new ServoManager(); + TaskManager servoMgr = new TaskManager(); Gamepad previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); waitForStart(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java index 5adddad4212d..5757ad6f73d5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java @@ -254,11 +254,11 @@ public SparkFunOTOS.Pose2D getPosition() { } public RobotAuto forward(double d) { - return driveStraight(0.6, d, getHeading()); + return driveStraight(0.6, -d, getHeading()); } public RobotAuto fastForward(double d) { - return driveStraight(0.9, d, getHeading()); + return driveStraight(0.9, -d, getHeading()); } @@ -271,11 +271,11 @@ public RobotAuto fastBackward(double d) { } public RobotAuto rightShift(double d) { - return driveStrafe(0.9, d, getHeading()); + return driveStrafe(0.9, -d, getHeading()); } public RobotAuto leftShift(double d) { - return driveStrafe(0.9, -d, getHeading()); + return driveStrafe(0.9, d, getHeading()); } public RobotAuto spin(double h) { @@ -283,7 +283,7 @@ public RobotAuto spin(double h) { } public RobotAuto fastSpin(double h) { - return turnToHeading(0.7, h); + return turnToHeading(0.9, h); } public RobotAuto sleep(long milliseconds) { @@ -339,8 +339,8 @@ public RobotAuto gotoPosition(double x, double y, double h) { public RobotAuto gotoPosition2(double[] CurrentPos, double[] DesiredPos) { double[] Displacement = getDisplacement(CurrentPos, DesiredPos); double DesiredHeading = DesiredPos[2]; - return leftShift(-Displacement[1]) - .fastForward(-Displacement[0]) + return leftShift(Displacement[1]) + .fastForward(Displacement[0]) .fastSpin(DesiredHeading); } public RobotAuto gotoPosition2(double x, double y, double h) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest.java new file mode 100644 index 000000000000..99263d520439 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest.java @@ -0,0 +1,35 @@ +package org.firstinspires.ftc.teamcode.test; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.RobotAuto; +import org.firstinspires.ftc.teamcode.hardware.RobotChassis; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + +@Autonomous +public class AutoTest extends LinearOpMode { + RobotAuto robotAuto; + RobotChassis robotChassis; + RobotTop robotTop; + + @Override + public void runOpMode() { + robotChassis = new RobotChassis(this); + robotAuto = new RobotAuto(this); + waitForStart(); + resetRuntime(); + robotAuto.turnToHeading(0.7, 45); + robotChassis.driveRobot(0.6,0,0); + sleep((int)(24 * 1.414/0.6)); + robotChassis.stopMotor(); + robotAuto.turnToHeading(0.7,180); + robotTop.setLeftPower(0.6); + while (robotTop.getLiftPosition() <= 1000){} + robotTop.setLeftPower(0); + robotAuto.fastBackward(10); + robotTop.setLeftPower(-0.3); + while (robotTop.getLiftPosition() >= 925){} + robotTop.setLiftServoPosition(0.2); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java index b9a022769ea6..7b4a7b2e71b5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/WheelOffset.java @@ -5,7 +5,6 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; -@Disabled @TeleOp(group = "Test") public class WheelOffset extends LinearOpMode { @Override @@ -16,7 +15,7 @@ public void runOpMode(){ DcMotor backRightMotor = hardwareMap.dcMotor.get("BR"); waitForStart(); - double k1=1,k2=0.8535,k3=0.9985,k4 = 0.849; + double k1=0.6179,k2=0.7116,k3=6176,k4 = 1; frontLeftMotor.setPower(1*k1); backLeftMotor.setPower(1*k2); From 5d0cd2a0fadfd0374837a9cbcc4fcff572aece28 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sun, 8 Dec 2024 12:42:59 +0800 Subject: [PATCH 93/93] code of HangZhou FTC --- .../ftc/teamcode/ManualOpMode.java | 2 +- .../test/RobotStateMachine.java | 93 +++++++++++++++++++ .../ftc/teamcode/test/AutoTest.java | 17 +--- build.gradle | 2 +- gradle/wrapper/gradle-wrapper.properties | 3 +- 5 files changed, 100 insertions(+), 17 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/RobotStateMachine.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java index d451e8c0971c..937238c1d2de 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -149,7 +149,7 @@ public void runOpMode() { } robotTop.setLeftPower(0.5); } else { - if(LIFT_TOP_POSITION - 100 <= liftPosition && liftPosition <= LIFT_TOP_POSITION){ + if(LIFT_HANGING_POSITION - 100 <= liftPosition && liftPosition <= LIFT_HANGING_POSITION){ robotTop.setLeftPower(0.03); }else{ robotTop.setLeftPower(0); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/RobotStateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/RobotStateMachine.java new file mode 100644 index 000000000000..8ace175db1c6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/RobotStateMachine.java @@ -0,0 +1,93 @@ +package org.firstinspires.ftc.teamcode.advancedManual.test; + +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.teamcode.advancedManual.TaskManager; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + +public class RobotStateMachine { + enum ArmState{ + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK + } + enum LiftState { + BOTTOM, GOING_UP, TOP, GOING_DOWN + } + RobotTop robotTop; + ArmState armState; + LiftState liftState; + Gamepad gamepad1; + Gamepad previousGamepad1; + TaskManager taskMgr; + + final double STRETCH_BACK_POSITION = 0.03; + final double STRETCH_OUT_POSITION = 0.3; + final double SPIN_X_DEFAULT_POSITION = 0.22; + final double SPIN_X_HOVERING_POSITION = 0.53; + final double SPIN_X_DOWN_POSITION = 0.58; + final double SPIN_Y_DEFAULT_POSITION = 0.1; + final double TURN_BACK_POSITION = 0.38; + final double TURN_OUT_HOVERING_POSITION = 0.64; + final double TURN_OUT_DOWN_POSITION = 0.71; + final double GRAB_OPEN_POSITION = 0.2; + final double GRAB_CLOSE_POSITION = 0.9; + final double CONTAINER_OPEN_POSITION = 0.35; + final double CONTAINER_CLOSE_POSITION = 1; + final double LIFT_SERVO_CLOSE = 0.6; + final double LIFT_SERVO_OPEN = 0.2; + + double armStretchPos; + double armTurnPos; + double armSpinXPos; + double armSpinYPos; + public RobotStateMachine(RobotTop robotTop){ + this.robotTop = robotTop; + this.armState = ArmState.IDLE; + this.liftState = LiftState.BOTTOM; + this.taskMgr = new TaskManager(); + } + protected void init(){ + int liftPosition; + double armStretchPos = STRETCH_BACK_POSITION; + double armTurnPos = TURN_BACK_POSITION; + double armSpinXPos = SPIN_X_DEFAULT_POSITION; + double armSpinYPos = SPIN_Y_DEFAULT_POSITION; + boolean armGrabbing = false; + boolean grabbingFlag = false; + boolean topServoState = false; + boolean containerRelease = false; + double recognitionAngle = 0; + } + + private void initializeRobotPositions() { + robotTop.setArmStretchPosition(armStretchPos); + robotTop.setArmTurnPosition(armTurnPos); + robotTop.setContainerServoPosition(CONTAINER_OPEN_POSITION); + robotTop.setLiftServoPosition(LIFT_SERVO_CLOSE); + } + + public void update(){ + switch (armState){ + case IDLE: + handleArmIdle(); + case TURNING_OUT: + handleArmTurningOut(); + case TURNED: + handleArmTurned(); + case TURNING_BACK: + handleArmTurningBack(); + case WITHDRAWING: + handleWithdrawingState(); + } + } + protected void handleArmIdle(){ + if(gamepad1.x && !previousGamepad1.x){ + armStretchPos = STRETCH_OUT_POSITION; + robotTop.setArmStretchPosition(armStretchPos); + robotTop.setTopServoPosition(0); + } + } + protected void handleArmTurningOut(){} + protected void handleArmTurned(){} + protected void handleArmTurningBack(){} + protected void handleWithdrawingState(){} +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest.java index 99263d520439..d02139b6208d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest.java @@ -5,13 +5,11 @@ import org.firstinspires.ftc.teamcode.hardware.RobotAuto; import org.firstinspires.ftc.teamcode.hardware.RobotChassis; -import org.firstinspires.ftc.teamcode.hardware.RobotTop; @Autonomous public class AutoTest extends LinearOpMode { RobotAuto robotAuto; RobotChassis robotChassis; - RobotTop robotTop; @Override public void runOpMode() { @@ -19,17 +17,8 @@ public void runOpMode() { robotAuto = new RobotAuto(this); waitForStart(); resetRuntime(); - robotAuto.turnToHeading(0.7, 45); - robotChassis.driveRobot(0.6,0,0); - sleep((int)(24 * 1.414/0.6)); - robotChassis.stopMotor(); - robotAuto.turnToHeading(0.7,180); - robotTop.setLeftPower(0.6); - while (robotTop.getLiftPosition() <= 1000){} - robotTop.setLeftPower(0); - robotAuto.fastBackward(10); - robotTop.setLeftPower(-0.3); - while (robotTop.getLiftPosition() >= 925){} - robotTop.setLiftServoPosition(0.2); + while (getRuntime() <= 4){ + robotChassis.driveRobot(0,-0.5,0); + } } } \ No newline at end of file diff --git a/build.gradle b/build.gradle index 9337c71c59d9..e8f79f72f864 100644 --- a/build.gradle +++ b/build.gradle @@ -13,7 +13,7 @@ buildscript { } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:7.2.0' + classpath 'com.android.tools.build:gradle:7.4.2' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index aa991fceae6e..7cc7ca52760a 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,6 @@ +#Sun Dec 08 12:29:19 CST 2024 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-7.4.2-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists