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 001/143] 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 002/143] 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 003/143] 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 004/143] 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 005/143] 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 006/143] 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 007/143] 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 008/143] 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 009/143] 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 010/143] 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 d569d6bb92d313838283ea9f1da14e8ab7d674bc Mon Sep 17 00:00:00 2001 From: LeChocolatChaud Date: Sun, 15 Sep 2024 22:23:43 +0800 Subject: [PATCH 011/143] Fix this damn S-word looking commit history by doing a reasonable change --- .../org/firstinspires/ftc/teamcode/ManualOpMode.java | 10 ++++++++++ 1 file changed, 10 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..0722aa8c0629 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +public class ManualOpMode extends LinearOpMode { + @Override + public void runOpMode() { + + } +} From 234fd46ad64a16ebb648481388e51d84a5d11372 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sun, 15 Sep 2024 22:45:57 +0800 Subject: [PATCH 012/143] 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 013/143] 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 39bb7e56eabd14df79e2af2e02bdbedbb4f35022 Mon Sep 17 00:00:00 2001 From: LeChocolatChaud Date: Mon, 16 Sep 2024 00:00:06 +0800 Subject: [PATCH 014/143] Some concepts about Task based multitask --- .../org/firstinspires/ftc/teamcode/Task.java | 7 +++++ .../ftc/teamcode/TaskOpMode.java | 26 +++++++++++++++++++ 2 files changed, 33 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java new file mode 100644 index 000000000000..ad3262c7fbb6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java @@ -0,0 +1,7 @@ +package org.firstinspires.ftc.teamcode; + +public interface Task { + int TICK_MS = 50; + void iterate(); + boolean hasNext(); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java new file mode 100644 index 000000000000..9513ee5d9e43 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java @@ -0,0 +1,26 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import java.util.ArrayList; +import java.util.List; + +public abstract class TaskOpMode extends LinearOpMode { + + List tasks; + + public TaskOpMode() { + tasks = new ArrayList<>(); + } + + @Override + public void runOpMode() { + for (Task task: tasks) { + if (!task.hasNext()) { + tasks.remove(task); + } + task.iterate(); + } + sleep(Task.TICK_MS); + } +} From 914b2b6e19a5ee3c6e33ca62990649b4e32772e8 Mon Sep 17 00:00:00 2001 From: LeChocolatChaud Date: Mon, 16 Sep 2024 00:07:53 +0800 Subject: [PATCH 015/143] add register method --- .../main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java index 9513ee5d9e43..604e92b49b0e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java @@ -23,4 +23,8 @@ public void runOpMode() { } sleep(Task.TICK_MS); } + + public void registerTask(Task task) { + tasks.add(task); + } } From 550f4813144ea30a0acc5532cb0ddae75704d42d Mon Sep 17 00:00:00 2001 From: LeChocolatChaud Date: Mon, 16 Sep 2024 02:10:54 +0800 Subject: [PATCH 016/143] feat: a more detailed concept demo on Task --- .../ftc/teamcode/ManualOpMode.java | 16 ++- .../firstinspires/ftc/teamcode/RobotTop.java | 21 ++++ .../org/firstinspires/ftc/teamcode/Task.java | 2 + .../ftc/teamcode/TaskOpMode.java | 32 ++++- .../ftc/teamcode/TimedServo.java | 115 ++++++++++++++++++ TeamCode/src/main/res/values/strings.xml | 2 +- 6 files changed, 180 insertions(+), 8 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTop.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TimedServo.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 0722aa8c0629..eed7f025a520 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -2,9 +2,21 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -public class ManualOpMode extends LinearOpMode { +public class ManualOpMode extends TaskOpMode { + + private RobotTop top; + @Override - public void runOpMode() { + public void linearInit() { + top = new RobotTop(this); + } + @Override + public void linearLoop() { + if (gamepad1.a && !gamepad1Snapshot.a) { + top.stretchArm(); + } else if (!gamepad1.a && gamepad1Snapshot.a) { + top.retractArm(); + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTop.java new file mode 100644 index 000000000000..5d2e67319202 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTop.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode; + +import android.content.res.Resources; + +import com.qualcomm.robotcore.hardware.Servo; + +public class RobotTop { + TimedServo armServo; + + public RobotTop(TaskOpMode opMode) { + this.armServo = new TimedServo(opMode.hardwareMap.get(Servo.class, Resources.getSystem().getString(R.string.armServo))); + } + + public Task stretchArm() { + return armServo.setPosition(1, 500); + } + + public Task retractArm() { + return armServo.setPosition(0, 500); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java index ad3262c7fbb6..5cce4edc5986 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java @@ -2,6 +2,8 @@ public interface Task { int TICK_MS = 50; + default void init() {} void iterate(); + default void finish() {} boolean hasNext(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java index 604e92b49b0e..d30d1a61f8e2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Gamepad; import java.util.ArrayList; import java.util.List; @@ -8,23 +9,44 @@ public abstract class TaskOpMode extends LinearOpMode { List tasks; + Gamepad gamepad1Snapshot; + Gamepad gamepad2Snapshot; public TaskOpMode() { tasks = new ArrayList<>(); } + public abstract void linearInit(); + + public void linearStart() {}; + + public abstract void linearLoop(); + + public void linearStop() {}; + @Override public void runOpMode() { - for (Task task: tasks) { - if (!task.hasNext()) { - tasks.remove(task); + linearInit(); + waitForStart(); + linearStart(); + while (opModeIsActive()) { + for (Task task : tasks) { + if (!task.hasNext()) { + task.finish(); + tasks.remove(task); + } + task.iterate(); } - task.iterate(); + linearLoop(); + gamepad1Snapshot.copy(gamepad1); + gamepad2Snapshot.copy(gamepad2); + sleep(Task.TICK_MS); } - sleep(Task.TICK_MS); + linearStop(); } public void registerTask(Task task) { + task.init(); tasks.add(task); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TimedServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TimedServo.java new file mode 100644 index 000000000000..9ba3f3c488ae --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TimedServo.java @@ -0,0 +1,115 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.ServoController; + +public class TimedServo { + 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); + } + + 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/res/values/strings.xml b/TeamCode/src/main/res/values/strings.xml index d781ec5f1e5a..435c43d7058f 100644 --- a/TeamCode/src/main/res/values/strings.xml +++ b/TeamCode/src/main/res/values/strings.xml @@ -1,4 +1,4 @@ - + Arm From f8a4a0a3b949c63117f8eff9b09f3539cbd921fb Mon Sep 17 00:00:00 2001 From: LeChocolatChaud Date: Mon, 16 Sep 2024 16:10:05 +0800 Subject: [PATCH 017/143] feat: adding scheduler design and some frequently used task --- .../ftc/teamcode/ManualOpMode.java | 3 +- .../ftc/teamcode/OneTimeTask.java | 11 +++ .../firstinspires/ftc/teamcode/RobotTop.java | 10 +-- .../firstinspires/ftc/teamcode/SleepTask.java | 19 ++++++ .../org/firstinspires/ftc/teamcode/Task.java | 13 +++- .../ftc/teamcode/TaskOpMode.java | 67 +++++++++++++++---- .../ftc/teamcode/TaskStatus.java | 34 ++++++++++ 7 files changed, 138 insertions(+), 19 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OneTimeTask.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleepTask.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.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 eed7f025a520..71686baef81a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -1,7 +1,8 @@ package org.firstinspires.ftc.teamcode; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +@TeleOp public class ManualOpMode extends TaskOpMode { private RobotTop top; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OneTimeTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OneTimeTask.java new file mode 100644 index 000000000000..d29080e9ea01 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OneTimeTask.java @@ -0,0 +1,11 @@ +package org.firstinspires.ftc.teamcode; + +public abstract class OneTimeTask implements Task { + @Override + public void iterate() {} + + @Override + public boolean hasNext() { + return false; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTop.java index 5d2e67319202..7cd8f315bdd7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTop.java @@ -6,16 +6,18 @@ public class RobotTop { TimedServo armServo; + TaskOpMode opMode; public RobotTop(TaskOpMode opMode) { + this.opMode = opMode; this.armServo = new TimedServo(opMode.hardwareMap.get(Servo.class, Resources.getSystem().getString(R.string.armServo))); } - public Task stretchArm() { - return armServo.setPosition(1, 500); + public void stretchArm() { + opMode.scheduleTask(armServo.setPosition(1, 500)); } - public Task retractArm() { - return armServo.setPosition(0, 500); + public void retractArm() { + opMode.scheduleTask(armServo.setPosition(0, 500)); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleepTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleepTask.java new file mode 100644 index 000000000000..1c92fccf8a5f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleepTask.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode; + +public class SleepTask implements Task { + 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.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java index 5cce4edc5986..a0bf6558a250 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java @@ -2,8 +2,17 @@ public interface Task { int TICK_MS = 50; - default void init() {} + + default void init() { + } + void iterate(); - default void finish() {} + + default void finish() { + } + boolean hasNext(); + + default void cancel() { + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java index d30d1a61f8e2..35387ecee3a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java @@ -1,28 +1,34 @@ package org.firstinspires.ftc.teamcode; +import android.util.ArrayMap; + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.Gamepad; -import java.util.ArrayList; -import java.util.List; +import java.util.Map; public abstract class TaskOpMode extends LinearOpMode { - List tasks; + Map tasks; + Map taskStatuses; + int minimumFreeTaskId = 0; Gamepad gamepad1Snapshot; Gamepad gamepad2Snapshot; public TaskOpMode() { - tasks = new ArrayList<>(); + tasks = new ArrayMap<>(); + taskStatuses = new ArrayMap<>(); } public abstract void linearInit(); - public void linearStart() {}; + public void linearStart() { + } public abstract void linearLoop(); - public void linearStop() {}; + public void linearStop() { + } @Override public void runOpMode() { @@ -30,23 +36,60 @@ public void runOpMode() { waitForStart(); linearStart(); while (opModeIsActive()) { - for (Task task : tasks) { + for (Map.Entry taskEntry: tasks.entrySet()) { + int taskId = taskEntry.getKey(); + Task task = taskEntry.getValue(); if (!task.hasNext()) { task.finish(); - tasks.remove(task); + tasks.remove(taskId, task); + taskStatuses.remove(taskId); + continue; } + TaskStatus taskStatus = taskStatuses.get(taskId); + assert taskStatus != null; + if (tasks.containsKey(taskStatus.afterTaskId)) continue; + boolean shouldRunTask = taskStatus.tickDown(); + if (!shouldRunTask) continue; task.iterate(); } linearLoop(); - gamepad1Snapshot.copy(gamepad1); - gamepad2Snapshot.copy(gamepad2); sleep(Task.TICK_MS); } linearStop(); } - public void registerTask(Task task) { + public int scheduleTask(Task task) { + return scheduleTask(task, 0); + } + + public int scheduleTask(Task task, int delayTicks) { + return scheduleTask(task, delayTicks, 1); + } + + public int scheduleTask(Task task, int delayTicks, int loopTicks) { + return scheduleTask(task, delayTicks, loopTicks, -1); + } + + public int scheduleTask(Task task, int delayTicks, int loopTicks, int afterTaskId) { task.init(); - tasks.add(task); + int taskId = minimumFreeTaskId; + TaskStatus taskStatus = new TaskStatus(delayTicks, loopTicks, afterTaskId); + tasks.put(taskId, task); + taskStatuses.put(taskId, taskStatus); + minimumFreeTaskId++; + return taskId; + } + + public void cancelTask(int taskId, boolean chained) { + Task task = tasks.get(taskId); + if (task == null) return; + task.cancel(); + tasks.remove(taskId, task); + taskStatuses.remove(taskId); + if (chained) { + taskStatuses.entrySet().stream() + .filter(e -> e.getValue().afterTaskId == taskId) + .forEach(e -> cancelTask(e.getKey(), true)); + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java new file mode 100644 index 000000000000..62c65d834b19 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode; + +public class TaskStatus { + int delayTicks; + int loopTicks; + int remainingLoopTicks; + int afterTaskId; + + boolean idling = true; + + public TaskStatus(int delayTicks, int loopTicks, int afterTaskId) { + this.delayTicks = delayTicks; + this.loopTicks = loopTicks; + this.afterTaskId = afterTaskId; + this.remainingLoopTicks = loopTicks; + } + + public boolean tickDown() { + if (idling) { + delayTicks--; + if (delayTicks == 0) { + idling = false; + return true; + } + } else { + remainingLoopTicks--; + if (remainingLoopTicks == 0) { + remainingLoopTicks = loopTicks; + return true; + } + } + return false; + } +} From 4a4183ccb670b1a7527a854edd7bc35b33ecd1d7 Mon Sep 17 00:00:00 2001 From: LeChocolatChaud Date: Mon, 16 Sep 2024 16:57:56 +0800 Subject: [PATCH 018/143] fix: finalizing the task design and delete unused demo classes --- .../ftc/teamcode/ManualOpMode.java | 23 ------------------- .../firstinspires/ftc/teamcode/RobotTop.java | 23 ------------------- 2 files changed, 46 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTop.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 deleted file mode 100644 index 71686baef81a..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -@TeleOp -public class ManualOpMode extends TaskOpMode { - - private RobotTop top; - - @Override - public void linearInit() { - top = new RobotTop(this); - } - - @Override - public void linearLoop() { - if (gamepad1.a && !gamepad1Snapshot.a) { - top.stretchArm(); - } else if (!gamepad1.a && gamepad1Snapshot.a) { - top.retractArm(); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTop.java deleted file mode 100644 index 7cd8f315bdd7..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTop.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import android.content.res.Resources; - -import com.qualcomm.robotcore.hardware.Servo; - -public class RobotTop { - TimedServo armServo; - TaskOpMode opMode; - - public RobotTop(TaskOpMode opMode) { - this.opMode = opMode; - this.armServo = new TimedServo(opMode.hardwareMap.get(Servo.class, Resources.getSystem().getString(R.string.armServo))); - } - - public void stretchArm() { - opMode.scheduleTask(armServo.setPosition(1, 500)); - } - - public void retractArm() { - opMode.scheduleTask(armServo.setPosition(0, 500)); - } -} From 55c9e8e7c5f6911d4d0896d882c1ddc404197b0a Mon Sep 17 00:00:00 2001 From: LeChocolatChaud Date: Mon, 16 Sep 2024 17:01:41 +0800 Subject: [PATCH 019/143] fix: rollback non deployment changes --- TeamCode/src/main/res/values/strings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/res/values/strings.xml b/TeamCode/src/main/res/values/strings.xml index 435c43d7058f..d781ec5f1e5a 100644 --- a/TeamCode/src/main/res/values/strings.xml +++ b/TeamCode/src/main/res/values/strings.xml @@ -1,4 +1,4 @@ - Arm + From 1ef66dc7d2751284ea4c0886e1426936791dda5e Mon Sep 17 00:00:00 2001 From: LeCh2td <85344760+LeChocolatChaud@users.noreply.github.com> Date: Mon, 16 Sep 2024 19:55:32 +0800 Subject: [PATCH 020/143] Create android.yml --- .github/workflows/android.yml | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 .github/workflows/android.yml diff --git a/.github/workflows/android.yml b/.github/workflows/android.yml new file mode 100644 index 000000000000..521dce53f89f --- /dev/null +++ b/.github/workflows/android.yml @@ -0,0 +1,26 @@ +name: Android CI + +on: + push: + branches: [ "master" ] + pull_request: + branches: [ "master" ] + +jobs: + build: + + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v4 + - name: set up JDK 17 + uses: actions/setup-java@v4 + with: + java-version: '17' + distribution: 'zulu' + cache: gradle + + - name: Grant execute permission for gradlew + run: chmod +x gradlew + - name: Build with Gradle + run: ./gradlew build From 70758d1d2e48308af425ad3b5712cd44239a390f Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Mon, 16 Sep 2024 22:44:34 +0800 Subject: [PATCH 021/143] 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 a3b3d9b36602e37e7a99928a1c9a669537b88804 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Mon, 16 Sep 2024 23:39:21 +0800 Subject: [PATCH 022/143] Fix:try to change the logic of TaskOp to make it less complex maybe I was creating more bugs and made it more complex... --- .../ftc/teamcode/AutoTaskOpMode.java | 123 ++++++++++++++++++ .../ftc/teamcode/ManualTaskOpMode.java | 75 +++++++++++ .../ftc/teamcode/TaskOpMode.java | 4 +- .../ftc/teamcode/TaskStatus.java | 1 + 4 files changed, 201 insertions(+), 2 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTaskOpMode.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualTaskOpMode.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTaskOpMode.java new file mode 100644 index 000000000000..b30f92d86def --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTaskOpMode.java @@ -0,0 +1,123 @@ +package org.firstinspires.ftc.teamcode; + +import android.util.ArrayMap; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import java.util.Map; + +public abstract class AutoTaskOpMode extends LinearOpMode { + + Map taskMap; + Map taskLinks; + // format: + Map currentTasks; + int minimumFreeTaskId = 0; + + public AutoTaskOpMode() { + taskMap = new ArrayMap<>(); + taskLinks = new ArrayMap<>(); + } + + public abstract void linearInit(); + + public abstract void linearStart(); + + public void linearLoop(){} + + public void linearStop() { + } + + @Override + public void runOpMode() { + linearInit(); + waitForStart(); + linearStart(); + while (opModeIsActive()) { + for (Map.Entry taskEntry: taskMap.entrySet()) { + int taskId = taskEntry.getKey(); + Task task = taskEntry.getValue(); + if (!task.hasNext()) { + task.finish(); + taskMap.remove(taskId, task); + + //I don't know whether it can run correctly... + //TODO: still need testing + for (Map.Entry eachLink: taskLinks.entrySet()) { + int eachTaskId = eachLink.getKey(); + int lastTaskId = eachLink.getValue(); + if(lastTaskId == taskId){ + runTask(eachTaskId); + } + } + continue; + } +// TaskStatus taskStatus = taskLinks.get(taskId); +// assert taskStatus != null; +// if (taskMap.containsKey(taskStatus.afterTaskId)) continue; +// boolean shouldRunTask = taskStatus.tickDown(); +// if (!shouldRunTask) continue; + task.iterate(); + } + linearLoop(); + sleep(Task.TICK_MS); + } + linearStop(); + } + + private boolean isRunning(){ + return taskMap.isEmpty(); + } + + public int createTask(Task task,int linkedTaskId){ + int taskId = minimumFreeTaskId; + taskMap.put(taskId, task); + taskLinks.put(taskId,linkedTaskId); + minimumFreeTaskId++; + return taskId; + } + public int createTask(Task task){ + return createTask(task,minimumFreeTaskId - 1); + } + + public void runTask(int taskId){ + Task task = taskMap.get(taskId); + task.init(); + currentTasks.put(taskId, task); + } + +// public int scheduleTask(Task task) { +// return scheduleTask(task, 0); +// } +// +// public int scheduleTask(Task task, int delayTicks) { +// return scheduleTask(task, delayTicks, 1); +// } +// +// public int scheduleTask(Task task, int delayTicks, int loopTicks) { +// return scheduleTask(task, delayTicks, loopTicks, -1); +// } +// +// public int scheduleTask(Task task, int delayTicks, int loopTicks, int afterTaskId) { +// task.init(); +// int taskId = minimumFreeTaskId; +// TaskStatus taskStatus = new TaskStatus(delayTicks, loopTicks, afterTaskId); +// taskMap.put(taskId, task); +// taskLinks.put(taskId, taskStatus); +// minimumFreeTaskId++; +// return taskId; +// } +// +// public void cancelTask(int taskId, boolean chained) { +// Task task = taskMap.get(taskId); +// if (task == null) return; +// task.cancel(); +// taskMap.remove(taskId, task); +// taskLinks.remove(taskId); +// if (chained) { +// taskLinks.entrySet().stream() +// .filter(e -> e.getValue().afterTaskId == taskId) +// .forEach(e -> cancelTask(e.getKey(), true)); +// } +// } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualTaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualTaskOpMode.java new file mode 100644 index 000000000000..9d34035c5632 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualTaskOpMode.java @@ -0,0 +1,75 @@ +package org.firstinspires.ftc.teamcode; + +import android.util.ArrayMap; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Gamepad; + +import java.util.Map; + +public abstract class ManualTaskOpMode extends LinearOpMode { + + Map tasks; + int minimumFreeTaskId = 0; + final int TICK_MS = 50; + + public ManualTaskOpMode() { + tasks = 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); + continue; + } + task.iterate(); + } + linearLoop(); + sleep(TICK_MS); + } + linearStop(); + } + + public boolean isRunning(){ + return tasks.isEmpty(); + } + public int createTask(Task task){ + task.init(); + int taskId = minimumFreeTaskId; + tasks.put(taskId, task); + minimumFreeTaskId++; + return taskId; + } + +// public void cancelTask(int taskId, boolean chained) { +// Task task = tasks.get(taskId); +// if (task == null) return; +// task.cancel(); +// tasks.remove(taskId, task); +// taskStatuses.remove(taskId); +// if (chained) { +// taskStatuses.entrySet().stream() +// .filter(e -> e.getValue().afterTaskId == taskId) +// .forEach(e -> cancelTask(e.getKey(), true)); +// } +// } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java index 35387ecee3a8..bc8c9b344f92 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java @@ -7,6 +7,7 @@ import java.util.Map; +//TODO: make it into a parent class,and the other two TaskOp will extend it public abstract class TaskOpMode extends LinearOpMode { Map tasks; @@ -22,8 +23,7 @@ public TaskOpMode() { public abstract void linearInit(); - public void linearStart() { - } + public abstract void linearStart(); public abstract void linearLoop(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java index 62c65d834b19..1dc5f18703ea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode; +//TODO:The class theoretically can be deleted if my changes in TaskOp are correct public class TaskStatus { int delayTicks; int loopTicks; From e3bcaa084b18a3a81426471970bc9e781729b807 Mon Sep 17 00:00:00 2001 From: LeChocolatChaud Date: Tue, 17 Sep 2024 01:06:03 +0800 Subject: [PATCH 023/143] fix: simplify the design by add more prebuilt tasks and clean up unused classes --- .../ftc/teamcode/AutoTaskOpMode.java | 123 ------------------ .../ftc/teamcode/ManualTaskOpMode.java | 75 ----------- .../ftc/teamcode/OneTimeTask.java | 11 -- .../ftc/teamcode/TaskOpMode.java | 68 +++++----- .../ftc/teamcode/TaskStatus.java | 35 ----- .../teamcode/{ => hardware}/TimedServo.java | 6 +- .../ftc/teamcode/task/OneTimeTask.java | 14 ++ .../ftc/teamcode/{ => task}/SleepTask.java | 4 +- .../ftc/teamcode/{ => task}/Task.java | 2 +- 9 files changed, 52 insertions(+), 286 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTaskOpMode.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualTaskOpMode.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OneTimeTask.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => hardware}/TimedServo.java (97%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/OneTimeTask.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => task}/SleepTask.java (78%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => task}/Task.java (81%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTaskOpMode.java deleted file mode 100644 index b30f92d86def..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTaskOpMode.java +++ /dev/null @@ -1,123 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import android.util.ArrayMap; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import java.util.Map; - -public abstract class AutoTaskOpMode extends LinearOpMode { - - Map taskMap; - Map taskLinks; - // format: - Map currentTasks; - int minimumFreeTaskId = 0; - - public AutoTaskOpMode() { - taskMap = new ArrayMap<>(); - taskLinks = new ArrayMap<>(); - } - - public abstract void linearInit(); - - public abstract void linearStart(); - - public void linearLoop(){} - - public void linearStop() { - } - - @Override - public void runOpMode() { - linearInit(); - waitForStart(); - linearStart(); - while (opModeIsActive()) { - for (Map.Entry taskEntry: taskMap.entrySet()) { - int taskId = taskEntry.getKey(); - Task task = taskEntry.getValue(); - if (!task.hasNext()) { - task.finish(); - taskMap.remove(taskId, task); - - //I don't know whether it can run correctly... - //TODO: still need testing - for (Map.Entry eachLink: taskLinks.entrySet()) { - int eachTaskId = eachLink.getKey(); - int lastTaskId = eachLink.getValue(); - if(lastTaskId == taskId){ - runTask(eachTaskId); - } - } - continue; - } -// TaskStatus taskStatus = taskLinks.get(taskId); -// assert taskStatus != null; -// if (taskMap.containsKey(taskStatus.afterTaskId)) continue; -// boolean shouldRunTask = taskStatus.tickDown(); -// if (!shouldRunTask) continue; - task.iterate(); - } - linearLoop(); - sleep(Task.TICK_MS); - } - linearStop(); - } - - private boolean isRunning(){ - return taskMap.isEmpty(); - } - - public int createTask(Task task,int linkedTaskId){ - int taskId = minimumFreeTaskId; - taskMap.put(taskId, task); - taskLinks.put(taskId,linkedTaskId); - minimumFreeTaskId++; - return taskId; - } - public int createTask(Task task){ - return createTask(task,minimumFreeTaskId - 1); - } - - public void runTask(int taskId){ - Task task = taskMap.get(taskId); - task.init(); - currentTasks.put(taskId, task); - } - -// public int scheduleTask(Task task) { -// return scheduleTask(task, 0); -// } -// -// public int scheduleTask(Task task, int delayTicks) { -// return scheduleTask(task, delayTicks, 1); -// } -// -// public int scheduleTask(Task task, int delayTicks, int loopTicks) { -// return scheduleTask(task, delayTicks, loopTicks, -1); -// } -// -// public int scheduleTask(Task task, int delayTicks, int loopTicks, int afterTaskId) { -// task.init(); -// int taskId = minimumFreeTaskId; -// TaskStatus taskStatus = new TaskStatus(delayTicks, loopTicks, afterTaskId); -// taskMap.put(taskId, task); -// taskLinks.put(taskId, taskStatus); -// minimumFreeTaskId++; -// return taskId; -// } -// -// public void cancelTask(int taskId, boolean chained) { -// Task task = taskMap.get(taskId); -// if (task == null) return; -// task.cancel(); -// taskMap.remove(taskId, task); -// taskLinks.remove(taskId); -// if (chained) { -// taskLinks.entrySet().stream() -// .filter(e -> e.getValue().afterTaskId == taskId) -// .forEach(e -> cancelTask(e.getKey(), true)); -// } -// } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualTaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualTaskOpMode.java deleted file mode 100644 index 9d34035c5632..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualTaskOpMode.java +++ /dev/null @@ -1,75 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import android.util.ArrayMap; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.Gamepad; - -import java.util.Map; - -public abstract class ManualTaskOpMode extends LinearOpMode { - - Map tasks; - int minimumFreeTaskId = 0; - final int TICK_MS = 50; - - public ManualTaskOpMode() { - tasks = 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); - continue; - } - task.iterate(); - } - linearLoop(); - sleep(TICK_MS); - } - linearStop(); - } - - public boolean isRunning(){ - return tasks.isEmpty(); - } - public int createTask(Task task){ - task.init(); - int taskId = minimumFreeTaskId; - tasks.put(taskId, task); - minimumFreeTaskId++; - return taskId; - } - -// public void cancelTask(int taskId, boolean chained) { -// Task task = tasks.get(taskId); -// if (task == null) return; -// task.cancel(); -// tasks.remove(taskId, task); -// taskStatuses.remove(taskId); -// if (chained) { -// taskStatuses.entrySet().stream() -// .filter(e -> e.getValue().afterTaskId == taskId) -// .forEach(e -> cancelTask(e.getKey(), true)); -// } -// } -} - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OneTimeTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OneTimeTask.java deleted file mode 100644 index d29080e9ea01..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OneTimeTask.java +++ /dev/null @@ -1,11 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -public abstract class OneTimeTask implements Task { - @Override - public void iterate() {} - - @Override - public boolean hasNext() { - return false; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java index bc8c9b344f92..0aed31b53f31 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java @@ -3,27 +3,27 @@ import android.util.ArrayMap; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.Gamepad; +import org.firstinspires.ftc.teamcode.task.Task; + +import java.util.List; import java.util.Map; +import java.util.stream.Collectors; -//TODO: make it into a parent class,and the other two TaskOp will extend it public abstract class TaskOpMode extends LinearOpMode { - Map tasks; - Map taskStatuses; - int minimumFreeTaskId = 0; - Gamepad gamepad1Snapshot; - Gamepad gamepad2Snapshot; + protected Map tasks; + protected Map taskLinks; public TaskOpMode() { tasks = new ArrayMap<>(); - taskStatuses = new ArrayMap<>(); + taskLinks = new ArrayMap<>(); } public abstract void linearInit(); - public abstract void linearStart(); + public void linearStart() { + } public abstract void linearLoop(); @@ -36,20 +36,17 @@ public void runOpMode() { waitForStart(); linearStart(); while (opModeIsActive()) { - for (Map.Entry taskEntry: tasks.entrySet()) { + for (Map.Entry taskEntry : tasks.entrySet()) { int taskId = taskEntry.getKey(); Task task = taskEntry.getValue(); if (!task.hasNext()) { task.finish(); tasks.remove(taskId, task); - taskStatuses.remove(taskId); + taskLinks.remove(taskId); continue; } - TaskStatus taskStatus = taskStatuses.get(taskId); - assert taskStatus != null; - if (tasks.containsKey(taskStatus.afterTaskId)) continue; - boolean shouldRunTask = taskStatus.tickDown(); - if (!shouldRunTask) continue; + Integer linkedTaskId = taskLinks.get(taskId); + if (tasks.containsKey(linkedTaskId)) continue; task.iterate(); } linearLoop(); @@ -58,38 +55,35 @@ public void runOpMode() { linearStop(); } - public int scheduleTask(Task task) { - return scheduleTask(task, 0); - } - - public int scheduleTask(Task task, int delayTicks) { - return scheduleTask(task, delayTicks, 1); + public int registerTask(Task task) { + return registerTask(task, -1); } - public int scheduleTask(Task task, int delayTicks, int loopTicks) { - return scheduleTask(task, delayTicks, loopTicks, -1); - } - - public int scheduleTask(Task task, int delayTicks, int loopTicks, int afterTaskId) { + public int registerTask(Task task, int linkedTaskId) { task.init(); - int taskId = minimumFreeTaskId; - TaskStatus taskStatus = new TaskStatus(delayTicks, loopTicks, afterTaskId); + int taskId = findMinFreeTaskId(); tasks.put(taskId, task); - taskStatuses.put(taskId, taskStatus); - minimumFreeTaskId++; + taskLinks.put(taskId, linkedTaskId); return taskId; } - public void cancelTask(int taskId, boolean chained) { + public void cancelTask(int taskId) { Task task = tasks.get(taskId); if (task == null) return; task.cancel(); tasks.remove(taskId, task); - taskStatuses.remove(taskId); - if (chained) { - taskStatuses.entrySet().stream() - .filter(e -> e.getValue().afterTaskId == taskId) - .forEach(e -> cancelTask(e.getKey(), true)); + 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/TaskStatus.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java deleted file mode 100644 index 1dc5f18703ea..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskStatus.java +++ /dev/null @@ -1,35 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -//TODO:The class theoretically can be deleted if my changes in TaskOp are correct -public class TaskStatus { - int delayTicks; - int loopTicks; - int remainingLoopTicks; - int afterTaskId; - - boolean idling = true; - - public TaskStatus(int delayTicks, int loopTicks, int afterTaskId) { - this.delayTicks = delayTicks; - this.loopTicks = loopTicks; - this.afterTaskId = afterTaskId; - this.remainingLoopTicks = loopTicks; - } - - public boolean tickDown() { - if (idling) { - delayTicks--; - if (delayTicks == 0) { - idling = false; - return true; - } - } else { - remainingLoopTicks--; - if (remainingLoopTicks == 0) { - remainingLoopTicks = loopTicks; - return true; - } - } - return false; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TimedServo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TimedServo.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java index 9ba3f3c488ae..1449a6e5260c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TimedServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java @@ -1,10 +1,12 @@ -package org.firstinspires.ftc.teamcode; +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 { - Servo servo; + protected Servo servo; public TimedServo(Servo servo) { this.servo = servo; 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 new file mode 100644 index 000000000000..8a1fd39bb8da --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/OneTimeTask.java @@ -0,0 +1,14 @@ +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/SleepTask.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java similarity index 78% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleepTask.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java index 1c92fccf8a5f..87ed2726201d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SleepTask.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/SleepTask.java @@ -1,7 +1,7 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.task; public class SleepTask implements Task { - int remainingSleepTicks; + protected int remainingSleepTicks; public SleepTask(int sleepTicks) { remainingSleepTicks = sleepTicks; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java similarity index 81% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java index a0bf6558a250..efe228aa534e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Task.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode; +package org.firstinspires.ftc.teamcode.task; public interface Task { int TICK_MS = 50; From a021735865165ae3532b680961f3ec3bc91c9976 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Tue, 17 Sep 2024 13:38:01 +0800 Subject: [PATCH 024/143] Docs: add docs --- .../ftc/teamcode/TaskOpMode.java | 15 +++++++++++ .../ftc/teamcode/hardware/TimedServo.java | 9 +++++++ .../firstinspires/ftc/teamcode/task/Task.java | 25 ++++++++++++++++--- 3 files changed, 45 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java index 0aed31b53f31..b154f72bb809 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java @@ -59,6 +59,16 @@ public int registerTask(Task task) { 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) { task.init(); int taskId = findMinFreeTaskId(); @@ -67,6 +77,11 @@ public int registerTask(Task task, int 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; 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 index 1449a6e5260c..f5f6a641ecc7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/TimedServo.java @@ -89,6 +89,15 @@ 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; 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 index efe228aa534e..2ced8b70b134 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java @@ -3,16 +3,33 @@ 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 when the task is registered in the TaskOpMode. + */ default void init() { } - void iterate(); - + /** + * The method is called when the task is finished. + */ default void finish() { } - boolean hasNext(); - + /** + * The method is called when the task is canceled. + */ default void cancel() { } } From 40b86b624a64fc1c06c632802773b07858c4d669 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Tue, 17 Sep 2024 14:57:37 +0800 Subject: [PATCH 025/143] Fix: call task.init() in a right timing --- .../main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java | 4 +++- .../main/java/org/firstinspires/ftc/teamcode/task/Task.java | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java index b154f72bb809..b2203474eaf5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java @@ -43,6 +43,9 @@ public void runOpMode() { task.finish(); tasks.remove(taskId, task); taskLinks.remove(taskId); + taskLinks.entrySet().stream() + .filter(e -> e.getValue() == taskId) + .forEach(e -> tasks.get(e).init()); continue; } Integer linkedTaskId = taskLinks.get(taskId); @@ -70,7 +73,6 @@ public int registerTask(Task task) { * @return the taskId of the registered task. */ public int registerTask(Task task, int linkedTaskId) { - task.init(); int taskId = findMinFreeTaskId(); tasks.put(taskId, task); taskLinks.put(taskId, linkedTaskId); 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 index 2ced8b70b134..6776ae6a1b33 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java @@ -16,7 +16,7 @@ public interface Task { boolean hasNext(); /** - * This method is called when the task is registered in the TaskOpMode. + * This method is called once when the task starts looping in the TaskOpMode. */ default void init() { } From e7d4314db8a6055d37093f805c775f961edefd5a Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Tue, 17 Sep 2024 14:57:37 +0800 Subject: [PATCH 026/143] Fix: call task.init() method at a right timing --- .../main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java | 4 +++- .../main/java/org/firstinspires/ftc/teamcode/task/Task.java | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java index b154f72bb809..b2203474eaf5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java @@ -43,6 +43,9 @@ public void runOpMode() { task.finish(); tasks.remove(taskId, task); taskLinks.remove(taskId); + taskLinks.entrySet().stream() + .filter(e -> e.getValue() == taskId) + .forEach(e -> tasks.get(e).init()); continue; } Integer linkedTaskId = taskLinks.get(taskId); @@ -70,7 +73,6 @@ public int registerTask(Task task) { * @return the taskId of the registered task. */ public int registerTask(Task task, int linkedTaskId) { - task.init(); int taskId = findMinFreeTaskId(); tasks.put(taskId, task); taskLinks.put(taskId, linkedTaskId); 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 index 2ced8b70b134..6776ae6a1b33 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/task/Task.java @@ -16,7 +16,7 @@ public interface Task { boolean hasNext(); /** - * This method is called when the task is registered in the TaskOpMode. + * This method is called once when the task starts looping in the TaskOpMode. */ default void init() { } From 6de00a36fa1c07d0ac2d95c2c99be503613a7e72 Mon Sep 17 00:00:00 2001 From: LeCh2td <85344760+LeChocolatChaud@users.noreply.github.com> Date: Tue, 17 Sep 2024 15:43:18 +0800 Subject: [PATCH 027/143] Update android.yml --- .github/workflows/android.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.github/workflows/android.yml b/.github/workflows/android.yml index 521dce53f89f..31227f9be2fa 100644 --- a/.github/workflows/android.yml +++ b/.github/workflows/android.yml @@ -3,8 +3,6 @@ name: Android CI on: push: branches: [ "master" ] - pull_request: - branches: [ "master" ] jobs: build: From 2cf83632e7e9c9a0c4b3a6aad1dc90187e80326c Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Tue, 17 Sep 2024 15:58:30 +0800 Subject: [PATCH 028/143] Fix: tasks with no link will not init --- .../src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java | 1 + 1 file changed, 1 insertion(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java index b2203474eaf5..07d3b166edd8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java @@ -59,6 +59,7 @@ public void runOpMode() { } public int registerTask(Task task) { + task.init(); return registerTask(task, -1); } From 2ff59d0a086a4bbda537f88fb9989531b10e3a77 Mon Sep 17 00:00:00 2001 From: LeCh2td <85344760+LeChocolatChaud@users.noreply.github.com> Date: Tue, 17 Sep 2024 16:18:42 +0800 Subject: [PATCH 029/143] Update android.yml --- .github/workflows/android.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/android.yml b/.github/workflows/android.yml index 31227f9be2fa..90d77bc95fdc 100644 --- a/.github/workflows/android.yml +++ b/.github/workflows/android.yml @@ -3,7 +3,9 @@ name: Android CI on: push: branches: [ "master" ] - + pull_request: + branches: [ "master" ] + jobs: build: From 345889adb3d822b3409e0ada06b86f4aa89c3ad2 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Tue, 17 Sep 2024 18:28:07 +0800 Subject: [PATCH 030/143] Fix: solve the warnings Fix the type error when getting next task. Handle the potential NullPointerException --- .../main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java index 07d3b166edd8..47ec4f901bdd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TaskOpMode.java @@ -45,7 +45,10 @@ public void runOpMode() { taskLinks.remove(taskId); taskLinks.entrySet().stream() .filter(e -> e.getValue() == taskId) - .forEach(e -> tasks.get(e).init()); + .forEach(e -> { + Task nextTask = tasks.get(e.getKey()); + if (nextTask != null) nextTask.init(); + }); continue; } Integer linkedTaskId = taskLinks.get(taskId); From e8a15164fa6a4dda4de4780fa0d36c9858f5d03e Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Tue, 17 Sep 2024 23:03:12 +0800 Subject: [PATCH 031/143] 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 8ea0624ffd5f6c0be7d984d0b6b0ff1f03b043c6 Mon Sep 17 00:00:00 2001 From: LeChocolatChaud Date: Mon, 30 Sep 2024 19:58:23 +0800 Subject: [PATCH 032/143] fix: prepare for a proper sync --- TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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..3c022b373bef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md @@ -16,7 +16,7 @@ To locate these samples, find the FtcRobotController module in the "Project/Andr Expand the following tree elements: FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples -### Naming of Samples +### 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. From 0f997ee949f0e5353032d8e0a34aa26b4b0d75bd Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Fri, 22 Nov 2024 16:38:03 +0800 Subject: [PATCH 033/143] 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 034/143] 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 035/143] 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 036/143] 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 037/143] 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 038/143] =?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 039/143] =?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 040/143] =?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 041/143] =?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 042/143] 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 043/143] 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 044/143] 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 045/143] 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 046/143] 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 047/143] 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 048/143] 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 049/143] 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 050/143] 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 051/143] 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 052/143] 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 053/143] 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 054/143] 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 055/143] 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 056/143] 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 057/143] 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 058/143] 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 059/143] 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 060/143] 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 061/143] 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 062/143] 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 063/143] 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 064/143] 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 065/143] 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 066/143] 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 067/143] 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 068/143] 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 069/143] 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 070/143] 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 071/143] =?UTF-8?q?add=20the=20=E8=B4=9F=E6=95=B0=20in=20r?= =?UTF-8?q?otate?= 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 072/143] 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 073/143] 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 074/143] 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 075/143] 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 076/143] 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 077/143] 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 078/143] 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 079/143] 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 080/143] 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 081/143] 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 082/143] 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 083/143] 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 084/143] 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 085/143] 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 086/143] 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 087/143] 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 088/143] 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 089/143] 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 090/143] 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 091/143] 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 092/143] 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 093/143] 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 094/143] 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 095/143] 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 096/143] 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 097/143] 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 098/143] 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 099/143] 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 100/143] 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 101/143] 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 102/143] 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 103/143] 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 104/143] 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 105/143] 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 106/143] 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 107/143] 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 108/143] 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 109/143] 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 110/143] ??? --- .../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 111/143] 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 From 7b694b3ebbdc1efe2e698b9ccf2d34622f836cec Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sun, 8 Dec 2024 16:00:08 +0800 Subject: [PATCH 112/143] Feat:Deleted unused roadrunner,fixed gradle building. --- .../ftc/teamcode/ManualOpMode.java | 10 +- .../EmptySequenceException.java | 4 - .../TrajectorySequence.java | 44 -- .../TrajectorySequenceBuilder.java | 679 ------------------ .../TrajectorySequenceRunner.java | 306 -------- .../drive/DriveConstants.java | 97 --- .../drive/SampleMecanumDrive.java | 299 -------- .../drive/SampleTankDrive.java | 293 -------- .../drive/StandardTrackingWheelLocalizer.java | 99 --- .../opmode/AutomaticFeedforwardTuner.java | 221 ------ .../drive/opmode/BackAndForth.java | 52 -- .../drive/opmode/DriveVelocityPIDTuner.java | 171 ----- .../drive/opmode/FollowerPIDTuner.java | 55 -- .../drive/opmode/LocalizationTest.java | 45 -- .../drive/opmode/ManualFeedforwardTuner.java | 152 ---- .../drive/opmode/MaxAngularVeloTuner.java | 73 -- .../drive/opmode/MaxVelocityTuner.java | 84 --- .../drive/opmode/MotorDirectionDebugger.java | 93 --- .../drive/opmode/SplineTest.java | 38 - .../drive/opmode/StrafeTest.java | 46 -- .../drive/opmode/StraightTest.java | 46 -- .../drive/opmode/TrackWidthTuner.java | 88 --- .../TrackingWheelForwardOffsetTuner.java | 104 --- .../TrackingWheelLateralDistanceTuner.java | 130 ---- .../drive/opmode/TurnTest.java | 27 - .../sequencesegment/SequenceSegment.java | 41 -- .../sequencesegment/TrajectorySegment.java | 20 - .../sequencesegment/TurnSegment.java | 36 - .../sequencesegment/WaitSegment.java | 12 - .../util/AssetsTrajectoryManager.java | 70 -- .../trajectorysequence/util/AxesSigns.java | 45 -- .../util/AxisDirection.java | 8 - .../util/DashboardUtil.java | 54 -- .../trajectorysequence/util/Encoder.java | 125 ---- .../trajectorysequence/util/LogFiles.java | 273 ------- .../trajectorysequence/util/LoggingUtil.java | 60 -- .../util/LynxModuleUtil.java | 124 ---- .../util/RegressionUtil.java | 156 ---- build.dependencies.gradle | 7 +- build.gradle | 5 + 40 files changed, 10 insertions(+), 4282 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleTankDrive.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/StandardTrackingWheelLocalizer.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/BackAndForth.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/DriveVelocityPIDTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/FollowerPIDTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/LocalizationTest.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/ManualFeedforwardTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxAngularVeloTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxVelocityTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MotorDirectionDebugger.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/SplineTest.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StrafeTest.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackWidthTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelForwardOffsetTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelLateralDistanceTuner.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TurnTest.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AssetsTrajectoryManager.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxesSigns.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxisDirection.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/DashboardUtil.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/Encoder.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LogFiles.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LoggingUtil.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LynxModuleUtil.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/RegressionUtil.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 937238c1d2de..f68560c74393 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -27,7 +27,6 @@ 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; @@ -70,7 +69,7 @@ public void runOpMode() { robotTop.setArmStretchPosition(armStretchPos); robotTop.setArmTurnPosition(armTurnPos); robotTop.setContainerServoPosition(0.35); - robotTop.setLiftServoPosition(0.6); + robotTop.setLiftServoPosition(0.2); while (opModeIsActive()) { robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); @@ -142,15 +141,15 @@ public void runOpMode() { if(!topServoOut){ robotTop.setTopServoPosition(0.05); } - robotTop.setLeftPower(-0.3); + robotTop.setLeftPower(-0.5); } else if (gamepad1.right_trigger != 0) { if(!topServoOut){ robotTop.setTopServoPosition(0.05); } robotTop.setLeftPower(0.5); } else { - if(LIFT_HANGING_POSITION - 100 <= liftPosition && liftPosition <= LIFT_HANGING_POSITION){ - robotTop.setLeftPower(0.03); + if(LIFT_TOP_POSITION - 150 <= liftPosition && liftPosition <= LIFT_TOP_POSITION){ + robotTop.setLeftPower(0.2); }else{ robotTop.setLeftPower(0); } @@ -268,7 +267,6 @@ 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/RoadRunner/trajectorysequence/EmptySequenceException.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java deleted file mode 100644 index d5071362c671..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/EmptySequenceException.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence; - - -public class EmptySequenceException extends RuntimeException { } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java deleted file mode 100644 index 64d3df5bd8f0..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequence.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence; - -import com.acmerobotics.roadrunner.geometry.Pose2d; - -import org.firstinspires.ftc.teamcode.RoadRunner.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/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java deleted file mode 100644 index 0b153bbe7b34..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceBuilder.java +++ /dev/null @@ -1,679 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.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.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; -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/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java deleted file mode 100644 index bcfa8698a008..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/TrajectorySequenceRunner.java +++ /dev/null @@ -1,306 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.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.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; -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/RoadRunner/trajectorysequence/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java deleted file mode 100644 index 5608ac17e836..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/DriveConstants.java +++ /dev/null @@ -1,97 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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 = 500; - public static final double MAX_RPM = 300; - - /* - * 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 = 3; // in (轮子半径 单位：英寸) - 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 - * 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.01; - public static double kStatic = 0.05; - - /* - * 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. - */ - //TODO:ADJUST THESE NUMBERS. - 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); - - /* - * 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/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java deleted file mode 100644 index 67ce1abaf000..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleMecanumDrive.java +++ /dev/null @@ -1,299 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.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; - -/* - * Simple mecanum drive hardware implementation for REV hardware. - */ -@Config -public class SampleMecanumDrive extends MecanumDrive { - 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 = 0.357124; - - public static double VX_WEIGHT = 1; - public static double VY_WEIGHT = 1; - public static double OMEGA_WEIGHT = 1.3; - - private TrajectorySequenceRunner trajectorySequenceRunner; - - 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; - - 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(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); - - LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap); - - batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next(); - - for (LynxModule module : hardwareMap.getAll(LynxModule.class)) { - module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); - } - - // 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, "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); - - for (DcMotorEx motor : motors) { - MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); - motorConfigurationType.setAchieveableMaxRPMFraction(1.0); - motor.setMotorType(motorConfigurationType); - } - - if (DriveConstants.RUN_USING_ENCODER) { - setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } - - setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - 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() - - 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, - DriveConstants.MAX_ANG_VEL, DriveConstants.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(DriveConstants.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(DriveConstants.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/RoadRunner/trajectorysequence/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleTankDrive.java deleted file mode 100644 index a9ee7df54da3..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/SampleTankDrive.java +++ /dev/null @@ -1,293 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.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; - -/* - * 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(DriveConstants.MAX_VEL, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH); - private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(DriveConstants.MAX_ACCEL); - - private TrajectoryFollower follower; - - private List motors, leftMotors, rightMotors; - private IMU imu; - - private VoltageSensor batteryVoltageSensor; - - public SampleTankDrive(HardwareMap hardwareMap) { - 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); - - 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 (DriveConstants.RUN_USING_ENCODER) { - setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } - - setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - 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() - - // 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, - DriveConstants.MAX_ANG_VEL, DriveConstants.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 += DriveConstants.encoderTicksToInches(leftMotor.getCurrentPosition()); - } - for (DcMotorEx rightMotor : rightMotors) { - rightSum += DriveConstants.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 += DriveConstants.encoderTicksToInches(leftMotor.getVelocity()); - } - for (DcMotorEx rightMotor : rightMotors) { - rightSum += DriveConstants.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/RoadRunner/trajectorysequence/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/StandardTrackingWheelLocalizer.java deleted file mode 100644 index 0b59fd321f5d..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/StandardTrackingWheelLocalizer.java +++ /dev/null @@ -1,99 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java deleted file mode 100644 index b3a0ec77e2e9..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/AutomaticFeedforwardTuner.java +++ /dev/null @@ -1,221 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; - -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; -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.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; - -/* - * 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. - */ - -@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/RoadRunner/trajectorysequence/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/BackAndForth.java deleted file mode 100644 index 5571ba48330f..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/BackAndForth.java +++ /dev/null @@ -1,52 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/DriveVelocityPIDTuner.java deleted file mode 100644 index 66abd1a5b4ee..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/DriveVelocityPIDTuner.java +++ /dev/null @@ -1,171 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; - -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; -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.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/FollowerPIDTuner.java deleted file mode 100644 index c1961726bfd5..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/FollowerPIDTuner.java +++ /dev/null @@ -1,55 +0,0 @@ -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.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 - * 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/RoadRunner/trajectorysequence/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/LocalizationTest.java deleted file mode 100644 index be5aa14ad4cf..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/LocalizationTest.java +++ /dev/null @@ -1,45 +0,0 @@ -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.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/ManualFeedforwardTuner.java deleted file mode 100644 index 9bf139e48b80..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/ManualFeedforwardTuner.java +++ /dev/null @@ -1,152 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.drive.opmode; - -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.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.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/drive/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxAngularVeloTuner.java deleted file mode 100644 index 54223ab82192..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxAngularVeloTuner.java +++ /dev/null @@ -1,73 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxVelocityTuner.java deleted file mode 100644 index ff9211b0c34a..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MaxVelocityTuner.java +++ /dev/null @@ -1,84 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.RoadRunner.trajectorysequence.drive.DriveConstants; -import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/drive/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MotorDirectionDebugger.java deleted file mode 100644 index 9b73a1de7a75..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/MotorDirectionDebugger.java +++ /dev/null @@ -1,93 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/SplineTest.java deleted file mode 100644 index e1d36ffa39b8..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/SplineTest.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StrafeTest.java deleted file mode 100644 index 77df5b71fa4d..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StrafeTest.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java deleted file mode 100644 index 28254cbb5bd6..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/StraightTest.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.RoadRunner.trajectorysequence.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 = 20; // 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/RoadRunner/trajectorysequence/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackWidthTuner.java deleted file mode 100644 index d0f301b9596d..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackWidthTuner.java +++ /dev/null @@ -1,88 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.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 - * 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/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelForwardOffsetTuner.java deleted file mode 100644 index d6c2dc649e90..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelForwardOffsetTuner.java +++ /dev/null @@ -1,104 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.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. - * 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/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelLateralDistanceTuner.java deleted file mode 100644 index e0b22b087a96..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TrackingWheelLateralDistanceTuner.java +++ /dev/null @@ -1,130 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.RoadRunner.trajectorysequence.drive.SampleMecanumDrive; -import org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TurnTest.java deleted file mode 100644 index 38beecf9dac5..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/drive/opmode/TurnTest.java +++ /dev/null @@ -1,27 +0,0 @@ -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.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java deleted file mode 100644 index 5c8e74c3d0b2..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/SequenceSegment.java +++ /dev/null @@ -1,41 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.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/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java deleted file mode 100644 index cfbfa49b5188..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TrajectorySegment.java +++ /dev/null @@ -1,20 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.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/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java deleted file mode 100644 index c46c54769b3c..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/TurnSegment.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.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/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java deleted file mode 100644 index 18337d05e416..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/sequencesegment/WaitSegment.java +++ /dev/null @@ -1,12 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.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/RoadRunner/trajectorysequence/util/AssetsTrajectoryManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AssetsTrajectoryManager.java deleted file mode 100644 index 8d2d4b2469fa..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AssetsTrajectoryManager.java +++ /dev/null @@ -1,70 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/util/AxesSigns.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxesSigns.java deleted file mode 100644 index a7f3bd409a7f..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxesSigns.java +++ /dev/null @@ -1,45 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/util/AxisDirection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxisDirection.java deleted file mode 100644 index 5a205bda5513..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/AxisDirection.java +++ /dev/null @@ -1,8 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/util/DashboardUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/DashboardUtil.java deleted file mode 100644 index 17265ab2fc7c..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/DashboardUtil.java +++ /dev/null @@ -1,54 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/util/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/Encoder.java deleted file mode 100644 index 908b90375dc9..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/Encoder.java +++ /dev/null @@ -1,125 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LogFiles.java deleted file mode 100644 index e28fa31ca282..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LogFiles.java +++ /dev/null @@ -1,273 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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.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; -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/RoadRunner/trajectorysequence/util/LoggingUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LoggingUtil.java deleted file mode 100644 index b7776cfcc5f8..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LoggingUtil.java +++ /dev/null @@ -1,60 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/util/LynxModuleUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LynxModuleUtil.java deleted file mode 100644 index b9de338346ae..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/LynxModuleUtil.java +++ /dev/null @@ -1,124 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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/RoadRunner/trajectorysequence/util/RegressionUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/RegressionUtil.java deleted file mode 100644 index ca227dfda7d7..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RoadRunner/trajectorysequence/util/RegressionUtil.java +++ /dev/null @@ -1,156 +0,0 @@ -package org.firstinspires.ftc.teamcode.RoadRunner.trajectorysequence.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 1ec9869b3ed6..e3c567ad65b5 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -14,9 +14,4 @@ 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' - } -} - +} \ No newline at end of file diff --git a/build.gradle b/build.gradle index e8f79f72f864..e548c0c70b00 100644 --- a/build.gradle +++ b/build.gradle @@ -40,3 +40,8 @@ repositories { maven { url "https://repo.sparkfun.com/artifactory/sparkfun-repo" } // 假设这是SparkFun的仓库URL } +repositories { + maven { + url = 'https://maven.brott.dev/' + } +} From 74162652a04c0df342c1cb591811f06d6287ce22 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sun, 8 Dec 2024 17:14:34 +0800 Subject: [PATCH 113/143] Feat:Completed the fix --- .../ftc/teamcode/test/RRTest.java | 58 ------------------- 1 file changed, 58 deletions(-) delete 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 deleted file mode 100644 index 084bfec67f0e..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/RRTest.java +++ /dev/null @@ -1,58 +0,0 @@ -package org.firstinspires.ftc.teamcode.test; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -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; - -@Disabled -@Autonomous(group = "Test") -public class RRTest extends LinearOpMode { - - private SampleMecanumDrive drive; - private SparkFunOTOSSensor otosSensor; - - @Override - public void runOpMode() throws InterruptedException { - // 初始化驱动器和传感器 - drive = new SampleMecanumDrive(hardwareMap); - // 创建轨迹序列 -// 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(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(); - - if (isStopRequested()) return; - - // 执行轨迹移动 - drive.followTrajectorySequence(trajectorySequence); - - - } -} From 5880e48cd184f69a5f478bfa0ab0db4211333976 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Thu, 26 Dec 2024 15:46:52 +0800 Subject: [PATCH 114/143] Feat:Delete Meepmeep. --- MeepMeep/.gitignore | 1 - MeepMeep/build.gradle | 17 ------ .../java/com/example/meepmeep/MeepTurbo.java | 55 ------------------- .../ftc/teamcode/RedLeftAuto.java | 11 ---- 4 files changed, 84 deletions(-) delete mode 100644 MeepMeep/.gitignore delete mode 100644 MeepMeep/build.gradle delete mode 100644 MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java diff --git a/MeepMeep/.gitignore b/MeepMeep/.gitignore deleted file mode 100644 index 42afabfd2abe..000000000000 --- a/MeepMeep/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/build \ No newline at end of file diff --git a/MeepMeep/build.gradle b/MeepMeep/build.gradle deleted file mode 100644 index cb8c19758d0b..000000000000 --- a/MeepMeep/build.gradle +++ /dev/null @@ -1,17 +0,0 @@ -plugins { - id 'java-library' -} - -java { - sourceCompatibility = JavaVersion.VERSION_17 - targetCompatibility = JavaVersion.VERSION_17 -} - -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/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java b/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java deleted file mode 100644 index e851a14aeeb6..000000000000 --- a/MeepMeep/src/main/java/com/example/meepmeep/MeepTurbo.java +++ /dev/null @@ -1,55 +0,0 @@ -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) - // Required: Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width - .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(59.71, 36.96, Math.toRadians(153.15))) -// .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(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) - // Background opacity from 0-1 - .setBackgroundAlpha(0.95f) - .addEntity(myBot) - .start(); - } -} - 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 efb7ac891d29..732938fa08b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java @@ -16,17 +16,6 @@ public void runOpMode(){ 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(); } } From 2c4244f3c622f59097c7fb0629f628c6ac55ddcd Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Thu, 26 Dec 2024 20:17:19 +0800 Subject: [PATCH 115/143] pid test --- .../advancedManual/test/ServoMgrTest.java | 15 ------ .../ftc/teamcode/hardware/RobotAuto.java | 4 +- .../ftc/teamcode/hardware/RobotChassis.java | 2 +- .../ftc/teamcode/test/AutoTest.java | 4 +- .../ftc/teamcode/test/PIDControllerTest.java | 48 +++++++++++++++++++ 5 files changed, 52 insertions(+), 21 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java 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 7a4333668fad..a66ddc3191e9 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 @@ -83,18 +83,3 @@ private int findMinFreeTaskId() { 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/hardware/RobotAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAuto.java index 5757ad6f73d5..788d63a6204e 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()); } 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 34892e780196..3bd5ec67fb3c 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 @@ -33,7 +33,7 @@ public void initMovement() { leftFrontDrive.setDirection(DcMotor.Direction.FORWARD); leftBackDrive.setDirection(DcMotor.Direction.FORWARD); - rightFrontDrive.setDirection(DcMotor.Direction.FORWARD); + rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); rightBackDrive.setDirection(DcMotor.Direction.FORWARD); leftFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); 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 d02139b6208d..987d07284d7b 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 @@ -17,8 +17,6 @@ public void runOpMode() { robotAuto = new RobotAuto(this); waitForStart(); resetRuntime(); - while (getRuntime() <= 4){ - robotChassis.driveRobot(0,-0.5,0); - } + robotAuto.fastForward(24); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java new file mode 100644 index 000000000000..bf1216a424d8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java @@ -0,0 +1,48 @@ +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 org.firstinspires.ftc.teamcode.hardware.RobotTop; + +@TeleOp +public class PIDControllerTest extends LinearOpMode { + @Override + public void runOpMode(){ + RobotTop robotTop = new RobotTop(this); + + waitForStart(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + double currentPos = 0, previousPos; + double targetPos = 1000; + double delta; + double p = 0, i = 0 , d = 0; + final double Kp = 0.014, Ki = 0.000013, Kd = -0.01; + double power; + + while (opModeIsActive()){ + robotTop.setTopServoPosition(0.05); + if(gamepad1.a) { + previousPos = currentPos; + currentPos = robotTop.getLiftPosition(); + delta = targetPos - currentPos; + p = Kp * delta; + i += Ki * delta; + d = Kd * (currentPos - previousPos); + power = Math.min(p + i + d, 0.8); + robotTop.setLeftPower(power); + previousGamepad1.copy(gamepad1); + telemetry.addData("power",power); + telemetry.addData("p",p); + telemetry.addData("i",i); + telemetry.addData("d",d); + telemetry.update(); + sleep(10); + }else { + robotTop.setLeftPower(0); + } + } + } +} From 0f60bcc0b3ce2d892f29e56743da35f65d862a85 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Wed, 1 Jan 2025 01:32:05 +0800 Subject: [PATCH 116/143] Update FTC SDK, Gradle version --- FtcRobotController/build.gradle | 6 ++++- .../src/main/AndroidManifest.xml | 4 +-- .../samples/BasicOmniOpMode_Linear.java | 20 +++++++-------- README.md | 21 +++++++++++++++- TeamCode/build.gradle | 5 +--- build.common.gradle | 6 +---- build.dependencies.gradle | 20 +++++++-------- build.gradle | 25 +++++-------------- gradle.properties | 2 ++ gradle/wrapper/gradle-wrapper.properties | 3 +-- settings.gradle | 1 - 11 files changed, 58 insertions(+), 55 deletions(-) diff --git a/FtcRobotController/build.gradle b/FtcRobotController/build.gradle index e25651fbb315..9fa21699dd27 100644 --- a/FtcRobotController/build.gradle +++ b/FtcRobotController/build.gradle @@ -14,7 +14,11 @@ android { buildConfigField "String", "APP_BUILD_TIME", '"' + (new SimpleDateFormat("yyyy-MM-dd'T'HH:mm:ss.SSSZ", Locale.ROOT).format(new Date())) + '"' } - compileSdkVersion 29 + buildFeatures { + buildConfig = true + } + + compileSdkVersion 30 compileOptions { sourceCompatibility JavaVersion.VERSION_1_8 diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index 787878b92fe7..1ce6a3e16173 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -1,8 +1,8 @@ + android:versionCode="57" + android:versionName="10.1.1"> 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..1d14dfb76035 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(teleoperation遥控的) 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 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.注意，麦克纳姆轮正确安装时，从上往下看起来应该像X + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. * - * 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; diff --git a/README.md b/README.md index f7a02c95f849..d5856cc43cef 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ This repository contains the public FTC SDK for the INTO THE DEEP (2024-2025) co This GitHub repository contains the source code that is used to build an Android app to control a *FIRST* Tech Challenge competition robot. To use this SDK, download/clone the entire project to your local computer. ## Requirements -To use this Android Studio project, you will need Android Studio 2021.2 (codename Chipmunk) or later. +To use this Android Studio project, you will need Android Studio Ladybug (2024.2) or later. To program your robot in Blocks or OnBot Java, you do not need Android Studio. @@ -59,6 +59,25 @@ The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc # Release Information +## Version 10.1.1 (20241102-092223) + +### Breaking Changes + +* Support for Android Studio Ladybug. Requires Android Studio Ladybug. + +### Known Issues + +* Android Studio Ladybug's bundled JDK is version 21. JDK 21 has deprecated support for Java 1.8, and Ladybug will warn on this deprecation. + OnBotJava only supports Java 1.8, therefore, in order to ensure that software developed using Android Studio will + run within the OnBotJava environment, the targetCompatibility and sourceCompatibility versions for the SDK have been left at VERSION_1_8. + FIRST has decided that until it can devote the resources to migrating OnBotJava to a newer version of Java, the deprecation is the + lesser of two non-optimal situations. + +### Enhancements + +* Added `toString()` method to Pose2D +* Added `toString()` method to SparkFunOTOS.Pose2D + ## Version 10.1 (20240919-122750) ### Enhancements diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 8a4e34ccc19d..f90472c6ae28 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -25,7 +25,4 @@ 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.6' -} +} \ No newline at end of file diff --git a/build.common.gradle b/build.common.gradle index 046a4a163f93..a1d2fa4ad51c 100644 --- a/build.common.gradle +++ b/build.common.gradle @@ -21,7 +21,7 @@ apply plugin: 'com.android.application' android { - compileSdkVersion 29 + compileSdkVersion 30 signingConfigs { release { @@ -109,10 +109,6 @@ android { packagingOptions { pickFirst '**/*.so' } - sourceSets.main { - jni.srcDirs = [] - jniLibs.srcDir rootProject.file('libs') - } ndkVersion '21.3.6528147' } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index e3c567ad65b5..593b31a69a1e 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -1,17 +1,17 @@ repositories { mavenCentral() google() // Needed for androidx - maven { url = 'https://maven.brott.dev/' } } dependencies { - 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 'org.firstinspires.ftc:Inspection:10.1.1' + implementation 'org.firstinspires.ftc:Blocks:10.1.1' + implementation 'org.firstinspires.ftc:RobotCore:10.1.1' + implementation 'org.firstinspires.ftc:RobotServer:10.1.1' + implementation 'org.firstinspires.ftc:OnBotJava:10.1.1' + implementation 'org.firstinspires.ftc:Hardware:10.1.1' + implementation 'org.firstinspires.ftc:FtcCommon:10.1.1' + implementation 'org.firstinspires.ftc:Vision:10.1.1' implementation 'androidx.appcompat:appcompat:1.2.0' -} \ No newline at end of file +} + diff --git a/build.gradle b/build.gradle index e548c0c70b00..aa3a3c17a501 100644 --- a/build.gradle +++ b/build.gradle @@ -6,14 +6,14 @@ 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() } dependencies { // Note for FTC Teams: Do not modify this yourself. - classpath 'com.android.tools.build:gradle:7.4.2' + classpath 'com.android.tools.build:gradle:8.7.0' } } @@ -21,27 +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() } - -repositories { - mavenCentral() - // 你可以添加其他需要的仓库，例如： - maven { url "https://jitpack.io" } - maven { url "https://repo.sparkfun.com/artifactory/sparkfun-repo" } // 假设这是SparkFun的仓库URL -} - -repositories { - maven { - url = 'https://maven.brott.dev/' - } -} diff --git a/gradle.properties b/gradle.properties index 7a370c51bbe7..f5935e913f4b 100644 --- a/gradle.properties +++ b/gradle.properties @@ -8,3 +8,5 @@ android.enableJetifier=false # Allow Gradle to use up to 1 GB of RAM org.gradle.jvmargs=-Xmx1024M + +android.nonTransitiveRClass=false \ No newline at end of file diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index 7cc7ca52760a..19cfad969baf 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,6 +1,5 @@ -#Sun Dec 08 12:29:19 CST 2024 distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.9-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists diff --git a/settings.gradle b/settings.gradle index a56e34042da1..9e2cfb3b4b03 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,3 +1,2 @@ include ':FtcRobotController' include ':TeamCode' -include ':MeepMeep' From 44235a48a3c32f5afccf5a393d2cfe8f0a9cfa40 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Wed, 15 Jan 2025 21:13:47 +0800 Subject: [PATCH 117/143] Prepare for the new robot --- .../ftc/teamcode/ManualOpMode.java | 40 +++---- .../advancedManual/AdvancedManual.java | 102 +++++++++++++++++- .../advancedManual/ArmStateMachine.java | 16 +-- .../test/RobotStateMachine.java | 2 +- .../ftc/teamcode/hardware/RobotAuto.java | 2 +- .../ftc/teamcode/hardware/RobotTop.java | 58 ++++++---- .../ftc/teamcode/test/Armtest.java | 47 ++++++++ .../ftc/teamcode/test/PIDControllerTest.java | 4 +- .../ftc/teamcode/test/PIDControllerTest2.java | 53 +++++++++ .../ftc/teamcode/test/ServoTest.java | 16 +-- 10 files changed, 273 insertions(+), 67 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.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..a5b9f18a88d2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -28,8 +28,8 @@ public class ManualOpMode extends LinearOpMode { final int LIFT_TOP_POSITION = 1250; final int LIFT_BOTTOM_POSITION = 50; - final double STRETCH_BACK_POSITION = 0.03; - final double STRETCH_OUT_POSITION = 0.3; + final int STRETCH_BACK_POSITION = 0; // TODO: change it + final int STRETCH_OUT_POSITION = 0; // TODO: change it final double SPIN_X_DEFAULT_POSITION = 0.22; final double SPIN_X_HOVERING_POSITION = 0.53; final double SPIN_X_DOWN_POSITION = 0.58; @@ -52,7 +52,7 @@ public void runOpMode() { previousGamepad2.copy(gamepad2); int liftPosition; - double armStretchPos = STRETCH_BACK_POSITION; + int armStretchPos = STRETCH_BACK_POSITION; double armTurnPos = TURN_BACK_POSITION; double armSpinXPos = SPIN_X_DEFAULT_POSITION; double armSpinYPos = SPIN_Y_DEFAULT_POSITION; @@ -67,7 +67,7 @@ public void runOpMode() { waitForStart(); robotTop.setArmStretchPosition(armStretchPos); - robotTop.setArmTurnPosition(armTurnPos); + robotTop.setArmLeftTurnPosition(armTurnPos); robotTop.setContainerServoPosition(0.35); robotTop.setLiftServoPosition(0.2); while (opModeIsActive()) { @@ -181,7 +181,7 @@ public void runOpMode() { robotTop.setArmStretchPosition(armStretchPos); } else if (armState == ArmState.TURNED) { armState = ArmState.TURNING_BACK; - robotTop.setArmSpinYPosition(SPIN_Y_DEFAULT_POSITION); + robotTop.setArmLeftSpinPosition(SPIN_Y_DEFAULT_POSITION); } } if (armState == ArmState.WITHDRAWING) { @@ -195,23 +195,23 @@ public void runOpMode() { if (armState == ArmState.TURNING_OUT) { if (armTurnPos >= TURN_OUT_HOVERING_POSITION - 0.05) { armTurnPos = TURN_OUT_HOVERING_POSITION; - robotTop.setArmTurnPosition(armTurnPos); - robotTop.setArmSpinXPosition(SPIN_X_HOVERING_POSITION); + robotTop.setArmLeftTurnPosition(armTurnPos); + robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); armState = ArmState.TURNED; } else { armTurnPos += 0.03; - robotTop.setArmTurnPosition(armTurnPos); + robotTop.setArmLeftTurnPosition(armTurnPos); } } if (armState == ArmState.TURNING_BACK) { if (armTurnPos <= TURN_BACK_POSITION + 0.05) { armTurnPos = TURN_BACK_POSITION; - robotTop.setArmTurnPosition(armTurnPos); - robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION); + robotTop.setArmLeftTurnPosition(armTurnPos); + robotTop.setArmRightTurnPosition(SPIN_X_DEFAULT_POSITION); armState = ArmState.WITHDRAWING; } else { armTurnPos -= 0.03; - robotTop.setArmTurnPosition(armTurnPos); + robotTop.setArmLeftTurnPosition(armTurnPos); } } if (armState == ArmState.STRETCHED) { @@ -224,8 +224,8 @@ public void runOpMode() { robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); sleep(500); if (!armGrabbing && armState == ArmState.TURNED) { - robotTop.setArmTurnPosition(TURN_OUT_DOWN_POSITION); - robotTop.setArmSpinXPosition(SPIN_X_DOWN_POSITION); + robotTop.setArmLeftTurnPosition(TURN_OUT_DOWN_POSITION); + robotTop.setArmRightTurnPosition(SPIN_X_DOWN_POSITION); grabbingFlag = true; } else { robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); @@ -235,25 +235,25 @@ public void runOpMode() { if (grabbingFlag && !gamepad1.b) { robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); sleep(500); - robotTop.setArmTurnPosition(TURN_OUT_HOVERING_POSITION); - robotTop.setArmSpinXPosition(SPIN_X_HOVERING_POSITION); + robotTop.setArmLeftTurnPosition(TURN_OUT_HOVERING_POSITION); + robotTop.setArmRightTurnPosition(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); + robotTop.setArmRightTurnPosition(armSpinXPos); } else if (gamepad1.dpad_down) { armSpinXPos = Math.max(0, armSpinXPos - 0.02); - robotTop.setArmSpinXPosition(armSpinXPos); + robotTop.setArmRightTurnPosition(armSpinXPos); } if (gamepad1.dpad_right) { armSpinYPos = Math.min(1, armSpinYPos + 0.05); - robotTop.setArmSpinYPosition(armSpinYPos); + robotTop.setArmLeftSpinPosition(armSpinYPos); } else if (gamepad1.dpad_left) { armSpinYPos = Math.max(0, armSpinYPos - 0.05); - robotTop.setArmSpinYPosition(armSpinYPos); + robotTop.setArmLeftSpinPosition(armSpinYPos); } if (gamepad1.a && !previousGamepad1.a) { if (backGrabOpen) { @@ -268,7 +268,7 @@ public void runOpMode() { if ((gamepad2.left_trigger != 0 && previousGamepad2.left_trigger == 0 ) || (gamepad2.right_trigger != 0 && previousGamepad2.right_trigger == 0)) { recognitionAngle = robotVisionAngle.getDetectedAngle(); - robotTop.setArmSpinYPosition(calculateSpinY(recognitionAngle)); + robotTop.setArmLeftSpinPosition(calculateSpinY(recognitionAngle)); } // telemetry 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 b7726e8508b9..693d3b2c1561 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 @@ -4,25 +4,117 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.Gamepad; +import org.firstinspires.ftc.teamcode.hardware.RobotChassis; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + @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 { + enum ArmState{ + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK + } + RobotTop robotTop; + RobotChassis robotChassis; + ArmStateMachine.ArmState armState; + Gamepad gamepad1; + Gamepad previousGamepad1; + Gamepad gamepad2; + + //TODO: test the constants + final int STRETCH_BACK_POSITION = 0; + final int STRETCH_OUT_POSITION = 0; + final double SPIN_DEFAULT_POSITION = 0; + final double SPIN_HOVERING_POSITION = 0; + final double SPIN_DOWN_POSITION = 0; + final double TURN_BACK_POSITION = 0; + final double TURN_HOVERING_POSITION = 0; + final double TURN_DOWN_POSITION = 0; + final double GRAB_OPEN_POSITION = 0; + final double GRAB_CLOSE_POSITION = 0; + + boolean isGrabbing; @Override public void runOpMode(){ - TaskManager servoMgr = new TaskManager(); Gamepad previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); + this.robotTop = new RobotTop(this); + this.robotChassis = new RobotChassis(this); + this.armState = ArmStateMachine.ArmState.IDLE; + this.isGrabbing = false; waitForStart(); if (isStopRequested()) return; while (opModeIsActive()) { - //code - - servoMgr.updateServos(); - previousGamepad1.copy(gamepad1); + robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); + switch (armState){ + case IDLE: + handleIdleState(); + case TURNING_OUT: + handleTurningOutState(); + case TURNED: + handleTurnedState(); + case TURNING_BACK: + handleTurningBackState(); + case WITHDRAWING: + handleWithdrawingState(); + } + this.previousGamepad1.copy(gamepad1); sleep(50); } + + } + protected void handleIdleState(){ + if(gamepad1.x && !previousGamepad1.x){ + robotTop.setArmStretchPosition(STRETCH_OUT_POSITION); + robotTop.setArmLeftTurnPosition(TURN_HOVERING_POSITION); + robotTop.setArmRightTurnPosition(TURN_HOVERING_POSITION); + robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION); + robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION); + armState = ArmStateMachine.ArmState.TURNED; + } + } + protected void handleTurningOutState(){ + // if necessary + } + protected void handleTurnedState(){ + if(gamepad1.x && !previousGamepad1.x){ + robotTop.setArmStretchPosition(STRETCH_BACK_POSITION); + robotTop.setArmLeftTurnPosition(TURN_BACK_POSITION); + robotTop.setArmRightTurnPosition(TURN_BACK_POSITION); + robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); + robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); + armState = ArmStateMachine.ArmState.IDLE; + } + if(gamepad1.b && !previousGamepad1.b){ + if(isGrabbing){ + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + }else { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + } + } + if(gamepad1.dpad_up){ + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.002)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.002)); + } + if(gamepad1.dpad_down){ + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.002)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.002)); + } + if(gamepad1.dpad_left){ + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.002)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.002)); + } + if(gamepad1.dpad_right){ + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.002)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.002)); + } + } + protected void handleTurningBackState(){ + //if necessary + } + protected void handleWithdrawingState(){ + //if necessary } } 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 2941740faa0f..a709fc3f953f 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,6 +2,7 @@ import com.qualcomm.robotcore.hardware.Gamepad; +import org.firstinspires.ftc.teamcode.hardware.RobotChassis; import org.firstinspires.ftc.teamcode.hardware.RobotTop; public class ArmStateMachine { @@ -9,9 +10,11 @@ enum ArmState{ IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK } RobotTop robotTop; + RobotChassis robotChassis; ArmState armState; Gamepad gamepad1; Gamepad previousGamepad1; + Gamepad gamepad2; TaskManager taskMgr; final double STRETCH_BACK_POSITION = 0.03; @@ -30,8 +33,9 @@ enum ArmState{ double armTurnPos; double armSpinXPos; double armSpinYPos; - public ArmStateMachine(RobotTop robotTop){ + public ArmStateMachine(RobotTop robotTop, RobotChassis robotChassis){ this.robotTop = robotTop; + this.robotChassis = robotChassis; this.armState = ArmState.IDLE; this.taskMgr = new TaskManager(); } @@ -47,12 +51,15 @@ protected void init(){ boolean containerRelease = false; double recognitionAngle = 0; } - public void receiveGamepad(Gamepad gamepad1, Gamepad previousGamepad1){ + public void receiveGamepad(Gamepad gamepad1, Gamepad previousGamepad1, + Gamepad gamepad2){ this.gamepad1 = gamepad1; + this.gamepad2 = gamepad2; this.previousGamepad1 = previousGamepad1; } public void update(){ + robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); switch (armState){ case IDLE: handleIdleState(); @@ -67,11 +74,6 @@ public void update(){ } } protected void handleIdleState(){ - if(gamepad1.x && !previousGamepad1.x){ - armStretchPos = STRETCH_OUT_POSITION; - robotTop.setArmStretchPosition(armStretchPos); - robotTop.setTopServoPosition(0); - } } protected void handleTurningOutState(){} protected void handleTurnedState(){} 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 index 8ace175db1c6..b556fc1c1675 100644 --- 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 @@ -60,7 +60,7 @@ protected void init(){ private void initializeRobotPositions() { robotTop.setArmStretchPosition(armStretchPos); - robotTop.setArmTurnPosition(armTurnPos); + robotTop.setArmLeftTurnPosition(armTurnPos); robotTop.setContainerServoPosition(CONTAINER_OPEN_POSITION); robotTop.setLiftServoPosition(LIFT_SERVO_CLOSE); } 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 788d63a6204e..e7f5c346ecbc 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 @@ -350,7 +350,7 @@ public RobotAuto gotoPosition2(double x, double y, double h) { } public RobotAuto stretchArm(){ - robotTop.setArmStretchPosition(0.3); + robotTop.setArmStretchPosition(0); return this; } public RobotAuto setLiftPower(double power){ 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 480af2c51b82..d20ef1331307 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 @@ -16,10 +16,11 @@ public class RobotTop { Telemetry telemetry; protected DcMotor leftLiftMotor; protected DcMotor rightLiftMotor; - protected Servo armStretchServo; - protected Servo armTurnServo; - protected Servo armSpinXServo; - protected Servo armSpinYServo; + protected DcMotor armStretchMotor; + protected Servo armLeftTurnServo; + protected Servo armRightTurnServo; + protected Servo armLeftSpinServo; + protected Servo armRightSpinServo; protected Servo armGrabServo; protected Servo liftServo; protected Servo topServo; @@ -34,10 +35,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, "armStretch"); - armTurnServo = hardwareMap.get(Servo.class, "armTurn"); - armSpinXServo = hardwareMap.get(Servo.class, "armSpinX"); - armSpinYServo = hardwareMap.get(Servo.class, "armSpinY"); + armStretchMotor = hardwareMap.get(DcMotor.class, "armStretch"); + armLeftTurnServo = hardwareMap.get(Servo.class, "armTurnL"); + armRightTurnServo = hardwareMap.get(Servo.class, "armTurnR"); + armLeftSpinServo = hardwareMap.get(Servo.class, "armSpinL"); + armRightSpinServo = hardwareMap.get(Servo.class, "armSpinR"); armGrabServo = hardwareMap.get(Servo.class, "armGrab"); liftServo = hardwareMap.get(Servo.class, "liftServo"); topServo = hardwareMap.get(Servo.class, "liftTop"); @@ -53,6 +55,8 @@ public void init() { leftLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rightLiftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + armStretchMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); } public void setLeftPower(double power) { @@ -64,36 +68,44 @@ public int getLiftPosition() { return leftLiftMotor.getCurrentPosition(); } - public void setArmStretchPosition(double position) { - armStretchServo.setPosition(position); + public void setArmStretchPosition(int position) { + armStretchMotor.setTargetPosition(position); } public double getArmStretchPosition() { - return armStretchServo.getPosition(); + return armStretchMotor.getCurrentPosition(); + } + + public void setArmLeftTurnPosition(double position) { + armLeftTurnServo.setPosition(position); + } + + public double getArmLeftTurnPosition() { + return armLeftTurnServo.getPosition(); } - public void setArmTurnPosition(double position) { - armTurnServo.setPosition(position); + public void setArmRightTurnPosition(double position) { + armRightTurnServo.setPosition(position); } - public double getArmTurnPosition() { - return armTurnServo.getPosition(); + public double getArmRightTurnPosition() { + return armRightTurnServo.getPosition(); } - public void setArmSpinXPosition(double position) { - armSpinXServo.setPosition(position); + public void setArmLeftSpinPosition(double position) { + armLeftSpinServo.setPosition(position); } - public double getArmSpinXPosition() { - return armSpinXServo.getPosition(); + public double getArmLeftSpinPosition() { + return armLeftSpinServo.getPosition(); } - public void setArmSpinYPosition(double position) { - armSpinYServo.setPosition(position); + public void setArmRightSpinPosition(double position) { + armRightSpinServo.setPosition(position); } - public double getArmSpinYPosition() { - return armSpinYServo.getPosition(); + public double getArmRightSpinPosition() { + return armRightSpinServo.getPosition(); } public void setArmGrabPosition(double position) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java new file mode 100644 index 000000000000..ca82364ac65f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java @@ -0,0 +1,47 @@ +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(group = "Test") +public class Armtest extends LinearOpMode { + @Override + public void runOpMode(){ + Servo armServo0 = hardwareMap.get(Servo.class , "arm0"); + Servo armServo1 = hardwareMap.get(Servo.class , "arm1"); + + waitForStart(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + + if (isStopRequested()) return; + double leftPos = 0; + double rightPos = 0; + + while (opModeIsActive()) { + if(gamepad1.y){ + leftPos = Math.min(1, leftPos + 0.003); + rightPos = Math.min(1, rightPos + 0.003); + } + if(gamepad1.a){ + leftPos = Math.max(0, leftPos - 0.003); + rightPos = Math.max(0, rightPos - 0.003); + } + if(gamepad1.x){ + leftPos = Math.min(1, leftPos + 0.003); + rightPos = Math.max(0, rightPos - 0.003); + } + if(gamepad1.b){ + leftPos = Math.max(0, leftPos - 0.003); + rightPos = Math.min(1, rightPos + 0.003); + } + armServo0.setPosition(leftPos); + armServo1.setPosition(rightPos); + + sleep(10); + } + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java index bf1216a424d8..6ce8e65803ab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java @@ -19,7 +19,7 @@ public void runOpMode(){ double targetPos = 1000; double delta; double p = 0, i = 0 , d = 0; - final double Kp = 0.014, Ki = 0.000013, Kd = -0.01; + final double Kp = 0.1, Ki = 0.0, Kd = 0.0; double power; while (opModeIsActive()){ @@ -31,7 +31,7 @@ public void runOpMode(){ p = Kp * delta; i += Ki * delta; d = Kd * (currentPos - previousPos); - power = Math.min(p + i + d, 0.8); + power = Math.min(p + i + d, 1); robotTop.setLeftPower(power); previousGamepad1.copy(gamepad1); telemetry.addData("power",power); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java new file mode 100644 index 000000000000..63007e8d0d02 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java @@ -0,0 +1,53 @@ +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.Gamepad; + +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + +@TeleOp +public class PIDControllerTest2 extends LinearOpMode { + @Override + public void runOpMode(){ + DcMotor motor = hardwareMap.get(DcMotor.class, "motor"); + waitForStart(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + double currentPos = 0, previousPos; + double targetPos = 300; + double delta; + double p = 0, i = 0 , d = 0; + final double Kp = 0.02, Ki = 0.00007, Kd = -0.008, power_max = 1; + double power; + + while (opModeIsActive()){ + previousPos = currentPos; + currentPos = motor.getCurrentPosition(); + delta = targetPos - currentPos; + p = Kp * delta; + i += Ki * delta; + d = Kd * (currentPos - previousPos); + power = p + i + d; + if (power > power_max) + power = power_max; + else if(power < -power_max) + power = -power_max; + motor.setPower(power); + previousGamepad1.copy(gamepad1); + telemetry.addData("power",power); + telemetry.addData("p",p); + telemetry.addData("i",i); + telemetry.addData("d",d); + telemetry.update(); + sleep(10); + if(gamepad1.a) { + targetPos += 50; + } + if(gamepad1.b){ + targetPos -= 50; + } + } + } +} 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 6f0a70a0a1a9..1882cafff34a 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 @@ -6,12 +6,12 @@ import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; -@Disabled @TeleOp(group = "Test") public class ServoTest extends LinearOpMode { @Override public void runOpMode(){ - Servo armServo = hardwareMap.get(Servo.class , "arm"); + Servo armServo0 = hardwareMap.get(Servo.class , "arm0"); + Servo armServo1 = hardwareMap.get(Servo.class , "arm1"); //stretch(1): [0, 0.3] //turn(3): [0.38-back, 0.71-out] //grab(5): [0-open, 0.53-close] @@ -21,23 +21,23 @@ public void runOpMode(){ waitForStart(); Gamepad previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); - double servoPos = 0; + double servoPos = 0.5; while (opModeIsActive()){ boolean x = gamepad1.x; boolean b = gamepad1.b; if(x){ - servoPos += 0.01; + servoPos = Math.min(1, servoPos + 0.003); }if(b){ - servoPos -= 0.01; + servoPos = Math.max(0, servoPos - 0.003); } - - armServo.setPosition(servoPos); + armServo0.setPosition(servoPos); + armServo1.setPosition(servoPos); telemetry.addData("pos", servoPos); telemetry.update(); previousGamepad1.copy(gamepad1); - sleep(100); + sleep(10); } } } From a7c2c8401d175a1cd4220f7a98c78375149a9dc3 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Wed, 15 Jan 2025 21:13:47 +0800 Subject: [PATCH 118/143] Prepare for the new robot --- .../ftc/teamcode/ManualOpMode.java | 40 +++---- .../advancedManual/AdvancedManual.java | 102 +++++++++++++++++- .../advancedManual/ArmStateMachine.java | 16 +-- .../test/RobotStateMachine.java | 9 -- .../ftc/teamcode/hardware/RobotAuto.java | 2 +- .../ftc/teamcode/hardware/RobotTop.java | 58 ++++++---- .../ftc/teamcode/test/Armtest.java | 47 ++++++++ .../ftc/teamcode/test/PIDControllerTest.java | 4 +- .../ftc/teamcode/test/PIDControllerTest2.java | 53 +++++++++ .../ftc/teamcode/test/ServoTest.java | 16 +-- 10 files changed, 272 insertions(+), 75 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.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..a5b9f18a88d2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -28,8 +28,8 @@ public class ManualOpMode extends LinearOpMode { final int LIFT_TOP_POSITION = 1250; final int LIFT_BOTTOM_POSITION = 50; - final double STRETCH_BACK_POSITION = 0.03; - final double STRETCH_OUT_POSITION = 0.3; + final int STRETCH_BACK_POSITION = 0; // TODO: change it + final int STRETCH_OUT_POSITION = 0; // TODO: change it final double SPIN_X_DEFAULT_POSITION = 0.22; final double SPIN_X_HOVERING_POSITION = 0.53; final double SPIN_X_DOWN_POSITION = 0.58; @@ -52,7 +52,7 @@ public void runOpMode() { previousGamepad2.copy(gamepad2); int liftPosition; - double armStretchPos = STRETCH_BACK_POSITION; + int armStretchPos = STRETCH_BACK_POSITION; double armTurnPos = TURN_BACK_POSITION; double armSpinXPos = SPIN_X_DEFAULT_POSITION; double armSpinYPos = SPIN_Y_DEFAULT_POSITION; @@ -67,7 +67,7 @@ public void runOpMode() { waitForStart(); robotTop.setArmStretchPosition(armStretchPos); - robotTop.setArmTurnPosition(armTurnPos); + robotTop.setArmLeftTurnPosition(armTurnPos); robotTop.setContainerServoPosition(0.35); robotTop.setLiftServoPosition(0.2); while (opModeIsActive()) { @@ -181,7 +181,7 @@ public void runOpMode() { robotTop.setArmStretchPosition(armStretchPos); } else if (armState == ArmState.TURNED) { armState = ArmState.TURNING_BACK; - robotTop.setArmSpinYPosition(SPIN_Y_DEFAULT_POSITION); + robotTop.setArmLeftSpinPosition(SPIN_Y_DEFAULT_POSITION); } } if (armState == ArmState.WITHDRAWING) { @@ -195,23 +195,23 @@ public void runOpMode() { if (armState == ArmState.TURNING_OUT) { if (armTurnPos >= TURN_OUT_HOVERING_POSITION - 0.05) { armTurnPos = TURN_OUT_HOVERING_POSITION; - robotTop.setArmTurnPosition(armTurnPos); - robotTop.setArmSpinXPosition(SPIN_X_HOVERING_POSITION); + robotTop.setArmLeftTurnPosition(armTurnPos); + robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); armState = ArmState.TURNED; } else { armTurnPos += 0.03; - robotTop.setArmTurnPosition(armTurnPos); + robotTop.setArmLeftTurnPosition(armTurnPos); } } if (armState == ArmState.TURNING_BACK) { if (armTurnPos <= TURN_BACK_POSITION + 0.05) { armTurnPos = TURN_BACK_POSITION; - robotTop.setArmTurnPosition(armTurnPos); - robotTop.setArmSpinXPosition(SPIN_X_DEFAULT_POSITION); + robotTop.setArmLeftTurnPosition(armTurnPos); + robotTop.setArmRightTurnPosition(SPIN_X_DEFAULT_POSITION); armState = ArmState.WITHDRAWING; } else { armTurnPos -= 0.03; - robotTop.setArmTurnPosition(armTurnPos); + robotTop.setArmLeftTurnPosition(armTurnPos); } } if (armState == ArmState.STRETCHED) { @@ -224,8 +224,8 @@ public void runOpMode() { robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); sleep(500); if (!armGrabbing && armState == ArmState.TURNED) { - robotTop.setArmTurnPosition(TURN_OUT_DOWN_POSITION); - robotTop.setArmSpinXPosition(SPIN_X_DOWN_POSITION); + robotTop.setArmLeftTurnPosition(TURN_OUT_DOWN_POSITION); + robotTop.setArmRightTurnPosition(SPIN_X_DOWN_POSITION); grabbingFlag = true; } else { robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); @@ -235,25 +235,25 @@ public void runOpMode() { if (grabbingFlag && !gamepad1.b) { robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); sleep(500); - robotTop.setArmTurnPosition(TURN_OUT_HOVERING_POSITION); - robotTop.setArmSpinXPosition(SPIN_X_HOVERING_POSITION); + robotTop.setArmLeftTurnPosition(TURN_OUT_HOVERING_POSITION); + robotTop.setArmRightTurnPosition(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); + robotTop.setArmRightTurnPosition(armSpinXPos); } else if (gamepad1.dpad_down) { armSpinXPos = Math.max(0, armSpinXPos - 0.02); - robotTop.setArmSpinXPosition(armSpinXPos); + robotTop.setArmRightTurnPosition(armSpinXPos); } if (gamepad1.dpad_right) { armSpinYPos = Math.min(1, armSpinYPos + 0.05); - robotTop.setArmSpinYPosition(armSpinYPos); + robotTop.setArmLeftSpinPosition(armSpinYPos); } else if (gamepad1.dpad_left) { armSpinYPos = Math.max(0, armSpinYPos - 0.05); - robotTop.setArmSpinYPosition(armSpinYPos); + robotTop.setArmLeftSpinPosition(armSpinYPos); } if (gamepad1.a && !previousGamepad1.a) { if (backGrabOpen) { @@ -268,7 +268,7 @@ public void runOpMode() { if ((gamepad2.left_trigger != 0 && previousGamepad2.left_trigger == 0 ) || (gamepad2.right_trigger != 0 && previousGamepad2.right_trigger == 0)) { recognitionAngle = robotVisionAngle.getDetectedAngle(); - robotTop.setArmSpinYPosition(calculateSpinY(recognitionAngle)); + robotTop.setArmLeftSpinPosition(calculateSpinY(recognitionAngle)); } // telemetry 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 b7726e8508b9..693d3b2c1561 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 @@ -4,25 +4,117 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.Gamepad; +import org.firstinspires.ftc.teamcode.hardware.RobotChassis; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + @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 { + enum ArmState{ + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK + } + RobotTop robotTop; + RobotChassis robotChassis; + ArmStateMachine.ArmState armState; + Gamepad gamepad1; + Gamepad previousGamepad1; + Gamepad gamepad2; + + //TODO: test the constants + final int STRETCH_BACK_POSITION = 0; + final int STRETCH_OUT_POSITION = 0; + final double SPIN_DEFAULT_POSITION = 0; + final double SPIN_HOVERING_POSITION = 0; + final double SPIN_DOWN_POSITION = 0; + final double TURN_BACK_POSITION = 0; + final double TURN_HOVERING_POSITION = 0; + final double TURN_DOWN_POSITION = 0; + final double GRAB_OPEN_POSITION = 0; + final double GRAB_CLOSE_POSITION = 0; + + boolean isGrabbing; @Override public void runOpMode(){ - TaskManager servoMgr = new TaskManager(); Gamepad previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); + this.robotTop = new RobotTop(this); + this.robotChassis = new RobotChassis(this); + this.armState = ArmStateMachine.ArmState.IDLE; + this.isGrabbing = false; waitForStart(); if (isStopRequested()) return; while (opModeIsActive()) { - //code - - servoMgr.updateServos(); - previousGamepad1.copy(gamepad1); + robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); + switch (armState){ + case IDLE: + handleIdleState(); + case TURNING_OUT: + handleTurningOutState(); + case TURNED: + handleTurnedState(); + case TURNING_BACK: + handleTurningBackState(); + case WITHDRAWING: + handleWithdrawingState(); + } + this.previousGamepad1.copy(gamepad1); sleep(50); } + + } + protected void handleIdleState(){ + if(gamepad1.x && !previousGamepad1.x){ + robotTop.setArmStretchPosition(STRETCH_OUT_POSITION); + robotTop.setArmLeftTurnPosition(TURN_HOVERING_POSITION); + robotTop.setArmRightTurnPosition(TURN_HOVERING_POSITION); + robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION); + robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION); + armState = ArmStateMachine.ArmState.TURNED; + } + } + protected void handleTurningOutState(){ + // if necessary + } + protected void handleTurnedState(){ + if(gamepad1.x && !previousGamepad1.x){ + robotTop.setArmStretchPosition(STRETCH_BACK_POSITION); + robotTop.setArmLeftTurnPosition(TURN_BACK_POSITION); + robotTop.setArmRightTurnPosition(TURN_BACK_POSITION); + robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); + robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); + armState = ArmStateMachine.ArmState.IDLE; + } + if(gamepad1.b && !previousGamepad1.b){ + if(isGrabbing){ + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + }else { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + } + } + if(gamepad1.dpad_up){ + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.002)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.002)); + } + if(gamepad1.dpad_down){ + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.002)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.002)); + } + if(gamepad1.dpad_left){ + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.002)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.002)); + } + if(gamepad1.dpad_right){ + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.002)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.002)); + } + } + protected void handleTurningBackState(){ + //if necessary + } + protected void handleWithdrawingState(){ + //if necessary } } 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 2941740faa0f..a709fc3f953f 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,6 +2,7 @@ import com.qualcomm.robotcore.hardware.Gamepad; +import org.firstinspires.ftc.teamcode.hardware.RobotChassis; import org.firstinspires.ftc.teamcode.hardware.RobotTop; public class ArmStateMachine { @@ -9,9 +10,11 @@ enum ArmState{ IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK } RobotTop robotTop; + RobotChassis robotChassis; ArmState armState; Gamepad gamepad1; Gamepad previousGamepad1; + Gamepad gamepad2; TaskManager taskMgr; final double STRETCH_BACK_POSITION = 0.03; @@ -30,8 +33,9 @@ enum ArmState{ double armTurnPos; double armSpinXPos; double armSpinYPos; - public ArmStateMachine(RobotTop robotTop){ + public ArmStateMachine(RobotTop robotTop, RobotChassis robotChassis){ this.robotTop = robotTop; + this.robotChassis = robotChassis; this.armState = ArmState.IDLE; this.taskMgr = new TaskManager(); } @@ -47,12 +51,15 @@ protected void init(){ boolean containerRelease = false; double recognitionAngle = 0; } - public void receiveGamepad(Gamepad gamepad1, Gamepad previousGamepad1){ + public void receiveGamepad(Gamepad gamepad1, Gamepad previousGamepad1, + Gamepad gamepad2){ this.gamepad1 = gamepad1; + this.gamepad2 = gamepad2; this.previousGamepad1 = previousGamepad1; } public void update(){ + robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); switch (armState){ case IDLE: handleIdleState(); @@ -67,11 +74,6 @@ public void update(){ } } protected void handleIdleState(){ - if(gamepad1.x && !previousGamepad1.x){ - armStretchPos = STRETCH_OUT_POSITION; - robotTop.setArmStretchPosition(armStretchPos); - robotTop.setTopServoPosition(0); - } } protected void handleTurningOutState(){} protected void handleTurnedState(){} 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 index 8ace175db1c6..738028376717 100644 --- 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 @@ -59,10 +59,6 @@ protected void init(){ } private void initializeRobotPositions() { - robotTop.setArmStretchPosition(armStretchPos); - robotTop.setArmTurnPosition(armTurnPos); - robotTop.setContainerServoPosition(CONTAINER_OPEN_POSITION); - robotTop.setLiftServoPosition(LIFT_SERVO_CLOSE); } public void update(){ @@ -80,11 +76,6 @@ public void update(){ } } protected void handleArmIdle(){ - if(gamepad1.x && !previousGamepad1.x){ - armStretchPos = STRETCH_OUT_POSITION; - robotTop.setArmStretchPosition(armStretchPos); - robotTop.setTopServoPosition(0); - } } protected void handleArmTurningOut(){} protected void handleArmTurned(){} 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 788d63a6204e..e7f5c346ecbc 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 @@ -350,7 +350,7 @@ public RobotAuto gotoPosition2(double x, double y, double h) { } public RobotAuto stretchArm(){ - robotTop.setArmStretchPosition(0.3); + robotTop.setArmStretchPosition(0); return this; } public RobotAuto setLiftPower(double power){ 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 480af2c51b82..d20ef1331307 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 @@ -16,10 +16,11 @@ public class RobotTop { Telemetry telemetry; protected DcMotor leftLiftMotor; protected DcMotor rightLiftMotor; - protected Servo armStretchServo; - protected Servo armTurnServo; - protected Servo armSpinXServo; - protected Servo armSpinYServo; + protected DcMotor armStretchMotor; + protected Servo armLeftTurnServo; + protected Servo armRightTurnServo; + protected Servo armLeftSpinServo; + protected Servo armRightSpinServo; protected Servo armGrabServo; protected Servo liftServo; protected Servo topServo; @@ -34,10 +35,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, "armStretch"); - armTurnServo = hardwareMap.get(Servo.class, "armTurn"); - armSpinXServo = hardwareMap.get(Servo.class, "armSpinX"); - armSpinYServo = hardwareMap.get(Servo.class, "armSpinY"); + armStretchMotor = hardwareMap.get(DcMotor.class, "armStretch"); + armLeftTurnServo = hardwareMap.get(Servo.class, "armTurnL"); + armRightTurnServo = hardwareMap.get(Servo.class, "armTurnR"); + armLeftSpinServo = hardwareMap.get(Servo.class, "armSpinL"); + armRightSpinServo = hardwareMap.get(Servo.class, "armSpinR"); armGrabServo = hardwareMap.get(Servo.class, "armGrab"); liftServo = hardwareMap.get(Servo.class, "liftServo"); topServo = hardwareMap.get(Servo.class, "liftTop"); @@ -53,6 +55,8 @@ public void init() { leftLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rightLiftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + armStretchMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); } public void setLeftPower(double power) { @@ -64,36 +68,44 @@ public int getLiftPosition() { return leftLiftMotor.getCurrentPosition(); } - public void setArmStretchPosition(double position) { - armStretchServo.setPosition(position); + public void setArmStretchPosition(int position) { + armStretchMotor.setTargetPosition(position); } public double getArmStretchPosition() { - return armStretchServo.getPosition(); + return armStretchMotor.getCurrentPosition(); + } + + public void setArmLeftTurnPosition(double position) { + armLeftTurnServo.setPosition(position); + } + + public double getArmLeftTurnPosition() { + return armLeftTurnServo.getPosition(); } - public void setArmTurnPosition(double position) { - armTurnServo.setPosition(position); + public void setArmRightTurnPosition(double position) { + armRightTurnServo.setPosition(position); } - public double getArmTurnPosition() { - return armTurnServo.getPosition(); + public double getArmRightTurnPosition() { + return armRightTurnServo.getPosition(); } - public void setArmSpinXPosition(double position) { - armSpinXServo.setPosition(position); + public void setArmLeftSpinPosition(double position) { + armLeftSpinServo.setPosition(position); } - public double getArmSpinXPosition() { - return armSpinXServo.getPosition(); + public double getArmLeftSpinPosition() { + return armLeftSpinServo.getPosition(); } - public void setArmSpinYPosition(double position) { - armSpinYServo.setPosition(position); + public void setArmRightSpinPosition(double position) { + armRightSpinServo.setPosition(position); } - public double getArmSpinYPosition() { - return armSpinYServo.getPosition(); + public double getArmRightSpinPosition() { + return armRightSpinServo.getPosition(); } public void setArmGrabPosition(double position) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java new file mode 100644 index 000000000000..ca82364ac65f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java @@ -0,0 +1,47 @@ +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(group = "Test") +public class Armtest extends LinearOpMode { + @Override + public void runOpMode(){ + Servo armServo0 = hardwareMap.get(Servo.class , "arm0"); + Servo armServo1 = hardwareMap.get(Servo.class , "arm1"); + + waitForStart(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + + if (isStopRequested()) return; + double leftPos = 0; + double rightPos = 0; + + while (opModeIsActive()) { + if(gamepad1.y){ + leftPos = Math.min(1, leftPos + 0.003); + rightPos = Math.min(1, rightPos + 0.003); + } + if(gamepad1.a){ + leftPos = Math.max(0, leftPos - 0.003); + rightPos = Math.max(0, rightPos - 0.003); + } + if(gamepad1.x){ + leftPos = Math.min(1, leftPos + 0.003); + rightPos = Math.max(0, rightPos - 0.003); + } + if(gamepad1.b){ + leftPos = Math.max(0, leftPos - 0.003); + rightPos = Math.min(1, rightPos + 0.003); + } + armServo0.setPosition(leftPos); + armServo1.setPosition(rightPos); + + sleep(10); + } + } +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java index bf1216a424d8..6ce8e65803ab 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java @@ -19,7 +19,7 @@ public void runOpMode(){ double targetPos = 1000; double delta; double p = 0, i = 0 , d = 0; - final double Kp = 0.014, Ki = 0.000013, Kd = -0.01; + final double Kp = 0.1, Ki = 0.0, Kd = 0.0; double power; while (opModeIsActive()){ @@ -31,7 +31,7 @@ public void runOpMode(){ p = Kp * delta; i += Ki * delta; d = Kd * (currentPos - previousPos); - power = Math.min(p + i + d, 0.8); + power = Math.min(p + i + d, 1); robotTop.setLeftPower(power); previousGamepad1.copy(gamepad1); telemetry.addData("power",power); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java new file mode 100644 index 000000000000..63007e8d0d02 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java @@ -0,0 +1,53 @@ +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.Gamepad; + +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + +@TeleOp +public class PIDControllerTest2 extends LinearOpMode { + @Override + public void runOpMode(){ + DcMotor motor = hardwareMap.get(DcMotor.class, "motor"); + waitForStart(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + double currentPos = 0, previousPos; + double targetPos = 300; + double delta; + double p = 0, i = 0 , d = 0; + final double Kp = 0.02, Ki = 0.00007, Kd = -0.008, power_max = 1; + double power; + + while (opModeIsActive()){ + previousPos = currentPos; + currentPos = motor.getCurrentPosition(); + delta = targetPos - currentPos; + p = Kp * delta; + i += Ki * delta; + d = Kd * (currentPos - previousPos); + power = p + i + d; + if (power > power_max) + power = power_max; + else if(power < -power_max) + power = -power_max; + motor.setPower(power); + previousGamepad1.copy(gamepad1); + telemetry.addData("power",power); + telemetry.addData("p",p); + telemetry.addData("i",i); + telemetry.addData("d",d); + telemetry.update(); + sleep(10); + if(gamepad1.a) { + targetPos += 50; + } + if(gamepad1.b){ + targetPos -= 50; + } + } + } +} 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 6f0a70a0a1a9..1882cafff34a 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 @@ -6,12 +6,12 @@ import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; -@Disabled @TeleOp(group = "Test") public class ServoTest extends LinearOpMode { @Override public void runOpMode(){ - Servo armServo = hardwareMap.get(Servo.class , "arm"); + Servo armServo0 = hardwareMap.get(Servo.class , "arm0"); + Servo armServo1 = hardwareMap.get(Servo.class , "arm1"); //stretch(1): [0, 0.3] //turn(3): [0.38-back, 0.71-out] //grab(5): [0-open, 0.53-close] @@ -21,23 +21,23 @@ public void runOpMode(){ waitForStart(); Gamepad previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); - double servoPos = 0; + double servoPos = 0.5; while (opModeIsActive()){ boolean x = gamepad1.x; boolean b = gamepad1.b; if(x){ - servoPos += 0.01; + servoPos = Math.min(1, servoPos + 0.003); }if(b){ - servoPos -= 0.01; + servoPos = Math.max(0, servoPos - 0.003); } - - armServo.setPosition(servoPos); + armServo0.setPosition(servoPos); + armServo1.setPosition(servoPos); telemetry.addData("pos", servoPos); telemetry.update(); previousGamepad1.copy(gamepad1); - sleep(100); + sleep(10); } } } From da5b0dd18775b9bf2395eb27e540dbacae6a721d Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Fri, 17 Jan 2025 20:47:25 +0800 Subject: [PATCH 119/143] Feat:Added Autonomous. --- .../ftc/teamcode/test/AutoTest.java | 30 +++++++++++-------- 1 file changed, 18 insertions(+), 12 deletions(-) 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 987d07284d7b..38c89a51ecc7 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 @@ -1,22 +1,28 @@ 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 com.qualcomm. robotcore.eventloop. opmode.Autonomous; +import com.qualcomm. robotcore. eventloop. opmode. LinearOpMode; +import com.qualcomm. robotcore.hardware.DcMotorSimple; @Autonomous public class AutoTest extends LinearOpMode { - RobotAuto robotAuto; - RobotChassis robotChassis; - @Override public void runOpMode() { - robotChassis = new RobotChassis(this); - robotAuto = new RobotAuto(this); + DcMotorSimple frontLeft = hardwareMap.get(DcMotorSimple.class, "FL"); + DcMotorSimple backLeft = hardwareMap.get(DcMotorSimple.class, "BL"); + DcMotorSimple frontRight = hardwareMap.get(DcMotorSimple.class, "FR"); + DcMotorSimple backRight = hardwareMap.get(DcMotorSimple.class, "BR"); + + frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + backLeft.setDirection(DcMotorSimple.Direction.REVERSE); + waitForStart(); - resetRuntime(); - robotAuto.fastForward(24); + + frontLeft.setPower(0.5); + backLeft.setPower(0.5); + frontRight.setPower(0.5); + backRight.setPower(0.5); + + sleep(500); } } \ No newline at end of file From ce0ad4845b7dad9fe7665b4aeb326ad0a7d48d9c Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sat, 18 Jan 2025 08:23:46 +0800 Subject: [PATCH 120/143] Prepare for the new robot --- .../ftc/teamcode/ManualOpMode.java | 496 ++++++++++-------- .../advancedManual/AdvancedManual.java | 230 ++++++-- .../advancedManual/test/TurnServoTest.java | 37 ++ .../ftc/teamcode/hardware/PIDController.java | 19 + .../ftc/teamcode/hardware/RobotAuto.java | 4 +- .../ftc/teamcode/hardware/RobotTop.java | 74 ++- .../ftc/teamcode/test/Armtest.java | 34 +- .../ftc/teamcode/test/PIDControllerTest2.java | 50 +- .../ftc/teamcode/test/ServoTest.java | 2 +- .../ftc/teamcode/test/tttest.java | 36 ++ 10 files changed, 698 insertions(+), 284 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TurnServoTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/PIDController.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/tttest.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 a5b9f18a88d2..fce72be16d6b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -1,80 +1,146 @@ package org.firstinspires.ftc.teamcode; +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 org.firstinspires.ftc.teamcode.hardware.RobotChassis; import org.firstinspires.ftc.teamcode.hardware.RobotTop; -import org.firstinspires.ftc.teamcode.hardware.RobotVision.RobotVisionAngle; +@Disabled @TeleOp public class ManualOpMode extends LinearOpMode { - RobotChassis robotChassis; + enum ArmState { + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED + } + + enum LiftState { + DISABLED, RUNNING + } + RobotTop robotTop; + RobotChassis robotChassis; + ArmState armState; + LiftState liftState; + Gamepad previousGamepad1; - // servoName(port): positionRange[a, b]; defaultPos - // control hub: - // 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: - // 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 + //TODO: test the constants + final int STRETCH_BACK_POSITION = 70; + final int STRETCH_OUT_POSITION = 1500; + final double SPIN_DEFAULT_POSITION = 0.3; + final double SPIN_HOVERING_POSITION = 1; + final double SPIN_DOWN_POSITION = 0; + final double TURN_BACK_POSITION = 0.5; + final double TURN_HOVERING_POSITION = 0.8; + final double TURN_DOWN_POSITION = 0.85; + final double GRAB_OPEN_POSITION = 0.4; + final double GRAB_CLOSE_POSITION = 0.92; + final double TOP_BACK = 0.03; + final double TOP_OUT = 0.66; - final int LIFT_TOP_POSITION = 1250; - final int LIFT_BOTTOM_POSITION = 50; - final int STRETCH_BACK_POSITION = 0; // TODO: change it - final int STRETCH_OUT_POSITION = 0; // TODO: change it - 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.68; - final double GRAB_OPEN_POSITION = 0.2; - final double GRAB_CLOSE_POSITION = 0.9; + boolean isGrabbing; + boolean topServoOut; + boolean backGrabOpen; + int liftPosition; + double turnPosition; @Override public void runOpMode() { - robotChassis = new RobotChassis(this); - robotTop = new RobotTop(this); - RobotVisionAngle robotVisionAngle = new RobotVisionAngle(); - robotVisionAngle.initialize(hardwareMap); - Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); - Gamepad previousGamepad2 = new Gamepad(); - previousGamepad2.copy(gamepad2); - - int liftPosition; - int 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 topServoOut = false; - boolean containerRelease = true; - boolean backGrabOpen = false; - double recognitionAngle = 0; - LiftState liftState = LiftState.BOTTOM; - ArmState armState = ArmState.IDLE; - + this.robotTop = new RobotTop(this); + this.robotChassis = new RobotChassis(this); + this.armState = ArmState.IDLE; + this.liftState = LiftState.DISABLED; + this.isGrabbing = false; + this.topServoOut = false; + this.backGrabOpen = false; + this.liftPosition = 0; waitForStart(); - robotTop.setArmStretchPosition(armStretchPos); - robotTop.setArmLeftTurnPosition(armTurnPos); - robotTop.setContainerServoPosition(0.35); - robotTop.setLiftServoPosition(0.2); + robotTop.setTurnPosition(TURN_BACK_POSITION); + robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); + robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); while (opModeIsActive()) { robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); // robotLift liftPosition = robotTop.getLiftPosition(); + turnPosition = robotTop.getTurnPosition(); + + if(gamepad1.x && !gamepad1.left_bumper){ + robotTop.setStretchPower(0.5); + } else if (gamepad1.x && gamepad1.left_bumper) { + robotTop.setStretchPower(-0.5); + }else{ + robotTop.setStretchPower(0); + } + + if(gamepad1.y && !gamepad1.left_bumper){ + turnPosition += 0.005; + } else if (gamepad1.y && gamepad1.left_bumper) { + turnPosition -= 0.005; + } + robotTop.setTurnPosition(turnPosition); + + if(gamepad1.b && !previousGamepad1.b){ + if(isGrabbing){ + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + }else { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + } + isGrabbing = !isGrabbing; + } + + if(gamepad1.dpad_up){ + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.005)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.005)); + } + if(gamepad1.dpad_down){ + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.005)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.005)); + } + if(gamepad1.dpad_left){ + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.005)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.005)); + } + if(gamepad1.dpad_right){ + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.005)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.005)); + } + + if(gamepad1.right_trigger != 0){ + robotTop.setLeftPower(0.5); + robotTop.setLiftTargetPos(robotTop.getLiftPosition()); + } else if (gamepad1.left_trigger != 0) { + robotTop.setLeftPower(-0.5); + robotTop.setLiftTargetPos(robotTop.getLiftPosition()); + }else{ + if(robotTop.getLiftPosition() >= 200){ + robotTop.updateLiftPID(); + }else{ + robotTop.setLeftPower(0); + } + } + + if (gamepad1.right_bumper && !previousGamepad1.right_bumper) { + if (topServoOut) { + robotTop.setTopServoPosition(TOP_BACK); + } else { + robotTop.setTopServoPosition(TOP_OUT); + } + topServoOut = !topServoOut; + } + + if (gamepad1.a && !previousGamepad1.a) { + if (backGrabOpen) { + robotTop.setLiftServoPosition(0.1); + } else { + robotTop.setLiftServoPosition(0.6); + } + backGrabOpen = !backGrabOpen; + } + // if (liftState == LiftState.BOTTOM) { // if (gamepad1.y) { // liftState = LiftState.GOING_UP; @@ -123,165 +189,168 @@ public void runOpMode() { // 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(1000); - } - if (gamepad1.right_trigger != 0) { - armStretchPos = STRETCH_OUT_POSITION; - robotTop.setArmStretchPosition(armStretchPos); - armState = ArmState.STRETCHED; - sleep(1000); - } - } else { - if (gamepad1.left_trigger != 0) { - 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); - } else { - 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 (topServoOut) { - robotTop.setTopServoPosition(0.05); - } else { - robotTop.setTopServoPosition(0.6); - } - topServoOut = !topServoOut; - } - 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) { - if (armState == ArmState.IDLE) { - robotTop.setTopServoPosition(0); - armStretchPos = STRETCH_OUT_POSITION; - armState = ArmState.TURNING_OUT; - robotTop.setArmStretchPosition(armStretchPos); - } else if (armState == ArmState.TURNED) { - armState = ArmState.TURNING_BACK; - robotTop.setArmLeftSpinPosition(SPIN_Y_DEFAULT_POSITION); - } - } - if (armState == ArmState.WITHDRAWING) { - if (armStretchPos <= STRETCH_BACK_POSITION) { - armState = ArmState.IDLE; - } else { - armStretchPos -= 0.03; - robotTop.setArmStretchPosition(armStretchPos); - } - } - if (armState == ArmState.TURNING_OUT) { - if (armTurnPos >= TURN_OUT_HOVERING_POSITION - 0.05) { - armTurnPos = TURN_OUT_HOVERING_POSITION; - robotTop.setArmLeftTurnPosition(armTurnPos); - robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); - armState = ArmState.TURNED; - } else { - armTurnPos += 0.03; - robotTop.setArmLeftTurnPosition(armTurnPos); - } - } - if (armState == ArmState.TURNING_BACK) { - if (armTurnPos <= TURN_BACK_POSITION + 0.05) { - armTurnPos = TURN_BACK_POSITION; - robotTop.setArmLeftTurnPosition(armTurnPos); - robotTop.setArmRightTurnPosition(SPIN_X_DEFAULT_POSITION); - armState = ArmState.WITHDRAWING; - } else { - armTurnPos -= 0.03; - robotTop.setArmLeftTurnPosition(armTurnPos); - } - } - if (armState == ArmState.STRETCHED) { - if (gamepad1.x && !previousGamepad1.x) { - robotTop.setTopServoPosition(0); - armState = ArmState.WITHDRAWING; - } - } - if (gamepad1.b && !previousGamepad1.b) { - robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); - sleep(500); - if (!armGrabbing && armState == ArmState.TURNED) { - robotTop.setArmLeftTurnPosition(TURN_OUT_DOWN_POSITION); - robotTop.setArmRightTurnPosition(SPIN_X_DOWN_POSITION); - grabbingFlag = true; - } else { - robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); - armGrabbing = false; - } - } - if (grabbingFlag && !gamepad1.b) { - robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); - sleep(500); - robotTop.setArmLeftTurnPosition(TURN_OUT_HOVERING_POSITION); - robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); - armGrabbing = true; - grabbingFlag = false; - } - robotTop.setArmStretchPosition(armStretchPos); - if (gamepad1.dpad_up) { - armSpinXPos = Math.min(1, armSpinXPos + 0.02); - robotTop.setArmRightTurnPosition(armSpinXPos); - } else if (gamepad1.dpad_down) { - armSpinXPos = Math.max(0, armSpinXPos - 0.02); - robotTop.setArmRightTurnPosition(armSpinXPos); - } - if (gamepad1.dpad_right) { - armSpinYPos = Math.min(1, armSpinYPos + 0.05); - robotTop.setArmLeftSpinPosition(armSpinYPos); - } else if (gamepad1.dpad_left) { - armSpinYPos = Math.max(0, armSpinYPos - 0.05); - robotTop.setArmLeftSpinPosition(armSpinYPos); - } - if (gamepad1.a && !previousGamepad1.a) { - if (backGrabOpen) { - robotTop.setLiftServoPosition(0.1); - } else { - robotTop.setLiftServoPosition(0.6); - } - backGrabOpen = !backGrabOpen; - } - //vision - if ((gamepad2.left_trigger != 0 && previousGamepad2.left_trigger == 0 - ) || (gamepad2.right_trigger != 0 && previousGamepad2.right_trigger == 0)) { - recognitionAngle = robotVisionAngle.getDetectedAngle(); - robotTop.setArmLeftSpinPosition(calculateSpinY(recognitionAngle)); - } - // telemetry - telemetry.addData("liftPos", liftPosition); - telemetry.addData("state", liftState); - telemetry.addData("ArmState", armState); - telemetry.addData("XPos", armSpinXPos); - telemetry.addData("YPos", armSpinYPos); - telemetry.addData("angle", recognitionAngle); - telemetry.update(); +// if (liftState == LiftState.BOTTOM && armState == ArmState.IDLE) { +// if (gamepad1.left_trigger != 0) { +// armStretchPos = STRETCH_OUT_POSITION; +// robotTop.setArmStretchPosition(armStretchPos); +// armState = ArmState.STRETCHED; +// sleep(1000); +// } +// if (gamepad1.right_trigger != 0) { +// armStretchPos = STRETCH_OUT_POSITION; +// robotTop.setArmStretchPosition(armStretchPos); +// armState = ArmState.STRETCHED; +// sleep(1000); +// } +// } else { +// if (gamepad1.left_trigger != 0) { +// 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); +// } else { +// 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 (topServoOut) { +// robotTop.setTopServoPosition(0.05); +// } else { +// robotTop.setTopServoPosition(0.6); +// } +// topServoOut = !topServoOut; +// } +// 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) { +// if (armState == ArmState.IDLE) { +// robotTop.setTopServoPosition(0); +// armStretchPos = STRETCH_OUT_POSITION; +// armState = ArmState.TURNING_OUT; +// robotTop.setArmStretchPosition(armStretchPos); +// } else if (armState == ArmState.TURNED) { +// armState = ArmState.TURNING_BACK; +// robotTop.setArmLeftSpinPosition(SPIN_Y_DEFAULT_POSITION); +// } +// } +// if (armState == ArmState.WITHDRAWING) { +// if (armStretchPos <= STRETCH_BACK_POSITION) { +// armState = ArmState.IDLE; +// } else { +// armStretchPos -= 0.03; +// robotTop.setArmStretchPosition(armStretchPos); +// } +// } +// if (armState == ArmState.TURNING_OUT) { +// if (armTurnPos >= TURN_OUT_HOVERING_POSITION - 0.05) { +// armTurnPos = TURN_OUT_HOVERING_POSITION; +// robotTop.setArmLeftTurnPosition(armTurnPos); +// robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); +// armState = ArmState.TURNED; +// } else { +// armTurnPos += 0.03; +// robotTop.setArmLeftTurnPosition(armTurnPos); +// } +// } +// if (armState == ArmState.TURNING_BACK) { +// if (armTurnPos <= TURN_BACK_POSITION + 0.05) { +// armTurnPos = TURN_BACK_POSITION; +// robotTop.setArmLeftTurnPosition(armTurnPos); +// robotTop.setArmRightTurnPosition(SPIN_X_DEFAULT_POSITION); +// armState = ArmState.WITHDRAWING; +// } else { +// armTurnPos -= 0.03; +// robotTop.setArmLeftTurnPosition(armTurnPos); +// } +// } +// if (armState == ArmState.STRETCHED) { +// if (gamepad1.x && !previousGamepad1.x) { +// robotTop.setTopServoPosition(0); +// armState = ArmState.WITHDRAWING; +// } +// } +// if (gamepad1.b && !previousGamepad1.b) { +// robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); +// sleep(500); +// if (!armGrabbing && armState == ArmState.TURNED) { +// robotTop.setArmLeftTurnPosition(TURN_OUT_DOWN_POSITION); +// robotTop.setArmRightTurnPosition(SPIN_X_DOWN_POSITION); +// grabbingFlag = true; +// } else { +// robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); +// armGrabbing = false; +// } +// } +// if (grabbingFlag && !gamepad1.b) { +// robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); +// sleep(500); +// robotTop.setArmLeftTurnPosition(TURN_OUT_HOVERING_POSITION); +// robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); +// armGrabbing = true; +// grabbingFlag = false; +// } +// robotTop.setArmStretchPosition(armStretchPos); +// if (gamepad1.dpad_up) { +// armSpinXPos = Math.min(1, armSpinXPos + 0.02); +// robotTop.setArmRightTurnPosition(armSpinXPos); +// } else if (gamepad1.dpad_down) { +// armSpinXPos = Math.max(0, armSpinXPos - 0.02); +// robotTop.setArmRightTurnPosition(armSpinXPos); +// } +// if (gamepad1.dpad_right) { +// armSpinYPos = Math.min(1, armSpinYPos + 0.05); +// robotTop.setArmLeftSpinPosition(armSpinYPos); +// } else if (gamepad1.dpad_left) { +// armSpinYPos = Math.max(0, armSpinYPos - 0.05); +// robotTop.setArmLeftSpinPosition(armSpinYPos); +// } +// if (gamepad1.a && !previousGamepad1.a) { +// if (backGrabOpen) { +// robotTop.setLiftServoPosition(0.1); +// } else { +// robotTop.setLiftServoPosition(0.6); +// } +// backGrabOpen = !backGrabOpen; +// } +// +// //vision +// if ((gamepad2.left_trigger != 0 && previousGamepad2.left_trigger == 0 +// ) || (gamepad2.right_trigger != 0 && previousGamepad2.right_trigger == 0)) { +// recognitionAngle = robotVisionAngle.getDetectedAngle(); +// robotTop.setArmLeftSpinPosition(calculateSpinY(recognitionAngle)); +// } +// +// // telemetry +// telemetry.addData("liftPos", liftPosition); +// telemetry.addData("state", liftState); +// telemetry.addData("ArmState", armState); +// telemetry.addData("XPos", armSpinXPos); +// telemetry.addData("YPos", armSpinYPos); +// telemetry.addData("angle", recognitionAngle); +// telemetry.update(); previousGamepad1.copy(gamepad1); - previousGamepad2.copy(gamepad2); - sleep(50); +// previousGamepad2.copy(gamepad2); + sleep(10); } } @@ -299,13 +368,4 @@ protected double calculateSpinY(double angle) { 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 - } -} +} \ 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 index 693d3b2c1561..6e8a438d7aec 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,120 +1,278 @@ package org.firstinspires.ftc.teamcode.advancedManual; -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 org.firstinspires.ftc.teamcode.hardware.RobotChassis; import org.firstinspires.ftc.teamcode.hardware.RobotTop; -@Disabled // The ManualOp currently used is a piece of shit. // I'll write a better one here (if the time is enough). +@TeleOp public class AdvancedManual extends LinearOpMode { enum ArmState{ - IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED + } + enum LiftState{ + DISABLED, RUNNING } RobotTop robotTop; RobotChassis robotChassis; - ArmStateMachine.ArmState armState; - Gamepad gamepad1; + ArmState armState; + LiftState liftState; Gamepad previousGamepad1; - Gamepad gamepad2; //TODO: test the constants - final int STRETCH_BACK_POSITION = 0; - final int STRETCH_OUT_POSITION = 0; - final double SPIN_DEFAULT_POSITION = 0; - final double SPIN_HOVERING_POSITION = 0; + final int STRETCH_BACK_POSITION = 70; + final int STRETCH_OUT_POSITION = 1500; + final double SPIN_DEFAULT_POSITION = 0.3; + final double SPIN_HOVERING_POSITION = 1; final double SPIN_DOWN_POSITION = 0; final double TURN_BACK_POSITION = 0; - final double TURN_HOVERING_POSITION = 0; - final double TURN_DOWN_POSITION = 0; - final double GRAB_OPEN_POSITION = 0; - final double GRAB_CLOSE_POSITION = 0; + final double TURN_HOVERING_POSITION = 0.4; + final double TURN_DOWN_POSITION = 0.45; + final double GRAB_OPEN_POSITION = 0.4; + final double GRAB_CLOSE_POSITION = 0.92; + final double TOP_BACK = 0.03; + final double TOP_OUT = 0.66; boolean isGrabbing; + boolean topServoOut; + boolean backGrabOpen; + int liftPosition; + double targetTurnPosition; + double currentTurnPosition; @Override public void runOpMode(){ - Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); this.robotTop = new RobotTop(this); this.robotChassis = new RobotChassis(this); - this.armState = ArmStateMachine.ArmState.IDLE; + this.armState = ArmState.IDLE; + this.liftState = LiftState.DISABLED; this.isGrabbing = false; + this.topServoOut = false; + this.liftPosition = 0; + this.backGrabOpen = false; + this.targetTurnPosition = TURN_BACK_POSITION; waitForStart(); + robotTop.setTurnPosition(TURN_BACK_POSITION); + robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); + robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); + int armStretchStartPos = robotTop.getArmStretchPosition(); + telemetry.addData("aSTARTpos", armStretchStartPos); + telemetry.update(); if (isStopRequested()) return; while (opModeIsActive()) { robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); + if (gamepad1.a && !previousGamepad1.a) { + if (backGrabOpen) { + robotTop.setLiftServoPosition(0.1); + } else { + robotTop.setLiftServoPosition(0.6); + } + backGrabOpen = !backGrabOpen; + } switch (armState){ case IDLE: handleIdleState(); + break; case TURNING_OUT: handleTurningOutState(); + break; case TURNED: handleTurnedState(); + break; case TURNING_BACK: handleTurningBackState(); + break; case WITHDRAWING: handleWithdrawingState(); + break; + case LOCKED: + handleLockedState(); + break; + } + switch (liftState){ + case DISABLED: + handleDisabledState(); + break; + case RUNNING: + handleRunningState(); + break; } - this.previousGamepad1.copy(gamepad1); - sleep(50); + telemetry.addData("arm",armState); + telemetry.addData("lift",liftState); + telemetry.addData("armPos",robotTop.getTurnPosition()); + telemetry.addData("armStretchPosition", robotTop.getArmStretchPosition()); + telemetry.update(); + previousGamepad1.copy(gamepad1); + sleep(10); } } protected void handleIdleState(){ if(gamepad1.x && !previousGamepad1.x){ - robotTop.setArmStretchPosition(STRETCH_OUT_POSITION); - robotTop.setArmLeftTurnPosition(TURN_HOVERING_POSITION); - robotTop.setArmRightTurnPosition(TURN_HOVERING_POSITION); + robotTop.setStretchPower(0.9); + targetTurnPosition = TURN_HOVERING_POSITION; +// robotTop.setTurnPosition(TURN_HOVERING_POSITION); robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION); robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION); - armState = ArmStateMachine.ArmState.TURNED; + armState = ArmState.TURNING_OUT; + } + if(gamepad1.b && !previousGamepad1.b){ + if(isGrabbing){ + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + }else { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + } + } + if(liftState == LiftState.RUNNING){ + liftState = LiftState.DISABLED; + } + if(gamepad1.dpad_up){ + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); + } + if(gamepad1.dpad_down){ + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); + } + if(gamepad1.dpad_left){ + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); + } + if(gamepad1.dpad_right){ + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); } } + protected void handleTurningOutState(){ - // if necessary + if(robotTop.getArmStretchPosition() >= STRETCH_OUT_POSITION){ + robotTop.setStretchPower(0); + armState = ArmState.TURNED; + } + currentTurnPosition = robotTop.getTurnPosition(); + if(currentTurnPosition > targetTurnPosition + 0.005){ + robotTop.setTurnPosition(currentTurnPosition - 0.005); + } else if (currentTurnPosition < targetTurnPosition - 0.005) { + robotTop.setTurnPosition(currentTurnPosition + 0.005); + }else{ + robotTop.setTurnPosition(targetTurnPosition); + } +// if(robotTop.getTurnPosition() >= TURN_DOWN_POSITION){ +// robotTop.setStretchPower(0); +// armState = ArmState.TURNED; +// } +// robotTop.setTurnPosition(robotTop.getTurnPosition() + 0.005); } protected void handleTurnedState(){ if(gamepad1.x && !previousGamepad1.x){ - robotTop.setArmStretchPosition(STRETCH_BACK_POSITION); - robotTop.setArmLeftTurnPosition(TURN_BACK_POSITION); - robotTop.setArmRightTurnPosition(TURN_BACK_POSITION); + robotTop.setStretchPower(-0.9); + targetTurnPosition = TURN_BACK_POSITION; + // robotTop.setTurnPosition(TURN_BACK_POSITION); robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); - armState = ArmStateMachine.ArmState.IDLE; + armState = ArmState.WITHDRAWING; } if(gamepad1.b && !previousGamepad1.b){ if(isGrabbing){ + robotTop.setTurnPosition(TURN_DOWN_POSITION); + sleep(500); robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + sleep(500); + robotTop.setTurnPosition(TURN_HOVERING_POSITION); + isGrabbing = !isGrabbing; }else { robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + isGrabbing = !isGrabbing; } } if(gamepad1.dpad_up){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.002)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.002)); + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); } if(gamepad1.dpad_down){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.002)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.002)); + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); } if(gamepad1.dpad_left){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.002)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.002)); + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); } if(gamepad1.dpad_right){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.002)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.002)); + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); } } protected void handleTurningBackState(){ //if necessary } protected void handleWithdrawingState(){ - //if necessary + if(robotTop.getArmStretchPosition() <= STRETCH_BACK_POSITION){ + robotTop.setStretchPower(0); + armState = ArmState.IDLE; + } + currentTurnPosition = robotTop.getTurnPosition(); + if(currentTurnPosition > targetTurnPosition + 0.005){ + robotTop.setTurnPosition(currentTurnPosition - 0.005); + } else if (currentTurnPosition < targetTurnPosition - 0.005) { + robotTop.setTurnPosition(currentTurnPosition + 0.005); + }else{ + robotTop.setTurnPosition(targetTurnPosition); + } +// if(robotTop.getTurnPosition() <= TURN_BACK_POSITION){ +// robotTop.setStretchPower(0); +// armState = ArmState.IDLE; +// } +// robotTop.setTurnPosition(robotTop.getTurnPosition() - 0.005); + } + + protected void handleLockedState(){ + if(gamepad1.y && !previousGamepad1.y){ + liftState = LiftState.DISABLED; + robotTop.setStretchPower(-0.8); + armState = ArmState.WITHDRAWING; + } + } + + protected void handleDisabledState(){ + if(armState != ArmState.IDLE || armState != ArmState.LOCKED){ + liftState = LiftState.RUNNING; + } + if(gamepad1.right_trigger != 0 || gamepad1.left_trigger != 0){ + robotTop.setStretchPower(0.8); + sleep(700); + robotTop.setStretchPower(0); + armState = ArmState.LOCKED; + liftState = LiftState.RUNNING; + } + } + protected void handleRunningState(){ + if(gamepad1.right_trigger != 0){ + robotTop.setLeftPower(0.5); + robotTop.setLiftTargetPos(robotTop.getLiftPosition()); + } else if (gamepad1.left_trigger != 0) { + robotTop.setLeftPower(-0.5); + robotTop.setLiftTargetPos(robotTop.getLiftPosition()); + }else{ + if(robotTop.getLiftPosition() >= 200){ + robotTop.updateLiftPID(); + }else{ + robotTop.setLeftPower(0); + } + } + if (gamepad1.left_bumper && !previousGamepad1.left_bumper) { + if (topServoOut) { + robotTop.setTopServoPosition(TOP_BACK); + } else { + robotTop.setTopServoPosition(TOP_OUT); + } + topServoOut = !topServoOut; + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TurnServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TurnServoTest.java new file mode 100644 index 000000000000..640c3c032dab --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TurnServoTest.java @@ -0,0 +1,37 @@ +package org.firstinspires.ftc.teamcode.advancedManual.test; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +@TeleOp +public class TurnServoTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + Servo armLeftTurnServo = hardwareMap.get(Servo.class, "armTurnL"); + Servo armRightTurnServo = hardwareMap.get(Servo.class, "armTurnR"); + + double armPos = 0.0; // left + + boolean pU = gamepad1.dpad_up; + boolean pD = gamepad1.dpad_down; + + waitForStart(); + + while (opModeIsActive()) { + if (!pU && gamepad1.dpad_up) { + armPos += 0.01; + } else if (!pD && gamepad1.dpad_down) { + armPos -= 0.01; + } + + armLeftTurnServo.setPosition(armPos); + armRightTurnServo.setPosition(1 - armPos); + + pU = gamepad1.dpad_up; + pD = gamepad1.dpad_down; + + sleep(10); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/PIDController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/PIDController.java new file mode 100644 index 000000000000..02ef1d73d985 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/PIDController.java @@ -0,0 +1,19 @@ +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.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class PIDController { + OpMode opMode; + HardwareMap hardwareMap; + Telemetry telemetry; + public PIDController(LinearOpMode opMode) { + this.opMode = opMode; + hardwareMap = opMode.hardwareMap; + telemetry = opMode.telemetry; + } + +} 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 e7f5c346ecbc..a5225541836f 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 @@ -50,10 +50,10 @@ public RobotAuto(LinearOpMode opMode) { // 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; + static final double DRIVE_GEAR_REDUCTION = 15.0; // wheel diameter in inches - static final double WHEEL_DIAMETER_INCHES = 5.31; + static final double WHEEL_DIAMETER_INCHES = 3.0; static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * 3.1415); 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 d20ef1331307..6abfef7f470d 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 @@ -25,6 +25,9 @@ public class RobotTop { protected Servo liftServo; protected Servo topServo; protected Servo containerServo; + + protected int targetLiftPosition; + public RobotTop(LinearOpMode opMode) { this.opMode = opMode; hardwareMap = opMode.hardwareMap; @@ -56,7 +59,9 @@ public void init() { rightLiftMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightLiftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - armStretchMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + armStretchMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + armStretchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + armStretchMotor.setDirection(DcMotor.Direction.REVERSE); } public void setLeftPower(double power) { @@ -69,10 +74,14 @@ public int getLiftPosition() { } public void setArmStretchPosition(int position) { - armStretchMotor.setTargetPosition(position); + //armStretchMotor.setTargetPosition(position); + } + + public void setStretchPower(double p) { + armStretchMotor.setPower(p); } - public double getArmStretchPosition() { + public int getArmStretchPosition() { return armStretchMotor.getCurrentPosition(); } @@ -92,6 +101,15 @@ public double getArmRightTurnPosition() { return armRightTurnServo.getPosition(); } + public double getTurnPosition(){ + return getArmLeftTurnPosition(); + } + + public void setTurnPosition(double position){ + setArmLeftTurnPosition(position); + setArmRightTurnPosition(1 - position); + } + public void setArmLeftSpinPosition(double position) { armLeftSpinServo.setPosition(position); } @@ -115,6 +133,7 @@ public void setArmGrabPosition(double position) { public double getArmGrabPosition() { return armGrabServo.getPosition(); } + public void setLiftServoPosition(double position) { liftServo.setPosition(position); } @@ -122,6 +141,7 @@ public void setLiftServoPosition(double position) { public double getLiftServoPosition() { return liftServo.getPosition(); } + public void setTopServoPosition(double position) { topServo.setPosition(position); } @@ -129,6 +149,7 @@ public void setTopServoPosition(double position) { public double getTopServoPosition() { return topServo.getPosition(); } + public void setContainerServoPosition(double position) { containerServo.setPosition(position); } @@ -136,4 +157,51 @@ public void setContainerServoPosition(double position) { public double getContainerServoPosition() { return containerServo.getPosition(); } + + public void setTargetLiftPosition(int position){ + this.targetLiftPosition = position; + } + + //PID + double currentPos = 0, previousPos = 0, offset = 0; + double targetPos = 0; + double ticks_per_rev = 530; + double delta = 0; + double p = 0, i = 0 , d = 0; + final double Kp = 0.005, Ki = 0.000008, Kd = -0.01; + final double power_max = 1; + double power = 0; + + public void updateLiftPID() { + previousPos = currentPos; + currentPos = getLiftPosition(); + delta = targetPos - currentPos - offset; + if (delta > ticks_per_rev * 0.6) + power = 1; + else if (delta < -ticks_per_rev * 0.6) + power = -1; + else + { + p = Kp * delta; + i += Ki * delta; + d = Kd * (currentPos - previousPos); + power = p + i + d; + if (power > power_max) + power = power_max; + else if(power < -power_max) + power = -power_max; + } + setLeftPower(power); + telemetry.addData("power",power); + telemetry.addData("p",p); + telemetry.addData("i",i); + telemetry.addData("d",d); + telemetry.addData("target",targetPos); + } + + public void setLiftTargetPos(int pos){ + this.targetPos = Math.min(1260, pos); + this.targetPos = Math.max(40, targetPos); + } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java index ca82364ac65f..78f5d679251d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java @@ -2,6 +2,7 @@ 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.Gamepad; import com.qualcomm.robotcore.hardware.Servo; @@ -11,35 +12,56 @@ public class Armtest extends LinearOpMode { public void runOpMode(){ Servo armServo0 = hardwareMap.get(Servo.class , "arm0"); Servo armServo1 = hardwareMap.get(Servo.class , "arm1"); + DcMotor m = hardwareMap.get(DcMotor.class, "m"); waitForStart(); Gamepad previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); if (isStopRequested()) return; - double leftPos = 0; - double rightPos = 0; + double leftPos = 0.5; + double rightPos = 0.5; while (opModeIsActive()) { - if(gamepad1.y){ + if(gamepad1.dpad_up){ leftPos = Math.min(1, leftPos + 0.003); rightPos = Math.min(1, rightPos + 0.003); } - if(gamepad1.a){ + if(gamepad1.dpad_down){ leftPos = Math.max(0, leftPos - 0.003); rightPos = Math.max(0, rightPos - 0.003); } - if(gamepad1.x){ + if(gamepad1.dpad_left){ leftPos = Math.min(1, leftPos + 0.003); rightPos = Math.max(0, rightPos - 0.003); } - if(gamepad1.b){ + if(gamepad1.dpad_right){ leftPos = Math.max(0, leftPos - 0.003); rightPos = Math.min(1, rightPos + 0.003); } + + if(gamepad1.y){ + leftPos = Math.min(1, leftPos + 0.003); + rightPos = Math.max(0, rightPos - 0.003); + } + if(gamepad1.a){ + leftPos = Math.max(0, leftPos - 0.003); + rightPos = Math.min(1, rightPos + 0.003); + } + if(gamepad1.x){ + m.setPower(-0.3); + } else if (gamepad1.b) { + m.setPower(0.3); + }else { + m.setPower(0); + } armServo0.setPosition(leftPos); armServo1.setPosition(rightPos); + telemetry.addData("lp", leftPos); + telemetry.addData("pos", m.getCurrentPosition()); + telemetry.update(); + sleep(10); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java index 63007e8d0d02..30b02d13981e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java @@ -15,39 +15,53 @@ public void runOpMode(){ waitForStart(); Gamepad previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); - double currentPos = 0, previousPos; - double targetPos = 300; - double delta; + double currentPos = 0, previousPos = 0, offset = 0; + double targetPos = 0; + double ticks_per_rev = 530; + double delta = 0; double p = 0, i = 0 , d = 0; - final double Kp = 0.02, Ki = 0.00007, Kd = -0.008, power_max = 1; - double power; - + final double Kp = 0.02, Ki = 0.00008, Kd = -0.01, power_max = 1; + double power = 0; + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); while (opModeIsActive()){ previousPos = currentPos; currentPos = motor.getCurrentPosition(); - delta = targetPos - currentPos; - p = Kp * delta; - i += Ki * delta; - d = Kd * (currentPos - previousPos); - power = p + i + d; - if (power > power_max) - power = power_max; - else if(power < -power_max) - power = -power_max; + delta = targetPos - currentPos - offset; + if (delta > ticks_per_rev * 0.75) + power = 1; + else if (delta < -ticks_per_rev * 0.75) + power = -1; + else + { + p = Kp * delta; + i += Ki * delta; + d = Kd * (currentPos - previousPos); + power = p + i + d; + if (power > power_max) + power = power_max; + else if(power < -power_max) + power = -power_max; + } motor.setPower(power); previousGamepad1.copy(gamepad1); telemetry.addData("power",power); telemetry.addData("p",p); telemetry.addData("i",i); telemetry.addData("d",d); + telemetry.addData("tpr",ticks_per_rev); telemetry.update(); sleep(10); if(gamepad1.a) { - targetPos += 50; + targetPos = 0; } if(gamepad1.b){ - targetPos -= 50; + targetPos = ticks_per_rev; + } + if(gamepad1.start){ + offset = motor.getCurrentPosition(); + p = i = d = 0; } } } -} +} \ 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 1882cafff34a..08edee753ad6 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 @@ -31,7 +31,7 @@ public void runOpMode(){ }if(b){ servoPos = Math.max(0, servoPos - 0.003); } - armServo0.setPosition(servoPos); + armServo0.setPosition(0.5-servoPos); armServo1.setPosition(servoPos); telemetry.addData("pos", servoPos); telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/tttest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/tttest.java new file mode 100644 index 000000000000..5bd7c0d3edfe --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/tttest.java @@ -0,0 +1,36 @@ +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.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +@TeleOp(group = "Test") +public class tttest extends LinearOpMode { + @Override + public void runOpMode(){ + DcMotor m = hardwareMap.get(DcMotor.class, "m"); + + waitForStart(); + Gamepad previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + + if (isStopRequested()) return; + m.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + while (opModeIsActive()) { + if(gamepad1.y){ + m.setTargetPosition(0); + } + if(gamepad1.a){ + m.setTargetPosition(300); + } + + // telemetry.addData("lp", leftPos); + telemetry.update(); + + sleep(10); + } + } +} From 6294633b132d961c036336b90ca88b2df8d97540 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sat, 18 Jan 2025 08:30:09 +0800 Subject: [PATCH 121/143] Feat:Added Autonomous. --- .../ftc/teamcode/test/AutoTest.java | 116 +++++++++++++++++- 1 file changed, 110 insertions(+), 6 deletions(-) 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 38c89a51ecc7..b8546baddfa2 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 @@ -2,27 +2,131 @@ 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.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; @Autonomous public class AutoTest extends LinearOpMode { @Override public void runOpMode() { - DcMotorSimple frontLeft = hardwareMap.get(DcMotorSimple.class, "FL"); - DcMotorSimple backLeft = hardwareMap.get(DcMotorSimple.class, "BL"); - DcMotorSimple frontRight = hardwareMap.get(DcMotorSimple.class, "FR"); - DcMotorSimple backRight = hardwareMap.get(DcMotorSimple.class, "BR"); + DcMotor frontLeft = hardwareMap.get(DcMotor.class, "FL"); + DcMotor backLeft = hardwareMap.get(DcMotor.class, "BL"); + DcMotor frontRight = hardwareMap.get(DcMotor.class, "FR"); + DcMotor backRight = hardwareMap.get(DcMotor.class, "BR"); + DcMotor leftLift = hardwareMap.get(DcMotor.class,"leftLift"); + DcMotor rightLift = hardwareMap.get(DcMotor.class,"rightLift"); + DcMotor armStretchMotor = hardwareMap.get(DcMotor.class, "armStretch"); + Servo liftServo = hardwareMap.get(Servo.class,"liftServo"); - frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); - backLeft.setDirection(DcMotorSimple.Direction.REVERSE); + leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + armStretchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + leftLift.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightLift.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + frontRight.setDirection(DcMotorSimple.Direction.REVERSE); + rightLift.setDirection(DcMotor.Direction.REVERSE); + armStretchMotor.setDirection(DcMotor.Direction.REVERSE); waitForStart(); + //tighten + liftServo.setPosition(0.95); + sleep(700); + + frontLeft.setPower(-0.7); + backLeft.setPower(0.7); + frontRight.setPower(0.7); + backRight.setPower(-0.7); + sleep(700); + + //forward + frontLeft.setPower(0.7); + backLeft.setPower(0.7); + frontRight.setPower(0.7); + backRight.setPower(0.7); + sleep(600); + frontLeft.setPower(0); + backLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + + //stretch arm + armStretchMotor.setPower(0.5); + sleep(1400); + armStretchMotor.setPower(0); + + //lift + leftLift.setPower(0.5); + rightLift.setPower(0.5); + sleep(1700); + leftLift.setPower(0); + rightLift.setPower(0); + frontLeft.setPower(0.5); backLeft.setPower(0.5); frontRight.setPower(0.5); backRight.setPower(0.5); + sleep(130); + frontLeft.setPower(0); + backLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + + leftLift.setPower(-0.5); + rightLift.setPower(-0.5); + while(leftLift.getCurrentPosition() < 800); + leftLift.setPower(0); + rightLift.setPower(0); + //Release + liftServo.setPosition(0.2); sleep(500); + + armStretchMotor.setPower(-0.5); + sleep(1400); + armStretchMotor.setPower(0); + + //back1 + frontLeft.setPower(-0.7); + backLeft.setPower(-0.7); + frontRight.setPower(-0.7); + backRight.setPower(-0.7); + sleep(500); + + //rafe + frontLeft.setPower(0.7); + backLeft.setPower(-0.7); + frontRight.setPower(-0.7); + backRight.setPower(0.7); + sleep(1500); + + //spin + frontLeft.setPower(-0.7); + backLeft.setPower(-0.7); + frontRight.setPower(0.7); + backRight.setPower(0.7); + sleep(1320); + frontLeft.setPower(0); + backLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + + //back2 + frontLeft.setPower(0.7); + backLeft.setPower(0.7); + frontRight.setPower(0.7); + backRight.setPower(0.7); + sleep(100); + + liftServo.setPosition(0.95); + sleep(700); + } } \ No newline at end of file From 8dc3d556c034afacdba1dbd50598e97238301857 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sat, 18 Jan 2025 08:32:12 +0800 Subject: [PATCH 122/143] Feat:Fixed Autonomous. --- .../ftc/teamcode/RedLeftAuto.java | 21 ------------------- .../test/RobotStateMachine.java | 9 ++++++++ 2 files changed, 9 insertions(+), 21 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java deleted file mode 100644 index 732938fa08b2..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedLeftAuto.java +++ /dev/null @@ -1,21 +0,0 @@ -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.grab() - .setLiftPower(0.5) - .release(); - } -} 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 index 738028376717..b556fc1c1675 100644 --- 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 @@ -59,6 +59,10 @@ protected void init(){ } private void initializeRobotPositions() { + robotTop.setArmStretchPosition(armStretchPos); + robotTop.setArmLeftTurnPosition(armTurnPos); + robotTop.setContainerServoPosition(CONTAINER_OPEN_POSITION); + robotTop.setLiftServoPosition(LIFT_SERVO_CLOSE); } public void update(){ @@ -76,6 +80,11 @@ public void update(){ } } protected void handleArmIdle(){ + if(gamepad1.x && !previousGamepad1.x){ + armStretchPos = STRETCH_OUT_POSITION; + robotTop.setArmStretchPosition(armStretchPos); + robotTop.setTopServoPosition(0); + } } protected void handleArmTurningOut(){} protected void handleArmTurned(){} From a793e5211dc7139e4fe018fea2210d87f51ec9bc Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sat, 18 Jan 2025 10:14:31 +0800 Subject: [PATCH 123/143] Prepare for the new robot --- .../advancedManual/AdvancedManual.java | 27 ++++++++++--------- .../ftc/teamcode/hardware/RobotTop.java | 5 ++-- 2 files changed, 18 insertions(+), 14 deletions(-) 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 6e8a438d7aec..1f21b25a8a6d 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 @@ -30,8 +30,8 @@ enum LiftState{ final double SPIN_HOVERING_POSITION = 1; final double SPIN_DOWN_POSITION = 0; final double TURN_BACK_POSITION = 0; - final double TURN_HOVERING_POSITION = 0.4; - final double TURN_DOWN_POSITION = 0.45; + final double TURN_HOVERING_POSITION = 0.35; + final double TURN_DOWN_POSITION = 0.4; final double GRAB_OPEN_POSITION = 0.4; final double GRAB_CLOSE_POSITION = 0.92; final double TOP_BACK = 0.03; @@ -157,10 +157,10 @@ protected void handleTurningOutState(){ armState = ArmState.TURNED; } currentTurnPosition = robotTop.getTurnPosition(); - if(currentTurnPosition > targetTurnPosition + 0.005){ - robotTop.setTurnPosition(currentTurnPosition - 0.005); - } else if (currentTurnPosition < targetTurnPosition - 0.005) { - robotTop.setTurnPosition(currentTurnPosition + 0.005); + if(currentTurnPosition > targetTurnPosition + 0.014){ + robotTop.setTurnPosition(currentTurnPosition - 0.014); + } else if (currentTurnPosition < targetTurnPosition - 0.014) { + robotTop.setTurnPosition(currentTurnPosition + 0.014); }else{ robotTop.setTurnPosition(targetTurnPosition); } @@ -218,10 +218,10 @@ protected void handleWithdrawingState(){ armState = ArmState.IDLE; } currentTurnPosition = robotTop.getTurnPosition(); - if(currentTurnPosition > targetTurnPosition + 0.005){ - robotTop.setTurnPosition(currentTurnPosition - 0.005); - } else if (currentTurnPosition < targetTurnPosition - 0.005) { - robotTop.setTurnPosition(currentTurnPosition + 0.005); + if(currentTurnPosition > targetTurnPosition + 0.014){ + robotTop.setTurnPosition(currentTurnPosition - 0.014); + } else if (currentTurnPosition < targetTurnPosition - 0.014) { + robotTop.setTurnPosition(currentTurnPosition + 0.014); }else{ robotTop.setTurnPosition(targetTurnPosition); } @@ -233,10 +233,11 @@ protected void handleWithdrawingState(){ } protected void handleLockedState(){ - if(gamepad1.y && !previousGamepad1.y){ + if(gamepad1.x && !previousGamepad1.x){ liftState = LiftState.DISABLED; robotTop.setStretchPower(-0.8); armState = ArmState.WITHDRAWING; + targetTurnPosition = TURN_BACK_POSITION; } } @@ -249,15 +250,17 @@ protected void handleDisabledState(){ sleep(700); robotTop.setStretchPower(0); armState = ArmState.LOCKED; - liftState = LiftState.RUNNING; + liftState = LiftState.DISABLED; } } protected void handleRunningState(){ if(gamepad1.right_trigger != 0){ robotTop.setLeftPower(0.5); + robotTop.setTopServoPosition(TOP_BACK); robotTop.setLiftTargetPos(robotTop.getLiftPosition()); } else if (gamepad1.left_trigger != 0) { robotTop.setLeftPower(-0.5); + robotTop.setTopServoPosition(TOP_BACK); robotTop.setLiftTargetPos(robotTop.getLiftPosition()); }else{ if(robotTop.getLiftPosition() >= 200){ 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 6abfef7f470d..326ad968d58b 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 @@ -168,8 +168,8 @@ public void setTargetLiftPosition(int position){ double ticks_per_rev = 530; double delta = 0; double p = 0, i = 0 , d = 0; - final double Kp = 0.005, Ki = 0.000008, Kd = -0.01; - final double power_max = 1; + final double Kp = 0.0008, Ki = 0.00001, Kd = -0.004; + final double power_max = 0.5; double power = 0; public void updateLiftPID() { @@ -196,6 +196,7 @@ else if(power < -power_max) telemetry.addData("p",p); telemetry.addData("i",i); telemetry.addData("d",d); + telemetry.addData("current",currentPos); telemetry.addData("target",targetPos); } From 94087b2a53bb586c3f5cb135b25820579f33a7bd Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sat, 18 Jan 2025 10:48:57 +0800 Subject: [PATCH 124/143] Feat:Fixed Autonomous. --- .../firstinspires/ftc/teamcode/LeftAuto.java | 150 ++++++++++++++++++ .../firstinspires/ftc/teamcode/RightAuto.java | 150 ++++++++++++++++++ 2 files changed, 300 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java new file mode 100644 index 000000000000..a23f43470605 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java @@ -0,0 +1,150 @@ +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; +import com.qualcomm.robotcore.hardware.Servo; + +@Autonomous +public class LeftAuto extends LinearOpMode { + @Override + public void runOpMode() { + DcMotor frontLeft = hardwareMap.get(DcMotor.class, "FL"); + DcMotor backLeft = hardwareMap.get(DcMotor.class, "BL"); + DcMotor frontRight = hardwareMap.get(DcMotor.class, "FR"); + DcMotor backRight = hardwareMap.get(DcMotor.class, "BR"); + DcMotor leftLift = hardwareMap.get(DcMotor.class,"leftLift"); + DcMotor rightLift = hardwareMap.get(DcMotor.class,"rightLift"); + DcMotor armStretchMotor = hardwareMap.get(DcMotor.class, "armStretch"); + Servo liftServo = hardwareMap.get(Servo.class,"liftServo"); + + leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + armStretchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + leftLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + rightLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + frontRight.setDirection(DcMotor.Direction.REVERSE); + rightLift.setDirection(DcMotor.Direction.REVERSE); + armStretchMotor.setDirection(DcMotor.Direction.REVERSE); + + waitForStart(); + + //tighten + liftServo.setPosition(0.95); + sleep(700); + + //right rafe + frontLeft.setPower(0.7); + backLeft.setPower(-0.7); + frontRight.setPower(-0.7); + backRight.setPower(0.7); + sleep(1000); + + //forward + frontLeft.setPower(0.7); + backLeft.setPower(0.7); + frontRight.setPower(0.7); + backRight.setPower(0.7); + sleep(600); + frontLeft.setPower(0); + backLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + + //stretch arm + armStretchMotor.setPower(0.5); + sleep(1400); + armStretchMotor.setPower(0); + + //lift1 + leftLift.setPower(0.5); + rightLift.setPower(0.5); + while(leftLift.getCurrentPosition() < 1200){} + leftLift.setPower(0); + rightLift.setPower(0); + + //forward close to target + frontLeft.setPower(0.5); + backLeft.setPower(0.5); + frontRight.setPower(0.5); + backRight.setPower(0.5); + sleep(130); + frontLeft.setPower(0); + backLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + + //lift2 + leftLift.setPower(-0.5); + rightLift.setPower(-0.5); + sleep(500); + leftLift.setPower(0); + rightLift.setPower(0); + + //Release specimen + liftServo.setPosition(0.2); + sleep(500); + + //drawback arm + armStretchMotor.setPower(-0.5); + sleep(1400); + armStretchMotor.setPower(0); + + //back + frontLeft.setPower(-0.7); + backLeft.setPower(-0.7); + frontRight.setPower(-0.7); + backRight.setPower(-0.7); + sleep(700); + + //left rafe + frontLeft.setPower(-0.7); + backLeft.setPower(0.7); + frontRight.setPower(0.7); + backRight.setPower(-0.7); + sleep(2500); + +// //back1 +// frontLeft.setPower(-0.7); +// backLeft.setPower(-0.7); +// frontRight.setPower(-0.7); +// backRight.setPower(-0.7); +// sleep(500); +// +// //right rafe +// frontLeft.setPower(-0.7); +// backLeft.setPower(0.7); +// frontRight.setPower(0.7); +// backRight.setPower(-0.7); +// sleep(1500); +// +// //spin +// frontLeft.setPower(-0.7); +// backLeft.setPower(-0.7); +// frontRight.setPower(0.7); +// backRight.setPower(0.7); +// sleep(1320); +// frontLeft.setPower(0); +// backLeft.setPower(0); +// frontRight.setPower(0); +// backRight.setPower(0); +// +// //back2 +// frontLeft.setPower(0.7); +// backLeft.setPower(0.7); +// frontRight.setPower(0.7); +// backRight.setPower(0.7); +// sleep(100); +// +// //grab +// liftServo.setPosition(0.95); +// sleep(700); + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java new file mode 100644 index 000000000000..fcc507c38624 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java @@ -0,0 +1,150 @@ +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; +import com.qualcomm.robotcore.hardware.Servo; + +@Autonomous +public class RightAuto extends LinearOpMode { + @Override + public void runOpMode() { + DcMotor frontLeft = hardwareMap.get(DcMotor.class, "FL"); + DcMotor backLeft = hardwareMap.get(DcMotor.class, "BL"); + DcMotor frontRight = hardwareMap.get(DcMotor.class, "FR"); + DcMotor backRight = hardwareMap.get(DcMotor.class, "BR"); + DcMotor leftLift = hardwareMap.get(DcMotor.class,"leftLift"); + DcMotor rightLift = hardwareMap.get(DcMotor.class,"rightLift"); + DcMotor armStretchMotor = hardwareMap.get(DcMotor.class, "armStretch"); + Servo liftServo = hardwareMap.get(Servo.class,"liftServo"); + + leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + armStretchMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + leftLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + rightLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + frontRight.setDirection(DcMotor.Direction.REVERSE); + rightLift.setDirection(DcMotor.Direction.REVERSE); + armStretchMotor.setDirection(DcMotor.Direction.REVERSE); + + waitForStart(); + + //tighten + liftServo.setPosition(0.95); + sleep(700); + + //left rafe + frontLeft.setPower(-0.7); + backLeft.setPower(0.7); + frontRight.setPower(0.7); + backRight.setPower(-0.7); + sleep(700); + + //forward + frontLeft.setPower(0.7); + backLeft.setPower(0.7); + frontRight.setPower(0.7); + backRight.setPower(0.7); + sleep(600); + frontLeft.setPower(0); + backLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + + //stretch arm + armStretchMotor.setPower(0.5); + sleep(1400); + armStretchMotor.setPower(0); + + //lift1 + leftLift.setPower(0.5); + rightLift.setPower(0.5); + while(leftLift.getCurrentPosition() < 1200){} + leftLift.setPower(0); + rightLift.setPower(0); + + //forward close to target + frontLeft.setPower(0.5); + backLeft.setPower(0.5); + frontRight.setPower(0.5); + backRight.setPower(0.5); + sleep(130); + frontLeft.setPower(0); + backLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + + //lift2 + leftLift.setPower(-0.5); + rightLift.setPower(-0.5); + sleep(500); + leftLift.setPower(0); + rightLift.setPower(0); + + //Release specimen + liftServo.setPosition(0.2); + sleep(500); + + //drawback arm + armStretchMotor.setPower(-0.5); + sleep(1400); + armStretchMotor.setPower(0); + + //back + frontLeft.setPower(-0.7); + backLeft.setPower(-0.7); + frontRight.setPower(-0.7); + backRight.setPower(-0.7); + sleep(700); + + //right rafe + frontLeft.setPower(0.7); + backLeft.setPower(-0.7); + frontRight.setPower(-0.7); + backRight.setPower(0.7); + sleep(2500); + +// //back1 +// frontLeft.setPower(-0.7); +// backLeft.setPower(-0.7); +// frontRight.setPower(-0.7); +// backRight.setPower(-0.7); +// sleep(500); +// +// //right rafe +// frontLeft.setPower(0.7); +// backLeft.setPower(-0.7); +// frontRight.setPower(-0.7); +// backRight.setPower(0.7); +// sleep(1500); +// +// //spin +// frontLeft.setPower(-0.7); +// backLeft.setPower(-0.7); +// frontRight.setPower(0.7); +// backRight.setPower(0.7); +// sleep(1320); +// frontLeft.setPower(0); +// backLeft.setPower(0); +// frontRight.setPower(0); +// backRight.setPower(0); +// +// //back2 +// frontLeft.setPower(0.7); +// backLeft.setPower(0.7); +// frontRight.setPower(0.7); +// backRight.setPower(0.7); +// sleep(100); +// +// //grab +// liftServo.setPosition(0.95); +// sleep(700); +// + } +} \ No newline at end of file From e5c7679be9a4594649296c422d0f02d65992105f Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sat, 18 Jan 2025 11:00:48 +0800 Subject: [PATCH 125/143] Combine auto and manual --- .../ftc/teamcode/advancedManual/AdvancedManual.java | 6 +++--- .../ftc/teamcode/advancedManual/test/RobotStateMachine.java | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) 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 1f21b25a8a6d..1188787a6b4c 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 @@ -26,11 +26,11 @@ enum LiftState{ //TODO: test the constants final int STRETCH_BACK_POSITION = 70; final int STRETCH_OUT_POSITION = 1500; - final double SPIN_DEFAULT_POSITION = 0.3; + final double SPIN_DEFAULT_POSITION = 0.2 ; final double SPIN_HOVERING_POSITION = 1; final double SPIN_DOWN_POSITION = 0; final double TURN_BACK_POSITION = 0; - final double TURN_HOVERING_POSITION = 0.35; + final double TURN_HOVERING_POSITION = 0.32; final double TURN_DOWN_POSITION = 0.4; final double GRAB_OPEN_POSITION = 0.4; final double GRAB_CLOSE_POSITION = 0.92; @@ -57,7 +57,7 @@ public void runOpMode(){ this.backGrabOpen = false; this.targetTurnPosition = TURN_BACK_POSITION; waitForStart(); - robotTop.setTurnPosition(TURN_BACK_POSITION); + // robotTop.setTurnPosition(TURN_BACK_POSITION); robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); int armStretchStartPos = robotTop.getArmStretchPosition(); 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 index b556fc1c1675..21a66a27afb1 100644 --- 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 @@ -59,7 +59,7 @@ protected void init(){ } private void initializeRobotPositions() { - robotTop.setArmStretchPosition(armStretchPos); + // robotTop.setArmStretchPosition(armStretchPos); robotTop.setArmLeftTurnPosition(armTurnPos); robotTop.setContainerServoPosition(CONTAINER_OPEN_POSITION); robotTop.setLiftServoPosition(LIFT_SERVO_CLOSE); @@ -82,7 +82,7 @@ public void update(){ protected void handleArmIdle(){ if(gamepad1.x && !previousGamepad1.x){ armStretchPos = STRETCH_OUT_POSITION; - robotTop.setArmStretchPosition(armStretchPos); + // robotTop.setArmStretchPosition(armStretchPos); robotTop.setTopServoPosition(0); } } From b753892a76c2fae5392a64f65a0a90062d92e2ec Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sat, 18 Jan 2025 11:37:21 +0800 Subject: [PATCH 126/143] Feat:Fixed Autonomous. --- .../main/java/org/firstinspires/ftc/teamcode/LeftAuto.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java index a23f43470605..bd0ef2df4ccc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java @@ -65,7 +65,7 @@ public void runOpMode() { //lift1 leftLift.setPower(0.5); rightLift.setPower(0.5); - while(leftLift.getCurrentPosition() < 1200){} + while(leftLift.getCurrentPosition() < 1000){} leftLift.setPower(0); rightLift.setPower(0); @@ -74,7 +74,7 @@ public void runOpMode() { backLeft.setPower(0.5); frontRight.setPower(0.5); backRight.setPower(0.5); - sleep(130); + sleep(200); frontLeft.setPower(0); backLeft.setPower(0); frontRight.setPower(0); From 23156a033dbff5272e89ab0ddb98e048f7dc940f Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sat, 18 Jan 2025 14:46:36 +0800 Subject: [PATCH 127/143] Feat:Fixed Autonomous. --- .../firstinspires/ftc/teamcode/LeftAuto.java | 22 +++-- .../firstinspires/ftc/teamcode/RightAuto.java | 23 +++-- .../test/RobotStateMachine.java | 93 ------------------- .../ftc/teamcode/hardware/RobotChassis.java | 1 - 4 files changed, 30 insertions(+), 109 deletions(-) delete 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/LeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java index bd0ef2df4ccc..e14c85e65c93 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java @@ -5,10 +5,13 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + @Autonomous public class LeftAuto extends LinearOpMode { @Override public void runOpMode() { + RobotTop robotTop = new RobotTop(this); DcMotor frontLeft = hardwareMap.get(DcMotor.class, "FL"); DcMotor backLeft = hardwareMap.get(DcMotor.class, "BL"); DcMotor frontRight = hardwareMap.get(DcMotor.class, "FR"); @@ -18,6 +21,7 @@ public void runOpMode() { DcMotor armStretchMotor = hardwareMap.get(DcMotor.class, "armStretch"); Servo liftServo = hardwareMap.get(Servo.class,"liftServo"); + leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -51,16 +55,18 @@ public void runOpMode() { backLeft.setPower(0.7); frontRight.setPower(0.7); backRight.setPower(0.7); - sleep(600); + sleep(650); frontLeft.setPower(0); backLeft.setPower(0); frontRight.setPower(0); backRight.setPower(0); //stretch arm - armStretchMotor.setPower(0.5); - sleep(1400); - armStretchMotor.setPower(0); +// armStretchMotor.setPower(0.5); +// sleep(1400); +// armStretchMotor.setPower(0); + robotTop.setTurnPosition(0.2); + sleep(500); //lift1 leftLift.setPower(0.5); @@ -92,9 +98,11 @@ public void runOpMode() { sleep(500); //drawback arm - armStretchMotor.setPower(-0.5); - sleep(1400); - armStretchMotor.setPower(0); +// armStretchMotor.setPower(-0.5); +// sleep(1400); +// armStretchMotor.setPower(0); + robotTop.setTurnPosition(0); + sleep(500); //back frontLeft.setPower(-0.7); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java index fcc507c38624..4d7727d2fcd1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java @@ -5,10 +5,13 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + @Autonomous public class RightAuto extends LinearOpMode { @Override public void runOpMode() { + RobotTop robotTop = new RobotTop(this); DcMotor frontLeft = hardwareMap.get(DcMotor.class, "FL"); DcMotor backLeft = hardwareMap.get(DcMotor.class, "BL"); DcMotor frontRight = hardwareMap.get(DcMotor.class, "FR"); @@ -58,14 +61,16 @@ public void runOpMode() { backRight.setPower(0); //stretch arm - armStretchMotor.setPower(0.5); - sleep(1400); - armStretchMotor.setPower(0); +// armStretchMotor.setPower(0.5); +// sleep(1400); +// armStretchMotor.setPower(0); + robotTop.setTurnPosition(0.2); + sleep(500); //lift1 leftLift.setPower(0.5); rightLift.setPower(0.5); - while(leftLift.getCurrentPosition() < 1200){} + while(leftLift.getCurrentPosition() < 1000){} leftLift.setPower(0); rightLift.setPower(0); @@ -74,7 +79,7 @@ public void runOpMode() { backLeft.setPower(0.5); frontRight.setPower(0.5); backRight.setPower(0.5); - sleep(130); + sleep(100); frontLeft.setPower(0); backLeft.setPower(0); frontRight.setPower(0); @@ -92,9 +97,11 @@ public void runOpMode() { sleep(500); //drawback arm - armStretchMotor.setPower(-0.5); - sleep(1400); - armStretchMotor.setPower(0); +// armStretchMotor.setPower(-0.5); +// sleep(1400); +// armStretchMotor.setPower(0); + robotTop.setTurnPosition(0); + sleep(500); //back frontLeft.setPower(-0.7); 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 deleted file mode 100644 index 21a66a27afb1..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/RobotStateMachine.java +++ /dev/null @@ -1,93 +0,0 @@ -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.setArmLeftTurnPosition(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/hardware/RobotChassis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotChassis.java index 3bd5ec67fb3c..5d29c45a8e21 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,4 +1,3 @@ -//底盘代码 package org.firstinspires.ftc.teamcode.hardware; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; From 5b341534c93fb6242d1d7eaef2cbee6990104324 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sat, 18 Jan 2025 18:12:23 +0800 Subject: [PATCH 128/143] Feat:Fixed Autonomous. --- .../main/java/org/firstinspires/ftc/teamcode/LeftAuto.java | 7 +++---- .../java/org/firstinspires/ftc/teamcode/RightAuto.java | 4 ++-- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java index e14c85e65c93..9d4827856b9d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java @@ -21,7 +21,6 @@ public void runOpMode() { DcMotor armStretchMotor = hardwareMap.get(DcMotor.class, "armStretch"); Servo liftServo = hardwareMap.get(Servo.class,"liftServo"); - leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -32,7 +31,6 @@ public void runOpMode() { leftLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); rightLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - frontRight.setDirection(DcMotor.Direction.REVERSE); rightLift.setDirection(DcMotor.Direction.REVERSE); armStretchMotor.setDirection(DcMotor.Direction.REVERSE); @@ -65,7 +63,7 @@ public void runOpMode() { // armStretchMotor.setPower(0.5); // sleep(1400); // armStretchMotor.setPower(0); - robotTop.setTurnPosition(0.2); + robotTop.setTurnPosition(0.6); sleep(500); //lift1 @@ -94,7 +92,7 @@ public void runOpMode() { rightLift.setPower(0); //Release specimen - liftServo.setPosition(0.2); + liftServo.setPosition(0.3); sleep(500); //drawback arm @@ -104,6 +102,7 @@ public void runOpMode() { robotTop.setTurnPosition(0); sleep(500); + //back frontLeft.setPower(-0.7); backLeft.setPower(-0.7); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java index 4d7727d2fcd1..c6f7c6abc790 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java @@ -93,14 +93,14 @@ public void runOpMode() { rightLift.setPower(0); //Release specimen - liftServo.setPosition(0.2); + liftServo.setPosition(0.6); sleep(500); //drawback arm // armStretchMotor.setPower(-0.5); // sleep(1400); // armStretchMotor.setPower(0); - robotTop.setTurnPosition(0); + robotTop.setTurnPosition(0.3); sleep(500); //back From f549603b56e1df85084ddd3431b8add12d359480 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sat, 18 Jan 2025 18:13:56 +0800 Subject: [PATCH 129/143] Code of FTC-Shanghai 1.18 --- .../firstinspires/ftc/teamcode/LeftAuto.java | 17 +- .../ftc/teamcode/ManualOpMode.java | 568 +++++++++--------- .../firstinspires/ftc/teamcode/RightAuto.java | 4 +- .../advancedManual/AdvancedManual.java | 281 --------- .../ftc/teamcode/hardware/PIDController.java | 19 - .../ftc/teamcode/hardware/RobotTop.java | 27 +- .../teamcode/hardware/SparkFunOTOSSensor.java | 48 -- .../ArmStateMachine.java | 5 +- .../teamcode/previousCode/PreviousManual.java | 370 ++++++++++++ .../Task.java | 2 +- .../TaskManager.java | 2 +- .../test/ServoMgrTest.java | 4 +- .../test/TimedServoTest.java | 6 +- .../test/TurnServoTest.java | 2 +- .../ftc/teamcode/test/Armtest.java | 2 + .../ftc/teamcode/test/AutoTest.java | 2 + .../ftc/teamcode/test/PIDControllerTest.java | 2 + .../ftc/teamcode/test/PIDControllerTest2.java | 2 + .../ftc/teamcode/test/ServoTest.java | 1 + .../ftc/teamcode/test/ThreadTest.java | 78 --- .../ftc/teamcode/test/WheelOffset.java | 1 + .../ftc/teamcode/test/tttest.java | 36 -- 22 files changed, 686 insertions(+), 793 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/PIDController.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{advancedManual => previousCode}/ArmStateMachine.java (95%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/PreviousManual.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{advancedManual => previousCode}/Task.java (92%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{advancedManual => previousCode}/TaskManager.java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{advancedManual => previousCode}/test/ServoMgrTest.java (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{advancedManual => previousCode}/test/TimedServoTest.java (81%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{advancedManual => previousCode}/test/TurnServoTest.java (94%) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ThreadTest.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/tttest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java index e14c85e65c93..9ae4cd3acd71 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java @@ -1,7 +1,7 @@ package org.firstinspires.ftc.teamcode; -import com.qualcomm. robotcore.eventloop. opmode.Autonomous; -import com.qualcomm. robotcore. eventloop. opmode. LinearOpMode; +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.Servo; @@ -16,10 +16,10 @@ public void runOpMode() { DcMotor backLeft = hardwareMap.get(DcMotor.class, "BL"); DcMotor frontRight = hardwareMap.get(DcMotor.class, "FR"); DcMotor backRight = hardwareMap.get(DcMotor.class, "BR"); - DcMotor leftLift = hardwareMap.get(DcMotor.class,"leftLift"); - DcMotor rightLift = hardwareMap.get(DcMotor.class,"rightLift"); + DcMotor leftLift = hardwareMap.get(DcMotor.class, "leftLift"); + DcMotor rightLift = hardwareMap.get(DcMotor.class, "rightLift"); DcMotor armStretchMotor = hardwareMap.get(DcMotor.class, "armStretch"); - Servo liftServo = hardwareMap.get(Servo.class,"liftServo"); + Servo liftServo = hardwareMap.get(Servo.class, "liftServo"); leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -65,13 +65,14 @@ public void runOpMode() { // armStretchMotor.setPower(0.5); // sleep(1400); // armStretchMotor.setPower(0); - robotTop.setTurnPosition(0.2); + robotTop.setTurnPosition(0.6); sleep(500); //lift1 leftLift.setPower(0.5); rightLift.setPower(0.5); - while(leftLift.getCurrentPosition() < 1000){} + while (leftLift.getCurrentPosition() < 1000) { + } leftLift.setPower(0); rightLift.setPower(0); @@ -101,7 +102,7 @@ public void runOpMode() { // armStretchMotor.setPower(-0.5); // sleep(1400); // armStretchMotor.setPower(0); - robotTop.setTurnPosition(0); + robotTop.setTurnPosition(0.3); sleep(500); //back 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 fce72be16d6b..22e1b5be8c04 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode; -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; @@ -8,11 +7,10 @@ import org.firstinspires.ftc.teamcode.hardware.RobotChassis; import org.firstinspires.ftc.teamcode.hardware.RobotTop; -@Disabled @TeleOp public class ManualOpMode extends LinearOpMode { enum ArmState { - IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED, LOCKING, UNLOCKING } enum LiftState { @@ -24,348 +22,326 @@ enum LiftState { ArmState armState; LiftState liftState; Gamepad previousGamepad1; + Gamepad previousGamepad2; - //TODO: test the constants + //Constants final int STRETCH_BACK_POSITION = 70; final int STRETCH_OUT_POSITION = 1500; - final double SPIN_DEFAULT_POSITION = 0.3; - final double SPIN_HOVERING_POSITION = 1; + final double SPIN_DEFAULT_POSITION = 0.2; + final double SPIN_HOVERING_POSITION = 0.85; final double SPIN_DOWN_POSITION = 0; - final double TURN_BACK_POSITION = 0.5; - final double TURN_HOVERING_POSITION = 0.8; - final double TURN_DOWN_POSITION = 0.85; + final double TURN_BACK_POSITION = 0.3; + final double TURN_HOVERING_POSITION = 0.62; + final double TURN_DOWN_POSITION = 0.7; final double GRAB_OPEN_POSITION = 0.4; final double GRAB_CLOSE_POSITION = 0.92; final double TOP_BACK = 0.03; final double TOP_OUT = 0.66; + // Variables boolean isGrabbing; boolean topServoOut; boolean backGrabOpen; int liftPosition; - double turnPosition; + double targetTurnPosition; + double currentTurnPosition; + boolean spinLeft; + boolean spinRight; @Override public void runOpMode() { previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); + previousGamepad2 = new Gamepad(); + previousGamepad2.copy(gamepad2); this.robotTop = new RobotTop(this); this.robotChassis = new RobotChassis(this); this.armState = ArmState.IDLE; this.liftState = LiftState.DISABLED; this.isGrabbing = false; this.topServoOut = false; - this.backGrabOpen = false; this.liftPosition = 0; + this.backGrabOpen = false; + this.targetTurnPosition = TURN_BACK_POSITION; + spinLeft = false; + spinRight = false; + waitForStart(); - robotTop.setTurnPosition(TURN_BACK_POSITION); + // robotTop.setTurnPosition(TURN_BACK_POSITION); robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); - while (opModeIsActive()) { - robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); + int armStretchStartPos = robotTop.getArmStretchPosition(); + telemetry.addData("aSTARTpos", armStretchStartPos); + telemetry.update(); - // robotLift - liftPosition = robotTop.getLiftPosition(); - turnPosition = robotTop.getTurnPosition(); + if (isStopRequested()) return; - if(gamepad1.x && !gamepad1.left_bumper){ - robotTop.setStretchPower(0.5); - } else if (gamepad1.x && gamepad1.left_bumper) { - robotTop.setStretchPower(-0.5); - }else{ - robotTop.setStretchPower(0); - } - - if(gamepad1.y && !gamepad1.left_bumper){ - turnPosition += 0.005; - } else if (gamepad1.y && gamepad1.left_bumper) { - turnPosition -= 0.005; - } - robotTop.setTurnPosition(turnPosition); - - if(gamepad1.b && !previousGamepad1.b){ - if(isGrabbing){ - robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); - }else { - robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + while (opModeIsActive()) { + robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); + if (gamepad1.a && !previousGamepad1.a) { + if (backGrabOpen) { + robotTop.setLiftServoPosition(0.1); + } else { + robotTop.setLiftServoPosition(0.6); } - isGrabbing = !isGrabbing; - } - - if(gamepad1.dpad_up){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.005)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.005)); - } - if(gamepad1.dpad_down){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.005)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.005)); + backGrabOpen = !backGrabOpen; } - if(gamepad1.dpad_left){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.005)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.005)); + switch (armState) { + case IDLE: + handleIdleState(); + break; + case TURNING_OUT: + handleTurningOutState(); + break; + case TURNED: + handleTurnedState(); + break; + case TURNING_BACK: + handleTurningBackState(); + break; + case WITHDRAWING: + handleWithdrawingState(); + break; + case LOCKED: + handleLockedState(); + break; + case LOCKING: + handleLockingState(); + break; + case UNLOCKING: + handleUnlockingState(); + break; } - if(gamepad1.dpad_right){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.005)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.005)); + switch (liftState) { + case DISABLED: + handleDisabledState(); + break; + case RUNNING: + handleRunningState(); + break; } + telemetry.addData("arm", armState); + telemetry.addData("lift", liftState); + telemetry.addData("armPos", robotTop.getTurnPosition()); + telemetry.addData("armStretchPosition", robotTop.getArmStretchPosition()); + telemetry.update(); + previousGamepad1.copy(gamepad1); + previousGamepad2.copy(gamepad2); + sleep(10); + } - if(gamepad1.right_trigger != 0){ - robotTop.setLeftPower(0.5); - robotTop.setLiftTargetPos(robotTop.getLiftPosition()); - } else if (gamepad1.left_trigger != 0) { - robotTop.setLeftPower(-0.5); - robotTop.setLiftTargetPos(robotTop.getLiftPosition()); - }else{ - if(robotTop.getLiftPosition() >= 200){ - robotTop.updateLiftPID(); - }else{ - robotTop.setLeftPower(0); - } - } + } - if (gamepad1.right_bumper && !previousGamepad1.right_bumper) { - if (topServoOut) { - robotTop.setTopServoPosition(TOP_BACK); - } else { - robotTop.setTopServoPosition(TOP_OUT); - } - topServoOut = !topServoOut; + protected void handleIdleState() { + if (gamepad1.x && !previousGamepad1.x) { + robotTop.setStretchPower(0.9); + targetTurnPosition = TURN_HOVERING_POSITION; +// robotTop.setTurnPosition(TURN_HOVERING_POSITION); + robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION); + robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION); + armState = ArmState.TURNING_OUT; + } + if (gamepad1.b && !previousGamepad1.b) { + if (isGrabbing) { + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + } else { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); } + } + if (liftState == LiftState.RUNNING) { + liftState = LiftState.DISABLED; + } + if (gamepad1.dpad_up) { + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); + } + if (gamepad1.dpad_down) { + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); + } + if (gamepad1.dpad_left) { + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); + } + if (gamepad1.dpad_right) { + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); + } + } - if (gamepad1.a && !previousGamepad1.a) { - if (backGrabOpen) { - robotTop.setLiftServoPosition(0.1); - } else { - robotTop.setLiftServoPosition(0.6); - } - backGrabOpen = !backGrabOpen; + protected void handleTurningOutState() { + if (robotTop.getArmStretchPosition() >= STRETCH_OUT_POSITION) { + robotTop.setStretchPower(0); + armState = ArmState.TURNED; + } + currentTurnPosition = robotTop.getTurnPosition(); + if (currentTurnPosition > targetTurnPosition + 0.014) { + robotTop.setTurnPosition(currentTurnPosition - 0.014); + } else if (currentTurnPosition < targetTurnPosition - 0.014) { + robotTop.setTurnPosition(currentTurnPosition + 0.014); + } else { + robotTop.setTurnPosition(targetTurnPosition); + } +// if(robotTop.getTurnPosition() >= TURN_DOWN_POSITION){ +// robotTop.setStretchPower(0); +// armState = ArmState.TURNED; +// } +// robotTop.setTurnPosition(robotTop.getTurnPosition() + 0.005); + } + + protected void handleTurnedState() { + if (gamepad1.x && !previousGamepad1.x) { + robotTop.setStretchPower(-0.9); + targetTurnPosition = TURN_BACK_POSITION; + // robotTop.setTurnPosition(TURN_BACK_POSITION); + spinRight = false; + spinLeft = false; + armState = ArmState.WITHDRAWING; + } + if (gamepad1.b && !previousGamepad1.b) { + if (isGrabbing) { + robotTop.setTurnPosition(TURN_DOWN_POSITION); + sleep(500); + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + sleep(500); + robotTop.setTurnPosition(TURN_HOVERING_POSITION); + isGrabbing = !isGrabbing; + } else { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + isGrabbing = !isGrabbing; } + } +// if(gamepad1.dpad_up){ +// robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); +// robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); +// } +// if(gamepad1.dpad_down){ +// robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); +// robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); +// } +// if(gamepad1.dpad_left){ +// robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); +// robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); +// } +// if(gamepad1.dpad_right){ +// robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); +// robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); +// } + if (gamepad1.left_bumper && !previousGamepad1.left_bumper) { + spinLeft = !spinLeft; + spinRight = false; + } + if (gamepad1.right_bumper && !previousGamepad1.right_bumper) { + spinRight = !spinRight; + spinLeft = false; + } + if (spinLeft) { + robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION + 0.15); + robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION - 0.15); + } else if (spinRight) { + robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION - 0.15); + robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION + 0.15); + } else { + robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION); + robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION); + } -// 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; -// } -// } + } + protected void handleTurningBackState() { + //if necessary + } + protected void handleWithdrawingState() { + if (robotTop.getArmStretchPosition() <= STRETCH_BACK_POSITION) { + robotTop.setStretchPower(0); + robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); + robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); + armState = ArmState.IDLE; + } + currentTurnPosition = robotTop.getTurnPosition(); + if (currentTurnPosition > targetTurnPosition + 0.014) { + robotTop.setTurnPosition(currentTurnPosition - 0.014); + } else if (currentTurnPosition < targetTurnPosition - 0.014) { + robotTop.setTurnPosition(currentTurnPosition + 0.014); + } else { + robotTop.setTurnPosition(targetTurnPosition); + } +// if(robotTop.getTurnPosition() <= TURN_BACK_POSITION){ +// robotTop.setStretchPower(0); +// armState = ArmState.IDLE; +// } +// robotTop.setTurnPosition(robotTop.getTurnPosition() - 0.005); + } -// if (liftState == LiftState.BOTTOM && armState == ArmState.IDLE) { -// if (gamepad1.left_trigger != 0) { -// armStretchPos = STRETCH_OUT_POSITION; -// robotTop.setArmStretchPosition(armStretchPos); -// armState = ArmState.STRETCHED; -// sleep(1000); -// } -// if (gamepad1.right_trigger != 0) { -// armStretchPos = STRETCH_OUT_POSITION; -// robotTop.setArmStretchPosition(armStretchPos); -// armState = ArmState.STRETCHED; -// sleep(1000); -// } -// } else { -// if (gamepad1.left_trigger != 0) { -// 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); -// } else { -// 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 (topServoOut) { -// robotTop.setTopServoPosition(0.05); -// } else { -// robotTop.setTopServoPosition(0.6); -// } -// topServoOut = !topServoOut; -// } -// 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) { -// if (armState == ArmState.IDLE) { -// robotTop.setTopServoPosition(0); -// armStretchPos = STRETCH_OUT_POSITION; -// armState = ArmState.TURNING_OUT; -// robotTop.setArmStretchPosition(armStretchPos); -// } else if (armState == ArmState.TURNED) { -// armState = ArmState.TURNING_BACK; -// robotTop.setArmLeftSpinPosition(SPIN_Y_DEFAULT_POSITION); -// } -// } -// if (armState == ArmState.WITHDRAWING) { -// if (armStretchPos <= STRETCH_BACK_POSITION) { -// armState = ArmState.IDLE; -// } else { -// armStretchPos -= 0.03; -// robotTop.setArmStretchPosition(armStretchPos); -// } -// } -// if (armState == ArmState.TURNING_OUT) { -// if (armTurnPos >= TURN_OUT_HOVERING_POSITION - 0.05) { -// armTurnPos = TURN_OUT_HOVERING_POSITION; -// robotTop.setArmLeftTurnPosition(armTurnPos); -// robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); -// armState = ArmState.TURNED; -// } else { -// armTurnPos += 0.03; -// robotTop.setArmLeftTurnPosition(armTurnPos); -// } -// } -// if (armState == ArmState.TURNING_BACK) { -// if (armTurnPos <= TURN_BACK_POSITION + 0.05) { -// armTurnPos = TURN_BACK_POSITION; -// robotTop.setArmLeftTurnPosition(armTurnPos); -// robotTop.setArmRightTurnPosition(SPIN_X_DEFAULT_POSITION); -// armState = ArmState.WITHDRAWING; -// } else { -// armTurnPos -= 0.03; -// robotTop.setArmLeftTurnPosition(armTurnPos); -// } -// } -// if (armState == ArmState.STRETCHED) { -// if (gamepad1.x && !previousGamepad1.x) { -// robotTop.setTopServoPosition(0); -// armState = ArmState.WITHDRAWING; -// } -// } -// if (gamepad1.b && !previousGamepad1.b) { -// robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); -// sleep(500); -// if (!armGrabbing && armState == ArmState.TURNED) { -// robotTop.setArmLeftTurnPosition(TURN_OUT_DOWN_POSITION); -// robotTop.setArmRightTurnPosition(SPIN_X_DOWN_POSITION); -// grabbingFlag = true; -// } else { -// robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); -// armGrabbing = false; -// } -// } -// if (grabbingFlag && !gamepad1.b) { -// robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); -// sleep(500); -// robotTop.setArmLeftTurnPosition(TURN_OUT_HOVERING_POSITION); -// robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); -// armGrabbing = true; -// grabbingFlag = false; -// } -// robotTop.setArmStretchPosition(armStretchPos); -// if (gamepad1.dpad_up) { -// armSpinXPos = Math.min(1, armSpinXPos + 0.02); -// robotTop.setArmRightTurnPosition(armSpinXPos); -// } else if (gamepad1.dpad_down) { -// armSpinXPos = Math.max(0, armSpinXPos - 0.02); -// robotTop.setArmRightTurnPosition(armSpinXPos); -// } -// if (gamepad1.dpad_right) { -// armSpinYPos = Math.min(1, armSpinYPos + 0.05); -// robotTop.setArmLeftSpinPosition(armSpinYPos); -// } else if (gamepad1.dpad_left) { -// armSpinYPos = Math.max(0, armSpinYPos - 0.05); -// robotTop.setArmLeftSpinPosition(armSpinYPos); -// } -// if (gamepad1.a && !previousGamepad1.a) { -// if (backGrabOpen) { -// robotTop.setLiftServoPosition(0.1); -// } else { -// robotTop.setLiftServoPosition(0.6); -// } -// backGrabOpen = !backGrabOpen; -// } -// -// //vision -// if ((gamepad2.left_trigger != 0 && previousGamepad2.left_trigger == 0 -// ) || (gamepad2.right_trigger != 0 && previousGamepad2.right_trigger == 0)) { -// recognitionAngle = robotVisionAngle.getDetectedAngle(); -// robotTop.setArmLeftSpinPosition(calculateSpinY(recognitionAngle)); -// } -// -// // telemetry -// telemetry.addData("liftPos", liftPosition); -// telemetry.addData("state", liftState); -// telemetry.addData("ArmState", armState); -// telemetry.addData("XPos", armSpinXPos); -// telemetry.addData("YPos", armSpinYPos); -// telemetry.addData("angle", recognitionAngle); -// telemetry.update(); - previousGamepad1.copy(gamepad1); -// previousGamepad2.copy(gamepad2); - sleep(10); + protected void handleLockedState() { + if (gamepad1.x && !previousGamepad1.x) { + liftState = LiftState.DISABLED; + armState = ArmState.UNLOCKING; + // robotTop.setTurnPosition(TURN_BACK_POSITION); + targetTurnPosition = TURN_BACK_POSITION; } } - protected double calculateSpinY(double angle) { - if (angle < -180 || angle > 180) { - return 0; + protected void handleLockingState() { + currentTurnPosition = robotTop.getTurnPosition(); + if (currentTurnPosition > targetTurnPosition + 0.014) { + robotTop.setTurnPosition(currentTurnPosition - 0.014); + } else if (currentTurnPosition < targetTurnPosition - 0.014) { + robotTop.setTurnPosition(currentTurnPosition + 0.014); + } else { + robotTop.setTurnPosition(targetTurnPosition); + armState = ArmState.LOCKED; } + } - double adjustedAngle; - if (angle <= 0) { - adjustedAngle = -angle / 180 + 0.1; + protected void handleUnlockingState() { + currentTurnPosition = robotTop.getTurnPosition(); + if (currentTurnPosition > targetTurnPosition + 0.014) { + robotTop.setTurnPosition(currentTurnPosition - 0.014); + } else if (currentTurnPosition < targetTurnPosition - 0.014) { + robotTop.setTurnPosition(currentTurnPosition + 0.014); } else { - adjustedAngle = 0.6 - angle / 180; + robotTop.setTurnPosition(targetTurnPosition); + armState = ArmState.IDLE; } + } - return Math.max(0, Math.min(adjustedAngle, 1)); // 确保值在0到1之间 + protected void handleDisabledState() { + if (armState != ArmState.IDLE || armState != ArmState.LOCKED) { + liftState = LiftState.RUNNING; + } + if (gamepad1.right_trigger != 0 || gamepad1.left_trigger != 0) { + // robotTop.setTurnPosition(TURN_DOWN_POSITION - 0.1); + targetTurnPosition = TURN_DOWN_POSITION - 0.1; + armState = ArmState.LOCKING; + liftState = LiftState.RUNNING; + } + } + + protected void handleRunningState() { + if (gamepad1.right_trigger != 0) { + robotTop.setLeftPower(0.5); + robotTop.setTopServoPosition(TOP_BACK); + robotTop.setLiftTargetPos(robotTop.getLiftPosition()); + } else if (gamepad1.left_trigger != 0) { + robotTop.setLeftPower(-0.5); + robotTop.setTopServoPosition(TOP_BACK); + robotTop.setLiftTargetPos(robotTop.getLiftPosition()); + } else { + if (robotTop.getLiftPosition() >= 200) { + robotTop.updateLiftPID(); + } else { + robotTop.setLeftPower(0); + } + } + if (gamepad2.left_bumper && !previousGamepad2.left_bumper) { + if (topServoOut) { + robotTop.setTopServoPosition(TOP_BACK); + } else { + robotTop.setTopServoPosition(TOP_OUT); + } + topServoOut = !topServoOut; + } } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java index 4d7727d2fcd1..03b3b189bebc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java @@ -64,7 +64,7 @@ public void runOpMode() { // armStretchMotor.setPower(0.5); // sleep(1400); // armStretchMotor.setPower(0); - robotTop.setTurnPosition(0.2); + robotTop.setTurnPosition(0.6); sleep(500); //lift1 @@ -100,7 +100,7 @@ public void runOpMode() { // armStretchMotor.setPower(-0.5); // sleep(1400); // armStretchMotor.setPower(0); - robotTop.setTurnPosition(0); + robotTop.setTurnPosition(0.3); sleep(500); //back 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 deleted file mode 100644 index 1188787a6b4c..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java +++ /dev/null @@ -1,281 +0,0 @@ -package org.firstinspires.ftc.teamcode.advancedManual; - -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; - -// The ManualOp currently used is a piece of shit. -// I'll write a better one here (if the time is enough). -@TeleOp -public class AdvancedManual extends LinearOpMode { - enum ArmState{ - IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED - } - enum LiftState{ - DISABLED, RUNNING - } - RobotTop robotTop; - RobotChassis robotChassis; - ArmState armState; - LiftState liftState; - Gamepad previousGamepad1; - - //TODO: test the constants - final int STRETCH_BACK_POSITION = 70; - final int STRETCH_OUT_POSITION = 1500; - final double SPIN_DEFAULT_POSITION = 0.2 ; - final double SPIN_HOVERING_POSITION = 1; - final double SPIN_DOWN_POSITION = 0; - final double TURN_BACK_POSITION = 0; - final double TURN_HOVERING_POSITION = 0.32; - final double TURN_DOWN_POSITION = 0.4; - final double GRAB_OPEN_POSITION = 0.4; - final double GRAB_CLOSE_POSITION = 0.92; - final double TOP_BACK = 0.03; - final double TOP_OUT = 0.66; - - boolean isGrabbing; - boolean topServoOut; - boolean backGrabOpen; - int liftPosition; - double targetTurnPosition; - double currentTurnPosition; - @Override - public void runOpMode(){ - previousGamepad1 = new Gamepad(); - previousGamepad1.copy(gamepad1); - this.robotTop = new RobotTop(this); - this.robotChassis = new RobotChassis(this); - this.armState = ArmState.IDLE; - this.liftState = LiftState.DISABLED; - this.isGrabbing = false; - this.topServoOut = false; - this.liftPosition = 0; - this.backGrabOpen = false; - this.targetTurnPosition = TURN_BACK_POSITION; - waitForStart(); - // robotTop.setTurnPosition(TURN_BACK_POSITION); - robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); - robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); - int armStretchStartPos = robotTop.getArmStretchPosition(); - telemetry.addData("aSTARTpos", armStretchStartPos); - telemetry.update(); - - if (isStopRequested()) return; - - while (opModeIsActive()) { - robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); - if (gamepad1.a && !previousGamepad1.a) { - if (backGrabOpen) { - robotTop.setLiftServoPosition(0.1); - } else { - robotTop.setLiftServoPosition(0.6); - } - backGrabOpen = !backGrabOpen; - } - switch (armState){ - case IDLE: - handleIdleState(); - break; - case TURNING_OUT: - handleTurningOutState(); - break; - case TURNED: - handleTurnedState(); - break; - case TURNING_BACK: - handleTurningBackState(); - break; - case WITHDRAWING: - handleWithdrawingState(); - break; - case LOCKED: - handleLockedState(); - break; - } - switch (liftState){ - case DISABLED: - handleDisabledState(); - break; - case RUNNING: - handleRunningState(); - break; - } - telemetry.addData("arm",armState); - telemetry.addData("lift",liftState); - telemetry.addData("armPos",robotTop.getTurnPosition()); - telemetry.addData("armStretchPosition", robotTop.getArmStretchPosition()); - telemetry.update(); - previousGamepad1.copy(gamepad1); - sleep(10); - } - - } - protected void handleIdleState(){ - if(gamepad1.x && !previousGamepad1.x){ - robotTop.setStretchPower(0.9); - targetTurnPosition = TURN_HOVERING_POSITION; -// robotTop.setTurnPosition(TURN_HOVERING_POSITION); - robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION); - robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION); - armState = ArmState.TURNING_OUT; - } - if(gamepad1.b && !previousGamepad1.b){ - if(isGrabbing){ - robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); - }else { - robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); - } - } - if(liftState == LiftState.RUNNING){ - liftState = LiftState.DISABLED; - } - if(gamepad1.dpad_up){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); - } - if(gamepad1.dpad_down){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); - } - if(gamepad1.dpad_left){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); - } - if(gamepad1.dpad_right){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); - } - } - - protected void handleTurningOutState(){ - if(robotTop.getArmStretchPosition() >= STRETCH_OUT_POSITION){ - robotTop.setStretchPower(0); - armState = ArmState.TURNED; - } - currentTurnPosition = robotTop.getTurnPosition(); - if(currentTurnPosition > targetTurnPosition + 0.014){ - robotTop.setTurnPosition(currentTurnPosition - 0.014); - } else if (currentTurnPosition < targetTurnPosition - 0.014) { - robotTop.setTurnPosition(currentTurnPosition + 0.014); - }else{ - robotTop.setTurnPosition(targetTurnPosition); - } -// if(robotTop.getTurnPosition() >= TURN_DOWN_POSITION){ -// robotTop.setStretchPower(0); -// armState = ArmState.TURNED; -// } -// robotTop.setTurnPosition(robotTop.getTurnPosition() + 0.005); - } - protected void handleTurnedState(){ - if(gamepad1.x && !previousGamepad1.x){ - robotTop.setStretchPower(-0.9); - targetTurnPosition = TURN_BACK_POSITION; - // robotTop.setTurnPosition(TURN_BACK_POSITION); - robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); - robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); - armState = ArmState.WITHDRAWING; - } - if(gamepad1.b && !previousGamepad1.b){ - if(isGrabbing){ - robotTop.setTurnPosition(TURN_DOWN_POSITION); - sleep(500); - robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); - sleep(500); - robotTop.setTurnPosition(TURN_HOVERING_POSITION); - isGrabbing = !isGrabbing; - }else { - robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); - isGrabbing = !isGrabbing; - } - } - if(gamepad1.dpad_up){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); - } - if(gamepad1.dpad_down){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); - } - if(gamepad1.dpad_left){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); - } - if(gamepad1.dpad_right){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); - } - } - protected void handleTurningBackState(){ - //if necessary - } - protected void handleWithdrawingState(){ - if(robotTop.getArmStretchPosition() <= STRETCH_BACK_POSITION){ - robotTop.setStretchPower(0); - armState = ArmState.IDLE; - } - currentTurnPosition = robotTop.getTurnPosition(); - if(currentTurnPosition > targetTurnPosition + 0.014){ - robotTop.setTurnPosition(currentTurnPosition - 0.014); - } else if (currentTurnPosition < targetTurnPosition - 0.014) { - robotTop.setTurnPosition(currentTurnPosition + 0.014); - }else{ - robotTop.setTurnPosition(targetTurnPosition); - } -// if(robotTop.getTurnPosition() <= TURN_BACK_POSITION){ -// robotTop.setStretchPower(0); -// armState = ArmState.IDLE; -// } -// robotTop.setTurnPosition(robotTop.getTurnPosition() - 0.005); - } - - protected void handleLockedState(){ - if(gamepad1.x && !previousGamepad1.x){ - liftState = LiftState.DISABLED; - robotTop.setStretchPower(-0.8); - armState = ArmState.WITHDRAWING; - targetTurnPosition = TURN_BACK_POSITION; - } - } - - protected void handleDisabledState(){ - if(armState != ArmState.IDLE || armState != ArmState.LOCKED){ - liftState = LiftState.RUNNING; - } - if(gamepad1.right_trigger != 0 || gamepad1.left_trigger != 0){ - robotTop.setStretchPower(0.8); - sleep(700); - robotTop.setStretchPower(0); - armState = ArmState.LOCKED; - liftState = LiftState.DISABLED; - } - } - protected void handleRunningState(){ - if(gamepad1.right_trigger != 0){ - robotTop.setLeftPower(0.5); - robotTop.setTopServoPosition(TOP_BACK); - robotTop.setLiftTargetPos(robotTop.getLiftPosition()); - } else if (gamepad1.left_trigger != 0) { - robotTop.setLeftPower(-0.5); - robotTop.setTopServoPosition(TOP_BACK); - robotTop.setLiftTargetPos(robotTop.getLiftPosition()); - }else{ - if(robotTop.getLiftPosition() >= 200){ - robotTop.updateLiftPID(); - }else{ - robotTop.setLeftPower(0); - } - } - if (gamepad1.left_bumper && !previousGamepad1.left_bumper) { - if (topServoOut) { - robotTop.setTopServoPosition(TOP_BACK); - } else { - robotTop.setTopServoPosition(TOP_OUT); - } - topServoOut = !topServoOut; - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/PIDController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/PIDController.java deleted file mode 100644 index 02ef1d73d985..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/PIDController.java +++ /dev/null @@ -1,19 +0,0 @@ -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.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.Telemetry; - -public class PIDController { - OpMode opMode; - HardwareMap hardwareMap; - Telemetry telemetry; - public PIDController(LinearOpMode opMode) { - this.opMode = opMode; - hardwareMap = opMode.hardwareMap; - telemetry = opMode.telemetry; - } - -} 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 326ad968d58b..accf69eb99c9 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 @@ -101,11 +101,11 @@ public double getArmRightTurnPosition() { return armRightTurnServo.getPosition(); } - public double getTurnPosition(){ + public double getTurnPosition() { return getArmLeftTurnPosition(); } - public void setTurnPosition(double position){ + public void setTurnPosition(double position) { setArmLeftTurnPosition(position); setArmRightTurnPosition(1 - position); } @@ -158,7 +158,7 @@ public double getContainerServoPosition() { return containerServo.getPosition(); } - public void setTargetLiftPosition(int position){ + public void setTargetLiftPosition(int position) { this.targetLiftPosition = position; } @@ -167,7 +167,7 @@ public void setTargetLiftPosition(int position){ double targetPos = 0; double ticks_per_rev = 530; double delta = 0; - double p = 0, i = 0 , d = 0; + double p = 0, i = 0, d = 0; final double Kp = 0.0008, Ki = 0.00001, Kd = -0.004; final double power_max = 0.5; double power = 0; @@ -180,27 +180,26 @@ public void updateLiftPID() { power = 1; else if (delta < -ticks_per_rev * 0.6) power = -1; - else - { + else { p = Kp * delta; i += Ki * delta; d = Kd * (currentPos - previousPos); power = p + i + d; if (power > power_max) power = power_max; - else if(power < -power_max) + else if (power < -power_max) power = -power_max; } setLeftPower(power); - telemetry.addData("power",power); - telemetry.addData("p",p); - telemetry.addData("i",i); - telemetry.addData("d",d); - telemetry.addData("current",currentPos); - telemetry.addData("target",targetPos); + telemetry.addData("power", power); + telemetry.addData("p", p); + telemetry.addData("i", i); + telemetry.addData("d", d); + telemetry.addData("current", currentPos); + telemetry.addData("target", targetPos); } - public void setLiftTargetPos(int pos){ + public void setLiftTargetPos(int pos) { this.targetPos = Math.min(1260, pos); this.targetPos = Math.max(40, targetPos); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java deleted file mode 100644 index 0f2f59952d1d..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java +++ /dev/null @@ -1,48 +0,0 @@ -package org.firstinspires.ftc.teamcode.hardware; - -import com.qualcomm.hardware.sparkfun.SparkFunOTOS; -import com.qualcomm.robotcore.hardware.HardwareMap; - -public class SparkFunOTOSSensor { - private SparkFunOTOS otosDevice; - private double x; - private double y; - private double heading; - - public SparkFunOTOSSensor(HardwareMap hardwareMap) { - // 假设 OTOS 传感器连接到 I2C 端口 - otosDevice = hardwareMap.get(SparkFunOTOS.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; - } - - public double getDistance() { - - return 0; - } -} - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/ArmStateMachine.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/ArmStateMachine.java index a709fc3f953f..c2725998e568 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/ArmStateMachine.java @@ -1,10 +1,11 @@ -package org.firstinspires.ftc.teamcode.advancedManual; +package org.firstinspires.ftc.teamcode.previousCode; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.hardware.Gamepad; import org.firstinspires.ftc.teamcode.hardware.RobotChassis; import org.firstinspires.ftc.teamcode.hardware.RobotTop; - +@Disabled public class ArmStateMachine { enum ArmState{ IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/PreviousManual.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/PreviousManual.java new file mode 100644 index 000000000000..d2c7d2bd017a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/PreviousManual.java @@ -0,0 +1,370 @@ +package org.firstinspires.ftc.teamcode.previousCode; + +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 org.firstinspires.ftc.teamcode.hardware.RobotChassis; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + +@Disabled +@TeleOp +public class PreviousManual extends LinearOpMode { + enum ArmState { + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED + } + + enum LiftState { + DISABLED, RUNNING + } + + RobotTop robotTop; + RobotChassis robotChassis; + ArmState armState; + LiftState liftState; + Gamepad previousGamepad1; + + //TODO: test the constants + final int STRETCH_BACK_POSITION = 70; + final int STRETCH_OUT_POSITION = 1500; + final double SPIN_DEFAULT_POSITION = 0.3; + final double SPIN_HOVERING_POSITION = 1; + final double SPIN_DOWN_POSITION = 0; + final double TURN_BACK_POSITION = 0.5; + final double TURN_HOVERING_POSITION = 0.8; + final double TURN_DOWN_POSITION = 0.85; + final double GRAB_OPEN_POSITION = 0.4; + final double GRAB_CLOSE_POSITION = 0.92; + final double TOP_BACK = 0.03; + final double TOP_OUT = 0.66; + + boolean isGrabbing; + boolean topServoOut; + boolean backGrabOpen; + int liftPosition; + double turnPosition; + + @Override + public void runOpMode() { + previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + this.robotTop = new RobotTop(this); + this.robotChassis = new RobotChassis(this); + this.armState = ArmState.IDLE; + this.liftState = LiftState.DISABLED; + this.isGrabbing = false; + this.topServoOut = false; + this.backGrabOpen = false; + this.liftPosition = 0; + waitForStart(); + robotTop.setTurnPosition(TURN_BACK_POSITION); + robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); + robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); + while (opModeIsActive()) { + robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); + + // robotLift + liftPosition = robotTop.getLiftPosition(); + turnPosition = robotTop.getTurnPosition(); + + if (gamepad1.x && !gamepad1.left_bumper) { + robotTop.setStretchPower(0.5); + } else if (gamepad1.x && gamepad1.left_bumper) { + robotTop.setStretchPower(-0.5); + } else { + robotTop.setStretchPower(0); + } + + if (gamepad1.y && !gamepad1.left_bumper) { + turnPosition += 0.005; + } else if (gamepad1.y && gamepad1.left_bumper) { + turnPosition -= 0.005; + } + robotTop.setTurnPosition(turnPosition); + + if (gamepad1.b && !previousGamepad1.b) { + if (isGrabbing) { + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + } else { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + } + isGrabbing = !isGrabbing; + } + + if (gamepad1.dpad_up) { + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.005)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.005)); + } + if (gamepad1.dpad_down) { + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.005)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.005)); + } + if (gamepad1.dpad_left) { + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.005)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.005)); + } + if (gamepad1.dpad_right) { + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.005)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.005)); + } + + if (gamepad1.right_trigger != 0) { + robotTop.setLeftPower(0.5); + robotTop.setLiftTargetPos(robotTop.getLiftPosition()); + } else if (gamepad1.left_trigger != 0) { + robotTop.setLeftPower(-0.5); + robotTop.setLiftTargetPos(robotTop.getLiftPosition()); + } else { + if (robotTop.getLiftPosition() >= 200) { + robotTop.updateLiftPID(); + } else { + robotTop.setLeftPower(0); + } + } + + if (gamepad1.right_bumper && !previousGamepad1.right_bumper) { + if (topServoOut) { + robotTop.setTopServoPosition(TOP_BACK); + } else { + robotTop.setTopServoPosition(TOP_OUT); + } + topServoOut = !topServoOut; + } + + if (gamepad1.a && !previousGamepad1.a) { + if (backGrabOpen) { + robotTop.setLiftServoPosition(0.1); + } else { + robotTop.setLiftServoPosition(0.6); + } + backGrabOpen = !backGrabOpen; + } + +// 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(1000); +// } +// if (gamepad1.right_trigger != 0) { +// armStretchPos = STRETCH_OUT_POSITION; +// robotTop.setArmStretchPosition(armStretchPos); +// armState = ArmState.STRETCHED; +// sleep(1000); +// } +// } else { +// if (gamepad1.left_trigger != 0) { +// 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); +// } else { +// 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 (topServoOut) { +// robotTop.setTopServoPosition(0.05); +// } else { +// robotTop.setTopServoPosition(0.6); +// } +// topServoOut = !topServoOut; +// } +// 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) { +// if (armState == ArmState.IDLE) { +// robotTop.setTopServoPosition(0); +// armStretchPos = STRETCH_OUT_POSITION; +// armState = ArmState.TURNING_OUT; +// robotTop.setArmStretchPosition(armStretchPos); +// } else if (armState == ArmState.TURNED) { +// armState = ArmState.TURNING_BACK; +// robotTop.setArmLeftSpinPosition(SPIN_Y_DEFAULT_POSITION); +// } +// } +// if (armState == ArmState.WITHDRAWING) { +// if (armStretchPos <= STRETCH_BACK_POSITION) { +// armState = ArmState.IDLE; +// } else { +// armStretchPos -= 0.03; +// robotTop.setArmStretchPosition(armStretchPos); +// } +// } +// if (armState == ArmState.TURNING_OUT) { +// if (armTurnPos >= TURN_OUT_HOVERING_POSITION - 0.05) { +// armTurnPos = TURN_OUT_HOVERING_POSITION; +// robotTop.setArmLeftTurnPosition(armTurnPos); +// robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); +// armState = ArmState.TURNED; +// } else { +// armTurnPos += 0.03; +// robotTop.setArmLeftTurnPosition(armTurnPos); +// } +// } +// if (armState == ArmState.TURNING_BACK) { +// if (armTurnPos <= TURN_BACK_POSITION + 0.05) { +// armTurnPos = TURN_BACK_POSITION; +// robotTop.setArmLeftTurnPosition(armTurnPos); +// robotTop.setArmRightTurnPosition(SPIN_X_DEFAULT_POSITION); +// armState = ArmState.WITHDRAWING; +// } else { +// armTurnPos -= 0.03; +// robotTop.setArmLeftTurnPosition(armTurnPos); +// } +// } +// if (armState == ArmState.STRETCHED) { +// if (gamepad1.x && !previousGamepad1.x) { +// robotTop.setTopServoPosition(0); +// armState = ArmState.WITHDRAWING; +// } +// } +// if (gamepad1.b && !previousGamepad1.b) { +// robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); +// sleep(500); +// if (!armGrabbing && armState == ArmState.TURNED) { +// robotTop.setArmLeftTurnPosition(TURN_OUT_DOWN_POSITION); +// robotTop.setArmRightTurnPosition(SPIN_X_DOWN_POSITION); +// grabbingFlag = true; +// } else { +// robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); +// armGrabbing = false; +// } +// } +// if (grabbingFlag && !gamepad1.b) { +// robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); +// sleep(500); +// robotTop.setArmLeftTurnPosition(TURN_OUT_HOVERING_POSITION); +// robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); +// armGrabbing = true; +// grabbingFlag = false; +// } +// robotTop.setArmStretchPosition(armStretchPos); +// if (gamepad1.dpad_up) { +// armSpinXPos = Math.min(1, armSpinXPos + 0.02); +// robotTop.setArmRightTurnPosition(armSpinXPos); +// } else if (gamepad1.dpad_down) { +// armSpinXPos = Math.max(0, armSpinXPos - 0.02); +// robotTop.setArmRightTurnPosition(armSpinXPos); +// } +// if (gamepad1.dpad_right) { +// armSpinYPos = Math.min(1, armSpinYPos + 0.05); +// robotTop.setArmLeftSpinPosition(armSpinYPos); +// } else if (gamepad1.dpad_left) { +// armSpinYPos = Math.max(0, armSpinYPos - 0.05); +// robotTop.setArmLeftSpinPosition(armSpinYPos); +// } +// if (gamepad1.a && !previousGamepad1.a) { +// if (backGrabOpen) { +// robotTop.setLiftServoPosition(0.1); +// } else { +// robotTop.setLiftServoPosition(0.6); +// } +// backGrabOpen = !backGrabOpen; +// } +// +// //vision +// if ((gamepad2.left_trigger != 0 && previousGamepad2.left_trigger == 0 +// ) || (gamepad2.right_trigger != 0 && previousGamepad2.right_trigger == 0)) { +// recognitionAngle = robotVisionAngle.getDetectedAngle(); +// robotTop.setArmLeftSpinPosition(calculateSpinY(recognitionAngle)); +// } +// +// // telemetry +// telemetry.addData("liftPos", liftPosition); +// telemetry.addData("state", liftState); +// telemetry.addData("ArmState", armState); +// telemetry.addData("XPos", armSpinXPos); +// telemetry.addData("YPos", armSpinYPos); +// telemetry.addData("angle", recognitionAngle); +// telemetry.update(); + previousGamepad1.copy(gamepad1); +// previousGamepad2.copy(gamepad2); + sleep(10); + } + } + + protected double calculateSpinY(double angle) { + if (angle < -180 || angle > 180) { + return 0; + } + + double adjustedAngle; + if (angle <= 0) { + adjustedAngle = -angle / 180 + 0.1; + } else { + adjustedAngle = 0.6 - angle / 180; + } + + return Math.max(0, Math.min(adjustedAngle, 1)); // 确保值在0到1之间 + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/Task.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/Task.java similarity index 92% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/Task.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/Task.java index f72d16f4cf59..21b6f668c748 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/Task.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/Task.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.advancedManual; +package org.firstinspires.ftc.teamcode.previousCode; 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/previousCode/TaskManager.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TaskManager.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/TaskManager.java index 30566ecd3e6d..05db0f3ba7ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TaskManager.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/TaskManager.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.advancedManual; +package org.firstinspires.ftc.teamcode.previousCode; import com.qualcomm.robotcore.hardware.Servo; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/ServoMgrTest.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/ServoMgrTest.java index a66ddc3191e9..3a71cf992687 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/ServoMgrTest.java @@ -1,8 +1,8 @@ -package org.firstinspires.ftc.teamcode.advancedManual.test; +package org.firstinspires.ftc.teamcode.previousCode.test; import android.util.ArrayMap; -import org.firstinspires.ftc.teamcode.advancedManual.Task; +import org.firstinspires.ftc.teamcode.previousCode.Task; import java.util.List; import java.util.Map; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TimedServoTest.java similarity index 81% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TimedServoTest.java index 24d386da3c73..1341ed608f6a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TimedServoTest.java @@ -1,14 +1,12 @@ -package org.firstinspires.ftc.teamcode.advancedManual.test; +package org.firstinspires.ftc.teamcode.previousCode.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.TaskManager; +import org.firstinspires.ftc.teamcode.previousCode.TaskManager; @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 { @Override public void runOpMode(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TurnServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TurnServoTest.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TurnServoTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TurnServoTest.java index 640c3c032dab..2e8d6173a516 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TurnServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TurnServoTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.advancedManual.test; +package org.firstinspires.ftc.teamcode.previousCode.test; 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/test/Armtest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java index 78f5d679251d..e98c11b2e583 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.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.Gamepad; import com.qualcomm.robotcore.hardware.Servo; +@Disabled @TeleOp(group = "Test") public class Armtest extends LinearOpMode { @Override 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 b8546baddfa2..33a474836c54 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 @@ -1,11 +1,13 @@ 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; import com.qualcomm. robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; +@Disabled @Autonomous public class AutoTest extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java index 6ce8e65803ab..5b8ec213d905 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.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 org.firstinspires.ftc.teamcode.hardware.RobotTop; +@Disabled @TeleOp public class PIDControllerTest extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java index 30b02d13981e..299c7287d9ae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java @@ -1,5 +1,6 @@ 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; @@ -7,6 +8,7 @@ import org.firstinspires.ftc.teamcode.hardware.RobotTop; +@Disabled @TeleOp public class PIDControllerTest2 extends LinearOpMode { @Override 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 08edee753ad6..9dd389940c3a 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 @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; +@Disabled @TeleOp(group = "Test") public class ServoTest extends LinearOpMode { @Override 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 deleted file mode 100644 index 60e5c974b123..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ThreadTest.java +++ /dev/null @@ -1,78 +0,0 @@ -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 index 7b4a7b2e71b5..27d0a2205498 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,6 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +@Disabled @TeleOp(group = "Test") public class WheelOffset extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/tttest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/tttest.java deleted file mode 100644 index 5bd7c0d3edfe..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/tttest.java +++ /dev/null @@ -1,36 +0,0 @@ -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.Gamepad; -import com.qualcomm.robotcore.hardware.Servo; - -@TeleOp(group = "Test") -public class tttest extends LinearOpMode { - @Override - public void runOpMode(){ - DcMotor m = hardwareMap.get(DcMotor.class, "m"); - - waitForStart(); - Gamepad previousGamepad1 = new Gamepad(); - previousGamepad1.copy(gamepad1); - - if (isStopRequested()) return; - m.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - while (opModeIsActive()) { - if(gamepad1.y){ - m.setTargetPosition(0); - } - if(gamepad1.a){ - m.setTargetPosition(300); - } - - // telemetry.addData("lp", leftPos); - telemetry.update(); - - sleep(10); - } - } -} From 2c3fbedd1b5d7894d0f7036415e41990a2691d10 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sat, 18 Jan 2025 18:13:56 +0800 Subject: [PATCH 130/143] Code of FTC-Shanghai 1.18 --- .../firstinspires/ftc/teamcode/LeftAuto.java | 17 +- .../ftc/teamcode/ManualOpMode.java | 568 +++++++++--------- .../firstinspires/ftc/teamcode/RightAuto.java | 4 +- .../advancedManual/AdvancedManual.java | 281 --------- .../ftc/teamcode/hardware/PIDController.java | 19 - .../ftc/teamcode/hardware/RobotTop.java | 27 +- .../teamcode/hardware/SparkFunOTOSSensor.java | 48 -- .../ArmStateMachine.java | 5 +- .../teamcode/previousCode/PreviousManual.java | 369 ++++++++++++ .../Task.java | 2 +- .../TaskManager.java | 2 +- .../test/ServoMgrTest.java | 4 +- .../test/TimedServoTest.java | 6 +- .../test/TurnServoTest.java | 2 +- .../ftc/teamcode/test/Armtest.java | 2 + .../ftc/teamcode/test/AutoTest.java | 2 + .../ftc/teamcode/test/PIDControllerTest.java | 2 + .../ftc/teamcode/test/PIDControllerTest2.java | 2 + .../ftc/teamcode/test/ServoTest.java | 1 + .../ftc/teamcode/test/ThreadTest.java | 78 --- .../ftc/teamcode/test/WheelOffset.java | 1 + .../ftc/teamcode/test/tttest.java | 36 -- 22 files changed, 685 insertions(+), 793 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/PIDController.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{advancedManual => previousCode}/ArmStateMachine.java (95%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/PreviousManual.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{advancedManual => previousCode}/Task.java (92%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{advancedManual => previousCode}/TaskManager.java (97%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{advancedManual => previousCode}/test/ServoMgrTest.java (95%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{advancedManual => previousCode}/test/TimedServoTest.java (81%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{advancedManual => previousCode}/test/TurnServoTest.java (94%) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ThreadTest.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/tttest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java index e14c85e65c93..9ae4cd3acd71 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java @@ -1,7 +1,7 @@ package org.firstinspires.ftc.teamcode; -import com.qualcomm. robotcore.eventloop. opmode.Autonomous; -import com.qualcomm. robotcore. eventloop. opmode. LinearOpMode; +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.Servo; @@ -16,10 +16,10 @@ public void runOpMode() { DcMotor backLeft = hardwareMap.get(DcMotor.class, "BL"); DcMotor frontRight = hardwareMap.get(DcMotor.class, "FR"); DcMotor backRight = hardwareMap.get(DcMotor.class, "BR"); - DcMotor leftLift = hardwareMap.get(DcMotor.class,"leftLift"); - DcMotor rightLift = hardwareMap.get(DcMotor.class,"rightLift"); + DcMotor leftLift = hardwareMap.get(DcMotor.class, "leftLift"); + DcMotor rightLift = hardwareMap.get(DcMotor.class, "rightLift"); DcMotor armStretchMotor = hardwareMap.get(DcMotor.class, "armStretch"); - Servo liftServo = hardwareMap.get(Servo.class,"liftServo"); + Servo liftServo = hardwareMap.get(Servo.class, "liftServo"); leftLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -65,13 +65,14 @@ public void runOpMode() { // armStretchMotor.setPower(0.5); // sleep(1400); // armStretchMotor.setPower(0); - robotTop.setTurnPosition(0.2); + robotTop.setTurnPosition(0.6); sleep(500); //lift1 leftLift.setPower(0.5); rightLift.setPower(0.5); - while(leftLift.getCurrentPosition() < 1000){} + while (leftLift.getCurrentPosition() < 1000) { + } leftLift.setPower(0); rightLift.setPower(0); @@ -101,7 +102,7 @@ public void runOpMode() { // armStretchMotor.setPower(-0.5); // sleep(1400); // armStretchMotor.setPower(0); - robotTop.setTurnPosition(0); + robotTop.setTurnPosition(0.3); sleep(500); //back 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 fce72be16d6b..22e1b5be8c04 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode; -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; @@ -8,11 +7,10 @@ import org.firstinspires.ftc.teamcode.hardware.RobotChassis; import org.firstinspires.ftc.teamcode.hardware.RobotTop; -@Disabled @TeleOp public class ManualOpMode extends LinearOpMode { enum ArmState { - IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED, LOCKING, UNLOCKING } enum LiftState { @@ -24,348 +22,326 @@ enum LiftState { ArmState armState; LiftState liftState; Gamepad previousGamepad1; + Gamepad previousGamepad2; - //TODO: test the constants + //Constants final int STRETCH_BACK_POSITION = 70; final int STRETCH_OUT_POSITION = 1500; - final double SPIN_DEFAULT_POSITION = 0.3; - final double SPIN_HOVERING_POSITION = 1; + final double SPIN_DEFAULT_POSITION = 0.2; + final double SPIN_HOVERING_POSITION = 0.85; final double SPIN_DOWN_POSITION = 0; - final double TURN_BACK_POSITION = 0.5; - final double TURN_HOVERING_POSITION = 0.8; - final double TURN_DOWN_POSITION = 0.85; + final double TURN_BACK_POSITION = 0.3; + final double TURN_HOVERING_POSITION = 0.62; + final double TURN_DOWN_POSITION = 0.7; final double GRAB_OPEN_POSITION = 0.4; final double GRAB_CLOSE_POSITION = 0.92; final double TOP_BACK = 0.03; final double TOP_OUT = 0.66; + // Variables boolean isGrabbing; boolean topServoOut; boolean backGrabOpen; int liftPosition; - double turnPosition; + double targetTurnPosition; + double currentTurnPosition; + boolean spinLeft; + boolean spinRight; @Override public void runOpMode() { previousGamepad1 = new Gamepad(); previousGamepad1.copy(gamepad1); + previousGamepad2 = new Gamepad(); + previousGamepad2.copy(gamepad2); this.robotTop = new RobotTop(this); this.robotChassis = new RobotChassis(this); this.armState = ArmState.IDLE; this.liftState = LiftState.DISABLED; this.isGrabbing = false; this.topServoOut = false; - this.backGrabOpen = false; this.liftPosition = 0; + this.backGrabOpen = false; + this.targetTurnPosition = TURN_BACK_POSITION; + spinLeft = false; + spinRight = false; + waitForStart(); - robotTop.setTurnPosition(TURN_BACK_POSITION); + // robotTop.setTurnPosition(TURN_BACK_POSITION); robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); - while (opModeIsActive()) { - robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); + int armStretchStartPos = robotTop.getArmStretchPosition(); + telemetry.addData("aSTARTpos", armStretchStartPos); + telemetry.update(); - // robotLift - liftPosition = robotTop.getLiftPosition(); - turnPosition = robotTop.getTurnPosition(); + if (isStopRequested()) return; - if(gamepad1.x && !gamepad1.left_bumper){ - robotTop.setStretchPower(0.5); - } else if (gamepad1.x && gamepad1.left_bumper) { - robotTop.setStretchPower(-0.5); - }else{ - robotTop.setStretchPower(0); - } - - if(gamepad1.y && !gamepad1.left_bumper){ - turnPosition += 0.005; - } else if (gamepad1.y && gamepad1.left_bumper) { - turnPosition -= 0.005; - } - robotTop.setTurnPosition(turnPosition); - - if(gamepad1.b && !previousGamepad1.b){ - if(isGrabbing){ - robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); - }else { - robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + while (opModeIsActive()) { + robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); + if (gamepad1.a && !previousGamepad1.a) { + if (backGrabOpen) { + robotTop.setLiftServoPosition(0.1); + } else { + robotTop.setLiftServoPosition(0.6); } - isGrabbing = !isGrabbing; - } - - if(gamepad1.dpad_up){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.005)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.005)); - } - if(gamepad1.dpad_down){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.005)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.005)); + backGrabOpen = !backGrabOpen; } - if(gamepad1.dpad_left){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.005)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.005)); + switch (armState) { + case IDLE: + handleIdleState(); + break; + case TURNING_OUT: + handleTurningOutState(); + break; + case TURNED: + handleTurnedState(); + break; + case TURNING_BACK: + handleTurningBackState(); + break; + case WITHDRAWING: + handleWithdrawingState(); + break; + case LOCKED: + handleLockedState(); + break; + case LOCKING: + handleLockingState(); + break; + case UNLOCKING: + handleUnlockingState(); + break; } - if(gamepad1.dpad_right){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.005)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.005)); + switch (liftState) { + case DISABLED: + handleDisabledState(); + break; + case RUNNING: + handleRunningState(); + break; } + telemetry.addData("arm", armState); + telemetry.addData("lift", liftState); + telemetry.addData("armPos", robotTop.getTurnPosition()); + telemetry.addData("armStretchPosition", robotTop.getArmStretchPosition()); + telemetry.update(); + previousGamepad1.copy(gamepad1); + previousGamepad2.copy(gamepad2); + sleep(10); + } - if(gamepad1.right_trigger != 0){ - robotTop.setLeftPower(0.5); - robotTop.setLiftTargetPos(robotTop.getLiftPosition()); - } else if (gamepad1.left_trigger != 0) { - robotTop.setLeftPower(-0.5); - robotTop.setLiftTargetPos(robotTop.getLiftPosition()); - }else{ - if(robotTop.getLiftPosition() >= 200){ - robotTop.updateLiftPID(); - }else{ - robotTop.setLeftPower(0); - } - } + } - if (gamepad1.right_bumper && !previousGamepad1.right_bumper) { - if (topServoOut) { - robotTop.setTopServoPosition(TOP_BACK); - } else { - robotTop.setTopServoPosition(TOP_OUT); - } - topServoOut = !topServoOut; + protected void handleIdleState() { + if (gamepad1.x && !previousGamepad1.x) { + robotTop.setStretchPower(0.9); + targetTurnPosition = TURN_HOVERING_POSITION; +// robotTop.setTurnPosition(TURN_HOVERING_POSITION); + robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION); + robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION); + armState = ArmState.TURNING_OUT; + } + if (gamepad1.b && !previousGamepad1.b) { + if (isGrabbing) { + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + } else { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); } + } + if (liftState == LiftState.RUNNING) { + liftState = LiftState.DISABLED; + } + if (gamepad1.dpad_up) { + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); + } + if (gamepad1.dpad_down) { + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); + } + if (gamepad1.dpad_left) { + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); + } + if (gamepad1.dpad_right) { + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); + } + } - if (gamepad1.a && !previousGamepad1.a) { - if (backGrabOpen) { - robotTop.setLiftServoPosition(0.1); - } else { - robotTop.setLiftServoPosition(0.6); - } - backGrabOpen = !backGrabOpen; + protected void handleTurningOutState() { + if (robotTop.getArmStretchPosition() >= STRETCH_OUT_POSITION) { + robotTop.setStretchPower(0); + armState = ArmState.TURNED; + } + currentTurnPosition = robotTop.getTurnPosition(); + if (currentTurnPosition > targetTurnPosition + 0.014) { + robotTop.setTurnPosition(currentTurnPosition - 0.014); + } else if (currentTurnPosition < targetTurnPosition - 0.014) { + robotTop.setTurnPosition(currentTurnPosition + 0.014); + } else { + robotTop.setTurnPosition(targetTurnPosition); + } +// if(robotTop.getTurnPosition() >= TURN_DOWN_POSITION){ +// robotTop.setStretchPower(0); +// armState = ArmState.TURNED; +// } +// robotTop.setTurnPosition(robotTop.getTurnPosition() + 0.005); + } + + protected void handleTurnedState() { + if (gamepad1.x && !previousGamepad1.x) { + robotTop.setStretchPower(-0.9); + targetTurnPosition = TURN_BACK_POSITION; + // robotTop.setTurnPosition(TURN_BACK_POSITION); + spinRight = false; + spinLeft = false; + armState = ArmState.WITHDRAWING; + } + if (gamepad1.b && !previousGamepad1.b) { + if (isGrabbing) { + robotTop.setTurnPosition(TURN_DOWN_POSITION); + sleep(500); + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + sleep(500); + robotTop.setTurnPosition(TURN_HOVERING_POSITION); + isGrabbing = !isGrabbing; + } else { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + isGrabbing = !isGrabbing; } + } +// if(gamepad1.dpad_up){ +// robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); +// robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); +// } +// if(gamepad1.dpad_down){ +// robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); +// robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); +// } +// if(gamepad1.dpad_left){ +// robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); +// robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); +// } +// if(gamepad1.dpad_right){ +// robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); +// robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); +// } + if (gamepad1.left_bumper && !previousGamepad1.left_bumper) { + spinLeft = !spinLeft; + spinRight = false; + } + if (gamepad1.right_bumper && !previousGamepad1.right_bumper) { + spinRight = !spinRight; + spinLeft = false; + } + if (spinLeft) { + robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION + 0.15); + robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION - 0.15); + } else if (spinRight) { + robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION - 0.15); + robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION + 0.15); + } else { + robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION); + robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION); + } -// 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; -// } -// } + } + protected void handleTurningBackState() { + //if necessary + } + protected void handleWithdrawingState() { + if (robotTop.getArmStretchPosition() <= STRETCH_BACK_POSITION) { + robotTop.setStretchPower(0); + robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); + robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); + armState = ArmState.IDLE; + } + currentTurnPosition = robotTop.getTurnPosition(); + if (currentTurnPosition > targetTurnPosition + 0.014) { + robotTop.setTurnPosition(currentTurnPosition - 0.014); + } else if (currentTurnPosition < targetTurnPosition - 0.014) { + robotTop.setTurnPosition(currentTurnPosition + 0.014); + } else { + robotTop.setTurnPosition(targetTurnPosition); + } +// if(robotTop.getTurnPosition() <= TURN_BACK_POSITION){ +// robotTop.setStretchPower(0); +// armState = ArmState.IDLE; +// } +// robotTop.setTurnPosition(robotTop.getTurnPosition() - 0.005); + } -// if (liftState == LiftState.BOTTOM && armState == ArmState.IDLE) { -// if (gamepad1.left_trigger != 0) { -// armStretchPos = STRETCH_OUT_POSITION; -// robotTop.setArmStretchPosition(armStretchPos); -// armState = ArmState.STRETCHED; -// sleep(1000); -// } -// if (gamepad1.right_trigger != 0) { -// armStretchPos = STRETCH_OUT_POSITION; -// robotTop.setArmStretchPosition(armStretchPos); -// armState = ArmState.STRETCHED; -// sleep(1000); -// } -// } else { -// if (gamepad1.left_trigger != 0) { -// 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); -// } else { -// 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 (topServoOut) { -// robotTop.setTopServoPosition(0.05); -// } else { -// robotTop.setTopServoPosition(0.6); -// } -// topServoOut = !topServoOut; -// } -// 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) { -// if (armState == ArmState.IDLE) { -// robotTop.setTopServoPosition(0); -// armStretchPos = STRETCH_OUT_POSITION; -// armState = ArmState.TURNING_OUT; -// robotTop.setArmStretchPosition(armStretchPos); -// } else if (armState == ArmState.TURNED) { -// armState = ArmState.TURNING_BACK; -// robotTop.setArmLeftSpinPosition(SPIN_Y_DEFAULT_POSITION); -// } -// } -// if (armState == ArmState.WITHDRAWING) { -// if (armStretchPos <= STRETCH_BACK_POSITION) { -// armState = ArmState.IDLE; -// } else { -// armStretchPos -= 0.03; -// robotTop.setArmStretchPosition(armStretchPos); -// } -// } -// if (armState == ArmState.TURNING_OUT) { -// if (armTurnPos >= TURN_OUT_HOVERING_POSITION - 0.05) { -// armTurnPos = TURN_OUT_HOVERING_POSITION; -// robotTop.setArmLeftTurnPosition(armTurnPos); -// robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); -// armState = ArmState.TURNED; -// } else { -// armTurnPos += 0.03; -// robotTop.setArmLeftTurnPosition(armTurnPos); -// } -// } -// if (armState == ArmState.TURNING_BACK) { -// if (armTurnPos <= TURN_BACK_POSITION + 0.05) { -// armTurnPos = TURN_BACK_POSITION; -// robotTop.setArmLeftTurnPosition(armTurnPos); -// robotTop.setArmRightTurnPosition(SPIN_X_DEFAULT_POSITION); -// armState = ArmState.WITHDRAWING; -// } else { -// armTurnPos -= 0.03; -// robotTop.setArmLeftTurnPosition(armTurnPos); -// } -// } -// if (armState == ArmState.STRETCHED) { -// if (gamepad1.x && !previousGamepad1.x) { -// robotTop.setTopServoPosition(0); -// armState = ArmState.WITHDRAWING; -// } -// } -// if (gamepad1.b && !previousGamepad1.b) { -// robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); -// sleep(500); -// if (!armGrabbing && armState == ArmState.TURNED) { -// robotTop.setArmLeftTurnPosition(TURN_OUT_DOWN_POSITION); -// robotTop.setArmRightTurnPosition(SPIN_X_DOWN_POSITION); -// grabbingFlag = true; -// } else { -// robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); -// armGrabbing = false; -// } -// } -// if (grabbingFlag && !gamepad1.b) { -// robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); -// sleep(500); -// robotTop.setArmLeftTurnPosition(TURN_OUT_HOVERING_POSITION); -// robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); -// armGrabbing = true; -// grabbingFlag = false; -// } -// robotTop.setArmStretchPosition(armStretchPos); -// if (gamepad1.dpad_up) { -// armSpinXPos = Math.min(1, armSpinXPos + 0.02); -// robotTop.setArmRightTurnPosition(armSpinXPos); -// } else if (gamepad1.dpad_down) { -// armSpinXPos = Math.max(0, armSpinXPos - 0.02); -// robotTop.setArmRightTurnPosition(armSpinXPos); -// } -// if (gamepad1.dpad_right) { -// armSpinYPos = Math.min(1, armSpinYPos + 0.05); -// robotTop.setArmLeftSpinPosition(armSpinYPos); -// } else if (gamepad1.dpad_left) { -// armSpinYPos = Math.max(0, armSpinYPos - 0.05); -// robotTop.setArmLeftSpinPosition(armSpinYPos); -// } -// if (gamepad1.a && !previousGamepad1.a) { -// if (backGrabOpen) { -// robotTop.setLiftServoPosition(0.1); -// } else { -// robotTop.setLiftServoPosition(0.6); -// } -// backGrabOpen = !backGrabOpen; -// } -// -// //vision -// if ((gamepad2.left_trigger != 0 && previousGamepad2.left_trigger == 0 -// ) || (gamepad2.right_trigger != 0 && previousGamepad2.right_trigger == 0)) { -// recognitionAngle = robotVisionAngle.getDetectedAngle(); -// robotTop.setArmLeftSpinPosition(calculateSpinY(recognitionAngle)); -// } -// -// // telemetry -// telemetry.addData("liftPos", liftPosition); -// telemetry.addData("state", liftState); -// telemetry.addData("ArmState", armState); -// telemetry.addData("XPos", armSpinXPos); -// telemetry.addData("YPos", armSpinYPos); -// telemetry.addData("angle", recognitionAngle); -// telemetry.update(); - previousGamepad1.copy(gamepad1); -// previousGamepad2.copy(gamepad2); - sleep(10); + protected void handleLockedState() { + if (gamepad1.x && !previousGamepad1.x) { + liftState = LiftState.DISABLED; + armState = ArmState.UNLOCKING; + // robotTop.setTurnPosition(TURN_BACK_POSITION); + targetTurnPosition = TURN_BACK_POSITION; } } - protected double calculateSpinY(double angle) { - if (angle < -180 || angle > 180) { - return 0; + protected void handleLockingState() { + currentTurnPosition = robotTop.getTurnPosition(); + if (currentTurnPosition > targetTurnPosition + 0.014) { + robotTop.setTurnPosition(currentTurnPosition - 0.014); + } else if (currentTurnPosition < targetTurnPosition - 0.014) { + robotTop.setTurnPosition(currentTurnPosition + 0.014); + } else { + robotTop.setTurnPosition(targetTurnPosition); + armState = ArmState.LOCKED; } + } - double adjustedAngle; - if (angle <= 0) { - adjustedAngle = -angle / 180 + 0.1; + protected void handleUnlockingState() { + currentTurnPosition = robotTop.getTurnPosition(); + if (currentTurnPosition > targetTurnPosition + 0.014) { + robotTop.setTurnPosition(currentTurnPosition - 0.014); + } else if (currentTurnPosition < targetTurnPosition - 0.014) { + robotTop.setTurnPosition(currentTurnPosition + 0.014); } else { - adjustedAngle = 0.6 - angle / 180; + robotTop.setTurnPosition(targetTurnPosition); + armState = ArmState.IDLE; } + } - return Math.max(0, Math.min(adjustedAngle, 1)); // 确保值在0到1之间 + protected void handleDisabledState() { + if (armState != ArmState.IDLE || armState != ArmState.LOCKED) { + liftState = LiftState.RUNNING; + } + if (gamepad1.right_trigger != 0 || gamepad1.left_trigger != 0) { + // robotTop.setTurnPosition(TURN_DOWN_POSITION - 0.1); + targetTurnPosition = TURN_DOWN_POSITION - 0.1; + armState = ArmState.LOCKING; + liftState = LiftState.RUNNING; + } + } + + protected void handleRunningState() { + if (gamepad1.right_trigger != 0) { + robotTop.setLeftPower(0.5); + robotTop.setTopServoPosition(TOP_BACK); + robotTop.setLiftTargetPos(robotTop.getLiftPosition()); + } else if (gamepad1.left_trigger != 0) { + robotTop.setLeftPower(-0.5); + robotTop.setTopServoPosition(TOP_BACK); + robotTop.setLiftTargetPos(robotTop.getLiftPosition()); + } else { + if (robotTop.getLiftPosition() >= 200) { + robotTop.updateLiftPID(); + } else { + robotTop.setLeftPower(0); + } + } + if (gamepad2.left_bumper && !previousGamepad2.left_bumper) { + if (topServoOut) { + robotTop.setTopServoPosition(TOP_BACK); + } else { + robotTop.setTopServoPosition(TOP_OUT); + } + topServoOut = !topServoOut; + } } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java index 4d7727d2fcd1..03b3b189bebc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java @@ -64,7 +64,7 @@ public void runOpMode() { // armStretchMotor.setPower(0.5); // sleep(1400); // armStretchMotor.setPower(0); - robotTop.setTurnPosition(0.2); + robotTop.setTurnPosition(0.6); sleep(500); //lift1 @@ -100,7 +100,7 @@ public void runOpMode() { // armStretchMotor.setPower(-0.5); // sleep(1400); // armStretchMotor.setPower(0); - robotTop.setTurnPosition(0); + robotTop.setTurnPosition(0.3); sleep(500); //back 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 deleted file mode 100644 index 1188787a6b4c..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/AdvancedManual.java +++ /dev/null @@ -1,281 +0,0 @@ -package org.firstinspires.ftc.teamcode.advancedManual; - -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; - -// The ManualOp currently used is a piece of shit. -// I'll write a better one here (if the time is enough). -@TeleOp -public class AdvancedManual extends LinearOpMode { - enum ArmState{ - IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED - } - enum LiftState{ - DISABLED, RUNNING - } - RobotTop robotTop; - RobotChassis robotChassis; - ArmState armState; - LiftState liftState; - Gamepad previousGamepad1; - - //TODO: test the constants - final int STRETCH_BACK_POSITION = 70; - final int STRETCH_OUT_POSITION = 1500; - final double SPIN_DEFAULT_POSITION = 0.2 ; - final double SPIN_HOVERING_POSITION = 1; - final double SPIN_DOWN_POSITION = 0; - final double TURN_BACK_POSITION = 0; - final double TURN_HOVERING_POSITION = 0.32; - final double TURN_DOWN_POSITION = 0.4; - final double GRAB_OPEN_POSITION = 0.4; - final double GRAB_CLOSE_POSITION = 0.92; - final double TOP_BACK = 0.03; - final double TOP_OUT = 0.66; - - boolean isGrabbing; - boolean topServoOut; - boolean backGrabOpen; - int liftPosition; - double targetTurnPosition; - double currentTurnPosition; - @Override - public void runOpMode(){ - previousGamepad1 = new Gamepad(); - previousGamepad1.copy(gamepad1); - this.robotTop = new RobotTop(this); - this.robotChassis = new RobotChassis(this); - this.armState = ArmState.IDLE; - this.liftState = LiftState.DISABLED; - this.isGrabbing = false; - this.topServoOut = false; - this.liftPosition = 0; - this.backGrabOpen = false; - this.targetTurnPosition = TURN_BACK_POSITION; - waitForStart(); - // robotTop.setTurnPosition(TURN_BACK_POSITION); - robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); - robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); - int armStretchStartPos = robotTop.getArmStretchPosition(); - telemetry.addData("aSTARTpos", armStretchStartPos); - telemetry.update(); - - if (isStopRequested()) return; - - while (opModeIsActive()) { - robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); - if (gamepad1.a && !previousGamepad1.a) { - if (backGrabOpen) { - robotTop.setLiftServoPosition(0.1); - } else { - robotTop.setLiftServoPosition(0.6); - } - backGrabOpen = !backGrabOpen; - } - switch (armState){ - case IDLE: - handleIdleState(); - break; - case TURNING_OUT: - handleTurningOutState(); - break; - case TURNED: - handleTurnedState(); - break; - case TURNING_BACK: - handleTurningBackState(); - break; - case WITHDRAWING: - handleWithdrawingState(); - break; - case LOCKED: - handleLockedState(); - break; - } - switch (liftState){ - case DISABLED: - handleDisabledState(); - break; - case RUNNING: - handleRunningState(); - break; - } - telemetry.addData("arm",armState); - telemetry.addData("lift",liftState); - telemetry.addData("armPos",robotTop.getTurnPosition()); - telemetry.addData("armStretchPosition", robotTop.getArmStretchPosition()); - telemetry.update(); - previousGamepad1.copy(gamepad1); - sleep(10); - } - - } - protected void handleIdleState(){ - if(gamepad1.x && !previousGamepad1.x){ - robotTop.setStretchPower(0.9); - targetTurnPosition = TURN_HOVERING_POSITION; -// robotTop.setTurnPosition(TURN_HOVERING_POSITION); - robotTop.setArmLeftSpinPosition(SPIN_HOVERING_POSITION); - robotTop.setArmRightSpinPosition(SPIN_HOVERING_POSITION); - armState = ArmState.TURNING_OUT; - } - if(gamepad1.b && !previousGamepad1.b){ - if(isGrabbing){ - robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); - }else { - robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); - } - } - if(liftState == LiftState.RUNNING){ - liftState = LiftState.DISABLED; - } - if(gamepad1.dpad_up){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); - } - if(gamepad1.dpad_down){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); - } - if(gamepad1.dpad_left){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); - } - if(gamepad1.dpad_right){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); - } - } - - protected void handleTurningOutState(){ - if(robotTop.getArmStretchPosition() >= STRETCH_OUT_POSITION){ - robotTop.setStretchPower(0); - armState = ArmState.TURNED; - } - currentTurnPosition = robotTop.getTurnPosition(); - if(currentTurnPosition > targetTurnPosition + 0.014){ - robotTop.setTurnPosition(currentTurnPosition - 0.014); - } else if (currentTurnPosition < targetTurnPosition - 0.014) { - robotTop.setTurnPosition(currentTurnPosition + 0.014); - }else{ - robotTop.setTurnPosition(targetTurnPosition); - } -// if(robotTop.getTurnPosition() >= TURN_DOWN_POSITION){ -// robotTop.setStretchPower(0); -// armState = ArmState.TURNED; -// } -// robotTop.setTurnPosition(robotTop.getTurnPosition() + 0.005); - } - protected void handleTurnedState(){ - if(gamepad1.x && !previousGamepad1.x){ - robotTop.setStretchPower(-0.9); - targetTurnPosition = TURN_BACK_POSITION; - // robotTop.setTurnPosition(TURN_BACK_POSITION); - robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); - robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); - armState = ArmState.WITHDRAWING; - } - if(gamepad1.b && !previousGamepad1.b){ - if(isGrabbing){ - robotTop.setTurnPosition(TURN_DOWN_POSITION); - sleep(500); - robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); - sleep(500); - robotTop.setTurnPosition(TURN_HOVERING_POSITION); - isGrabbing = !isGrabbing; - }else { - robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); - isGrabbing = !isGrabbing; - } - } - if(gamepad1.dpad_up){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); - } - if(gamepad1.dpad_down){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); - } - if(gamepad1.dpad_left){ - robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.008)); - robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.008)); - } - if(gamepad1.dpad_right){ - robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.008)); - robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.008)); - } - } - protected void handleTurningBackState(){ - //if necessary - } - protected void handleWithdrawingState(){ - if(robotTop.getArmStretchPosition() <= STRETCH_BACK_POSITION){ - robotTop.setStretchPower(0); - armState = ArmState.IDLE; - } - currentTurnPosition = robotTop.getTurnPosition(); - if(currentTurnPosition > targetTurnPosition + 0.014){ - robotTop.setTurnPosition(currentTurnPosition - 0.014); - } else if (currentTurnPosition < targetTurnPosition - 0.014) { - robotTop.setTurnPosition(currentTurnPosition + 0.014); - }else{ - robotTop.setTurnPosition(targetTurnPosition); - } -// if(robotTop.getTurnPosition() <= TURN_BACK_POSITION){ -// robotTop.setStretchPower(0); -// armState = ArmState.IDLE; -// } -// robotTop.setTurnPosition(robotTop.getTurnPosition() - 0.005); - } - - protected void handleLockedState(){ - if(gamepad1.x && !previousGamepad1.x){ - liftState = LiftState.DISABLED; - robotTop.setStretchPower(-0.8); - armState = ArmState.WITHDRAWING; - targetTurnPosition = TURN_BACK_POSITION; - } - } - - protected void handleDisabledState(){ - if(armState != ArmState.IDLE || armState != ArmState.LOCKED){ - liftState = LiftState.RUNNING; - } - if(gamepad1.right_trigger != 0 || gamepad1.left_trigger != 0){ - robotTop.setStretchPower(0.8); - sleep(700); - robotTop.setStretchPower(0); - armState = ArmState.LOCKED; - liftState = LiftState.DISABLED; - } - } - protected void handleRunningState(){ - if(gamepad1.right_trigger != 0){ - robotTop.setLeftPower(0.5); - robotTop.setTopServoPosition(TOP_BACK); - robotTop.setLiftTargetPos(robotTop.getLiftPosition()); - } else if (gamepad1.left_trigger != 0) { - robotTop.setLeftPower(-0.5); - robotTop.setTopServoPosition(TOP_BACK); - robotTop.setLiftTargetPos(robotTop.getLiftPosition()); - }else{ - if(robotTop.getLiftPosition() >= 200){ - robotTop.updateLiftPID(); - }else{ - robotTop.setLeftPower(0); - } - } - if (gamepad1.left_bumper && !previousGamepad1.left_bumper) { - if (topServoOut) { - robotTop.setTopServoPosition(TOP_BACK); - } else { - robotTop.setTopServoPosition(TOP_OUT); - } - topServoOut = !topServoOut; - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/PIDController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/PIDController.java deleted file mode 100644 index 02ef1d73d985..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/PIDController.java +++ /dev/null @@ -1,19 +0,0 @@ -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.HardwareMap; - -import org.firstinspires.ftc.robotcore.external.Telemetry; - -public class PIDController { - OpMode opMode; - HardwareMap hardwareMap; - Telemetry telemetry; - public PIDController(LinearOpMode opMode) { - this.opMode = opMode; - hardwareMap = opMode.hardwareMap; - telemetry = opMode.telemetry; - } - -} 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 326ad968d58b..accf69eb99c9 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 @@ -101,11 +101,11 @@ public double getArmRightTurnPosition() { return armRightTurnServo.getPosition(); } - public double getTurnPosition(){ + public double getTurnPosition() { return getArmLeftTurnPosition(); } - public void setTurnPosition(double position){ + public void setTurnPosition(double position) { setArmLeftTurnPosition(position); setArmRightTurnPosition(1 - position); } @@ -158,7 +158,7 @@ public double getContainerServoPosition() { return containerServo.getPosition(); } - public void setTargetLiftPosition(int position){ + public void setTargetLiftPosition(int position) { this.targetLiftPosition = position; } @@ -167,7 +167,7 @@ public void setTargetLiftPosition(int position){ double targetPos = 0; double ticks_per_rev = 530; double delta = 0; - double p = 0, i = 0 , d = 0; + double p = 0, i = 0, d = 0; final double Kp = 0.0008, Ki = 0.00001, Kd = -0.004; final double power_max = 0.5; double power = 0; @@ -180,27 +180,26 @@ public void updateLiftPID() { power = 1; else if (delta < -ticks_per_rev * 0.6) power = -1; - else - { + else { p = Kp * delta; i += Ki * delta; d = Kd * (currentPos - previousPos); power = p + i + d; if (power > power_max) power = power_max; - else if(power < -power_max) + else if (power < -power_max) power = -power_max; } setLeftPower(power); - telemetry.addData("power",power); - telemetry.addData("p",p); - telemetry.addData("i",i); - telemetry.addData("d",d); - telemetry.addData("current",currentPos); - telemetry.addData("target",targetPos); + telemetry.addData("power", power); + telemetry.addData("p", p); + telemetry.addData("i", i); + telemetry.addData("d", d); + telemetry.addData("current", currentPos); + telemetry.addData("target", targetPos); } - public void setLiftTargetPos(int pos){ + public void setLiftTargetPos(int pos) { this.targetPos = Math.min(1260, pos); this.targetPos = Math.max(40, targetPos); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java deleted file mode 100644 index 0f2f59952d1d..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SparkFunOTOSSensor.java +++ /dev/null @@ -1,48 +0,0 @@ -package org.firstinspires.ftc.teamcode.hardware; - -import com.qualcomm.hardware.sparkfun.SparkFunOTOS; -import com.qualcomm.robotcore.hardware.HardwareMap; - -public class SparkFunOTOSSensor { - private SparkFunOTOS otosDevice; - private double x; - private double y; - private double heading; - - public SparkFunOTOSSensor(HardwareMap hardwareMap) { - // 假设 OTOS 传感器连接到 I2C 端口 - otosDevice = hardwareMap.get(SparkFunOTOS.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; - } - - public double getDistance() { - - return 0; - } -} - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/ArmStateMachine.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/ArmStateMachine.java index a709fc3f953f..c2725998e568 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/ArmStateMachine.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/ArmStateMachine.java @@ -1,10 +1,11 @@ -package org.firstinspires.ftc.teamcode.advancedManual; +package org.firstinspires.ftc.teamcode.previousCode; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.hardware.Gamepad; import org.firstinspires.ftc.teamcode.hardware.RobotChassis; import org.firstinspires.ftc.teamcode.hardware.RobotTop; - +@Disabled public class ArmStateMachine { enum ArmState{ IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/PreviousManual.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/PreviousManual.java new file mode 100644 index 000000000000..841c6bde8c57 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/PreviousManual.java @@ -0,0 +1,369 @@ +package org.firstinspires.ftc.teamcode.previousCode; + +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 org.firstinspires.ftc.teamcode.hardware.RobotChassis; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + +@Disabled +@TeleOp +public class PreviousManual extends LinearOpMode { + enum ArmState { + IDLE, WITHDRAWING, TURNING_OUT, TURNED, TURNING_BACK, LOCKED + } + + enum LiftState { + DISABLED, RUNNING + } + + RobotTop robotTop; + RobotChassis robotChassis; + ArmState armState; + LiftState liftState; + Gamepad previousGamepad1; + + final int STRETCH_BACK_POSITION = 70; + final int STRETCH_OUT_POSITION = 1500; + final double SPIN_DEFAULT_POSITION = 0.3; + final double SPIN_HOVERING_POSITION = 1; + final double SPIN_DOWN_POSITION = 0; + final double TURN_BACK_POSITION = 0.5; + final double TURN_HOVERING_POSITION = 0.8; + final double TURN_DOWN_POSITION = 0.85; + final double GRAB_OPEN_POSITION = 0.4; + final double GRAB_CLOSE_POSITION = 0.92; + final double TOP_BACK = 0.03; + final double TOP_OUT = 0.66; + + boolean isGrabbing; + boolean topServoOut; + boolean backGrabOpen; + int liftPosition; + double turnPosition; + + @Override + public void runOpMode() { + previousGamepad1 = new Gamepad(); + previousGamepad1.copy(gamepad1); + this.robotTop = new RobotTop(this); + this.robotChassis = new RobotChassis(this); + this.armState = ArmState.IDLE; + this.liftState = LiftState.DISABLED; + this.isGrabbing = false; + this.topServoOut = false; + this.backGrabOpen = false; + this.liftPosition = 0; + waitForStart(); + robotTop.setTurnPosition(TURN_BACK_POSITION); + robotTop.setArmLeftSpinPosition(SPIN_DEFAULT_POSITION); + robotTop.setArmRightSpinPosition(SPIN_DEFAULT_POSITION); + while (opModeIsActive()) { + robotChassis.driveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); + + // robotLift + liftPosition = robotTop.getLiftPosition(); + turnPosition = robotTop.getTurnPosition(); + + if (gamepad1.x && !gamepad1.left_bumper) { + robotTop.setStretchPower(0.5); + } else if (gamepad1.x && gamepad1.left_bumper) { + robotTop.setStretchPower(-0.5); + } else { + robotTop.setStretchPower(0); + } + + if (gamepad1.y && !gamepad1.left_bumper) { + turnPosition += 0.005; + } else if (gamepad1.y && gamepad1.left_bumper) { + turnPosition -= 0.005; + } + robotTop.setTurnPosition(turnPosition); + + if (gamepad1.b && !previousGamepad1.b) { + if (isGrabbing) { + robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); + } else { + robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); + } + isGrabbing = !isGrabbing; + } + + if (gamepad1.dpad_up) { + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.005)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.005)); + } + if (gamepad1.dpad_down) { + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.005)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.005)); + } + if (gamepad1.dpad_left) { + robotTop.setArmLeftSpinPosition(Math.min(1, robotTop.getArmLeftSpinPosition() + 0.005)); + robotTop.setArmRightSpinPosition(Math.max(0, robotTop.getArmRightSpinPosition() - 0.005)); + } + if (gamepad1.dpad_right) { + robotTop.setArmLeftSpinPosition(Math.max(0, robotTop.getArmLeftSpinPosition() - 0.005)); + robotTop.setArmRightSpinPosition(Math.min(1, robotTop.getArmRightSpinPosition() + 0.005)); + } + + if (gamepad1.right_trigger != 0) { + robotTop.setLeftPower(0.5); + robotTop.setLiftTargetPos(robotTop.getLiftPosition()); + } else if (gamepad1.left_trigger != 0) { + robotTop.setLeftPower(-0.5); + robotTop.setLiftTargetPos(robotTop.getLiftPosition()); + } else { + if (robotTop.getLiftPosition() >= 200) { + robotTop.updateLiftPID(); + } else { + robotTop.setLeftPower(0); + } + } + + if (gamepad1.right_bumper && !previousGamepad1.right_bumper) { + if (topServoOut) { + robotTop.setTopServoPosition(TOP_BACK); + } else { + robotTop.setTopServoPosition(TOP_OUT); + } + topServoOut = !topServoOut; + } + + if (gamepad1.a && !previousGamepad1.a) { + if (backGrabOpen) { + robotTop.setLiftServoPosition(0.1); + } else { + robotTop.setLiftServoPosition(0.6); + } + backGrabOpen = !backGrabOpen; + } + +// 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(1000); +// } +// if (gamepad1.right_trigger != 0) { +// armStretchPos = STRETCH_OUT_POSITION; +// robotTop.setArmStretchPosition(armStretchPos); +// armState = ArmState.STRETCHED; +// sleep(1000); +// } +// } else { +// if (gamepad1.left_trigger != 0) { +// 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); +// } else { +// 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 (topServoOut) { +// robotTop.setTopServoPosition(0.05); +// } else { +// robotTop.setTopServoPosition(0.6); +// } +// topServoOut = !topServoOut; +// } +// 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) { +// if (armState == ArmState.IDLE) { +// robotTop.setTopServoPosition(0); +// armStretchPos = STRETCH_OUT_POSITION; +// armState = ArmState.TURNING_OUT; +// robotTop.setArmStretchPosition(armStretchPos); +// } else if (armState == ArmState.TURNED) { +// armState = ArmState.TURNING_BACK; +// robotTop.setArmLeftSpinPosition(SPIN_Y_DEFAULT_POSITION); +// } +// } +// if (armState == ArmState.WITHDRAWING) { +// if (armStretchPos <= STRETCH_BACK_POSITION) { +// armState = ArmState.IDLE; +// } else { +// armStretchPos -= 0.03; +// robotTop.setArmStretchPosition(armStretchPos); +// } +// } +// if (armState == ArmState.TURNING_OUT) { +// if (armTurnPos >= TURN_OUT_HOVERING_POSITION - 0.05) { +// armTurnPos = TURN_OUT_HOVERING_POSITION; +// robotTop.setArmLeftTurnPosition(armTurnPos); +// robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); +// armState = ArmState.TURNED; +// } else { +// armTurnPos += 0.03; +// robotTop.setArmLeftTurnPosition(armTurnPos); +// } +// } +// if (armState == ArmState.TURNING_BACK) { +// if (armTurnPos <= TURN_BACK_POSITION + 0.05) { +// armTurnPos = TURN_BACK_POSITION; +// robotTop.setArmLeftTurnPosition(armTurnPos); +// robotTop.setArmRightTurnPosition(SPIN_X_DEFAULT_POSITION); +// armState = ArmState.WITHDRAWING; +// } else { +// armTurnPos -= 0.03; +// robotTop.setArmLeftTurnPosition(armTurnPos); +// } +// } +// if (armState == ArmState.STRETCHED) { +// if (gamepad1.x && !previousGamepad1.x) { +// robotTop.setTopServoPosition(0); +// armState = ArmState.WITHDRAWING; +// } +// } +// if (gamepad1.b && !previousGamepad1.b) { +// robotTop.setArmGrabPosition(GRAB_OPEN_POSITION); +// sleep(500); +// if (!armGrabbing && armState == ArmState.TURNED) { +// robotTop.setArmLeftTurnPosition(TURN_OUT_DOWN_POSITION); +// robotTop.setArmRightTurnPosition(SPIN_X_DOWN_POSITION); +// grabbingFlag = true; +// } else { +// robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); +// armGrabbing = false; +// } +// } +// if (grabbingFlag && !gamepad1.b) { +// robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); +// sleep(500); +// robotTop.setArmLeftTurnPosition(TURN_OUT_HOVERING_POSITION); +// robotTop.setArmRightTurnPosition(SPIN_X_HOVERING_POSITION); +// armGrabbing = true; +// grabbingFlag = false; +// } +// robotTop.setArmStretchPosition(armStretchPos); +// if (gamepad1.dpad_up) { +// armSpinXPos = Math.min(1, armSpinXPos + 0.02); +// robotTop.setArmRightTurnPosition(armSpinXPos); +// } else if (gamepad1.dpad_down) { +// armSpinXPos = Math.max(0, armSpinXPos - 0.02); +// robotTop.setArmRightTurnPosition(armSpinXPos); +// } +// if (gamepad1.dpad_right) { +// armSpinYPos = Math.min(1, armSpinYPos + 0.05); +// robotTop.setArmLeftSpinPosition(armSpinYPos); +// } else if (gamepad1.dpad_left) { +// armSpinYPos = Math.max(0, armSpinYPos - 0.05); +// robotTop.setArmLeftSpinPosition(armSpinYPos); +// } +// if (gamepad1.a && !previousGamepad1.a) { +// if (backGrabOpen) { +// robotTop.setLiftServoPosition(0.1); +// } else { +// robotTop.setLiftServoPosition(0.6); +// } +// backGrabOpen = !backGrabOpen; +// } +// +// //vision +// if ((gamepad2.left_trigger != 0 && previousGamepad2.left_trigger == 0 +// ) || (gamepad2.right_trigger != 0 && previousGamepad2.right_trigger == 0)) { +// recognitionAngle = robotVisionAngle.getDetectedAngle(); +// robotTop.setArmLeftSpinPosition(calculateSpinY(recognitionAngle)); +// } +// +// // telemetry +// telemetry.addData("liftPos", liftPosition); +// telemetry.addData("state", liftState); +// telemetry.addData("ArmState", armState); +// telemetry.addData("XPos", armSpinXPos); +// telemetry.addData("YPos", armSpinYPos); +// telemetry.addData("angle", recognitionAngle); +// telemetry.update(); + previousGamepad1.copy(gamepad1); +// previousGamepad2.copy(gamepad2); + sleep(10); + } + } + + protected double calculateSpinY(double angle) { + if (angle < -180 || angle > 180) { + return 0; + } + + double adjustedAngle; + if (angle <= 0) { + adjustedAngle = -angle / 180 + 0.1; + } else { + adjustedAngle = 0.6 - angle / 180; + } + + return Math.max(0, Math.min(adjustedAngle, 1)); // 确保值在0到1之间 + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/Task.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/Task.java similarity index 92% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/Task.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/Task.java index f72d16f4cf59..21b6f668c748 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/Task.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/Task.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.advancedManual; +package org.firstinspires.ftc.teamcode.previousCode; 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/previousCode/TaskManager.java similarity index 97% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TaskManager.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/TaskManager.java index 30566ecd3e6d..05db0f3ba7ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/TaskManager.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/TaskManager.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.advancedManual; +package org.firstinspires.ftc.teamcode.previousCode; import com.qualcomm.robotcore.hardware.Servo; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/ServoMgrTest.java similarity index 95% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/ServoMgrTest.java index a66ddc3191e9..3a71cf992687 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/ServoMgrTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/ServoMgrTest.java @@ -1,8 +1,8 @@ -package org.firstinspires.ftc.teamcode.advancedManual.test; +package org.firstinspires.ftc.teamcode.previousCode.test; import android.util.ArrayMap; -import org.firstinspires.ftc.teamcode.advancedManual.Task; +import org.firstinspires.ftc.teamcode.previousCode.Task; import java.util.List; import java.util.Map; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TimedServoTest.java similarity index 81% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TimedServoTest.java index 24d386da3c73..1341ed608f6a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TimedServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TimedServoTest.java @@ -1,14 +1,12 @@ -package org.firstinspires.ftc.teamcode.advancedManual.test; +package org.firstinspires.ftc.teamcode.previousCode.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.TaskManager; +import org.firstinspires.ftc.teamcode.previousCode.TaskManager; @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 { @Override public void runOpMode(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TurnServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TurnServoTest.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TurnServoTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TurnServoTest.java index 640c3c032dab..2e8d6173a516 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/advancedManual/test/TurnServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TurnServoTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.advancedManual.test; +package org.firstinspires.ftc.teamcode.previousCode.test; 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/test/Armtest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java index 78f5d679251d..e98c11b2e583 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Armtest.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.Gamepad; import com.qualcomm.robotcore.hardware.Servo; +@Disabled @TeleOp(group = "Test") public class Armtest extends LinearOpMode { @Override 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 b8546baddfa2..33a474836c54 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 @@ -1,11 +1,13 @@ 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; import com.qualcomm. robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; +@Disabled @Autonomous public class AutoTest extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java index 6ce8e65803ab..5b8ec213d905 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.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 org.firstinspires.ftc.teamcode.hardware.RobotTop; +@Disabled @TeleOp public class PIDControllerTest extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java index 30b02d13981e..299c7287d9ae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java @@ -1,5 +1,6 @@ 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; @@ -7,6 +8,7 @@ import org.firstinspires.ftc.teamcode.hardware.RobotTop; +@Disabled @TeleOp public class PIDControllerTest2 extends LinearOpMode { @Override 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 08edee753ad6..9dd389940c3a 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 @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.Servo; +@Disabled @TeleOp(group = "Test") public class ServoTest extends LinearOpMode { @Override 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 deleted file mode 100644 index 60e5c974b123..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/ThreadTest.java +++ /dev/null @@ -1,78 +0,0 @@ -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 index 7b4a7b2e71b5..27d0a2205498 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,6 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +@Disabled @TeleOp(group = "Test") public class WheelOffset extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/tttest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/tttest.java deleted file mode 100644 index 5bd7c0d3edfe..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/tttest.java +++ /dev/null @@ -1,36 +0,0 @@ -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.Gamepad; -import com.qualcomm.robotcore.hardware.Servo; - -@TeleOp(group = "Test") -public class tttest extends LinearOpMode { - @Override - public void runOpMode(){ - DcMotor m = hardwareMap.get(DcMotor.class, "m"); - - waitForStart(); - Gamepad previousGamepad1 = new Gamepad(); - previousGamepad1.copy(gamepad1); - - if (isStopRequested()) return; - m.setMode(DcMotor.RunMode.RUN_TO_POSITION); - - while (opModeIsActive()) { - if(gamepad1.y){ - m.setTargetPosition(0); - } - if(gamepad1.a){ - m.setTargetPosition(300); - } - - // telemetry.addData("lp", leftPos); - telemetry.update(); - - sleep(10); - } - } -} From d5d94373bf86fd19ba8c1b2f4545ee897fc1f84c Mon Sep 17 00:00:00 2001 From: Ehicy <102399008+Ehicy@users.noreply.github.com> Date: Sat, 18 Jan 2025 18:27:18 +0800 Subject: [PATCH 131/143] Create AutoEasy Temporary commit to save a file AND let me a update on the idea --- .../ftc/teamcode/hardware/AutoEasy | 72 +++++++++++++++++++ 1 file changed, 72 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy new file mode 100644 index 000000000000..13f37c54c914 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy @@ -0,0 +1,72 @@ +package org.firstinspires.ftc.teamcode.hardware; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +public class AutoEasy { + LinearOpMode opMode; + DcMotor frontLeft; + DcMotor backLeft; + DcMotor frontRight; + DcMotor backRight; + + public AutoEasy(LinearOpMode opMode) { + this.opMode = opMode; + this.init(); + } + + private void init() { + frontLeft = opMode.hardwareMap.get(DcMotor.class, "FL"); + backLeft = opMode.hardwareMap.get(DcMotor.class, "BL"); + frontRight = opMode.hardwareMap.get(DcMotor.class, "FR"); + backRight = opMode.hardwareMap.get(DcMotor.class, "BR"); + + frontRight.setDirection(DcMotor.Direction.REVERSE); + setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + setRunMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + public void forward(double distance) { + double power = 0.7; + long sleepTime = (long) (distance * 1000); // Example conversion factor + + setDrivePower(power, power, power, power); + opMode.sleep(sleepTime); + stopMotors(); + } + + public void spin(double degree) { + double power = 0.7; + long sleepTime = (long) (degree * 10); // Example conversion factor + + setDrivePower(power, -power, power, -power); + opMode.sleep(sleepTime); + stopMotors(); + } + + private void setDrivePower(double leftFront, double leftBack, double rightFront, double rightBack) { + frontLeft.setPower(leftFront); + backLeft.setPower(leftBack); + frontRight.setPower(rightFront); + backRight.setPower(rightBack); + } + + private void stopMotors() { + setDrivePower(0, 0, 0, 0); + } + + private void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior behavior) { + frontLeft.setZeroPowerBehavior(behavior); + backLeft.setZeroPowerBehavior(behavior); + frontRight.setZeroPowerBehavior(behavior); + backRight.setZeroPowerBehavior(behavior); + } + + private void setRunMode(DcMotor.RunMode mode) { + frontLeft.setMode(mode); + backLeft.setMode(mode); + frontRight.setMode(mode); + backRight.setMode(mode); + } +} From bc2678fb785b293cc152bcdcd8cdc169bb406ee8 Mon Sep 17 00:00:00 2001 From: Ehicy <102399008+Ehicy@users.noreply.github.com> Date: Sat, 18 Jan 2025 20:34:40 +0800 Subject: [PATCH 132/143] Rename AutoEasy to AutoEasy.java --- .../ftc/teamcode/hardware/{AutoEasy => AutoEasy.java} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/{AutoEasy => AutoEasy.java} (100%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy.java similarity index 100% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy.java From e4b14fc541aedd10f302290b4e93b159cce3b113 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sat, 18 Jan 2025 20:28:51 +0800 Subject: [PATCH 133/143] Feat:Fixed Autonomous. --- .../firstinspires/ftc/teamcode/LeftAuto.java | 8 +-- .../firstinspires/ftc/teamcode/RightAuto.java | 70 +++++++++---------- 2 files changed, 37 insertions(+), 41 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java index b1e530c09c17..aead338f4b4c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LeftAuto.java @@ -112,10 +112,10 @@ public void runOpMode() { sleep(700); //left rafe - frontLeft.setPower(-0.7); - backLeft.setPower(0.7); - frontRight.setPower(0.7); - backRight.setPower(-0.7); + frontLeft.setPower(0.7); + backLeft.setPower(-0.7); + frontRight.setPower(-0.7); + backRight.setPower(0.7); sleep(2500); // //back1 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java index 6211c73a5ede..599ba2cfc2be 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java @@ -79,7 +79,7 @@ public void runOpMode() { backLeft.setPower(0.5); frontRight.setPower(0.5); backRight.setPower(0.5); - sleep(100); + sleep(250); frontLeft.setPower(0); backLeft.setPower(0); frontRight.setPower(0); @@ -100,58 +100,54 @@ public void runOpMode() { // armStretchMotor.setPower(-0.5); // sleep(1400); // armStretchMotor.setPower(0); - robotTop.setTurnPosition(0.3); + robotTop.setTurnPosition(0.2); sleep(500); - //back + //back1 frontLeft.setPower(-0.7); backLeft.setPower(-0.7); frontRight.setPower(-0.7); backRight.setPower(-0.7); - sleep(700); + sleep(500); //right rafe frontLeft.setPower(0.7); backLeft.setPower(-0.7); frontRight.setPower(-0.7); backRight.setPower(0.7); - sleep(2500); + sleep(1500); -// //back1 -// frontLeft.setPower(-0.7); -// backLeft.setPower(-0.7); -// frontRight.setPower(-0.7); -// backRight.setPower(-0.7); -// sleep(500); -// -// //right rafe + //spin + frontLeft.setPower(-0.7); + backLeft.setPower(-0.7); + frontRight.setPower(0.7); + backRight.setPower(0.7); + sleep(1320); + frontLeft.setPower(0); + backLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + + //back2 + frontLeft.setPower(0.7); + backLeft.setPower(0.7); + frontRight.setPower(0.7); + backRight.setPower(0.7); + sleep(100); + frontLeft.setPower(0); + backLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + + //grab + liftServo.setPosition(0.95); + sleep(700); + + //right rafe // frontLeft.setPower(0.7); // backLeft.setPower(-0.7); // frontRight.setPower(-0.7); // backRight.setPower(0.7); -// sleep(1500); -// -// //spin -// frontLeft.setPower(-0.7); -// backLeft.setPower(-0.7); -// frontRight.setPower(0.7); -// backRight.setPower(0.7); -// sleep(1320); -// frontLeft.setPower(0); -// backLeft.setPower(0); -// frontRight.setPower(0); -// backRight.setPower(0); -// -// //back2 -// frontLeft.setPower(0.7); -// backLeft.setPower(0.7); -// frontRight.setPower(0.7); -// backRight.setPower(0.7); -// sleep(100); -// -// //grab -// liftServo.setPosition(0.95); -// sleep(700); -// +// sleep(2500); fd } } \ No newline at end of file From 73cdc548998b565e624f53329c9b8fd77530d788 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sat, 18 Jan 2025 21:31:13 +0800 Subject: [PATCH 134/143] Feat:Advanced Autonomous. --- .../java/org/firstinspires/ftc/teamcode/RightAuto.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java index 599ba2cfc2be..3e85d4e3b81a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java @@ -129,11 +129,11 @@ public void runOpMode() { backRight.setPower(0); //back2 - frontLeft.setPower(0.7); - backLeft.setPower(0.7); - frontRight.setPower(0.7); - backRight.setPower(0.7); - sleep(100); + frontLeft.setPower(0.5); + backLeft.setPower(0.5); + frontRight.setPower(0.5); + backRight.setPower(0.5); + sleep(200); frontLeft.setPower(0); backLeft.setPower(0); frontRight.setPower(0); From 53bfe5e1b5874bd19febb93783bd37f2676b6f6c Mon Sep 17 00:00:00 2001 From: Ehicy <102399008+Ehicy@users.noreply.github.com> Date: Sun, 19 Jan 2025 08:40:42 +0800 Subject: [PATCH 135/143] give li a convenient way to write the auto give li a convenient way to write the auto --- .../{AutoEasy.java => RobotAutoEasy.java} | 29 ++++++++++++++----- 1 file changed, 22 insertions(+), 7 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/{AutoEasy.java => RobotAutoEasy.java} (70%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAutoEasy.java similarity index 70% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAutoEasy.java index 13f37c54c914..f7b412d81cdb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotAutoEasy.java @@ -2,16 +2,19 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Servo; -public class AutoEasy { +public class RobotAutoEasy { LinearOpMode opMode; DcMotor frontLeft; DcMotor backLeft; DcMotor frontRight; DcMotor backRight; - public AutoEasy(LinearOpMode opMode) { + final static double ForwardRatio = 600; + final static double SpinRatio = 50; + final static double ShiftRatio = 50; + + public RobotAutoEasy(LinearOpMode opMode) { this.opMode = opMode; this.init(); } @@ -27,22 +30,33 @@ private void init() { setRunMode(DcMotor.RunMode.RUN_USING_ENCODER); } - public void forward(double distance) { + public RobotAutoEasy forward(double distance) { double power = 0.7; - long sleepTime = (long) (distance * 1000); // Example conversion factor + long sleepTime = (long) (distance * ForwardRatio); // Example conversion factor setDrivePower(power, power, power, power); opMode.sleep(sleepTime); stopMotors(); + return this; } - public void spin(double degree) { + public RobotAutoEasy spin(double degree) { double power = 0.7; - long sleepTime = (long) (degree * 10); // Example conversion factor + long sleepTime = (long) (degree * SpinRatio); // Example conversion factor + setDrivePower(power, power, -power, -power); + opMode.sleep(sleepTime); + stopMotors(); + return this; + } + + public RobotAutoEasy shift(double distance) { + double power = 0.7; + long sleepTime = (long) (distance * ShiftRatio); // Example conversion factor setDrivePower(power, -power, power, -power); opMode.sleep(sleepTime); stopMotors(); + return this; } private void setDrivePower(double leftFront, double leftBack, double rightFront, double rightBack) { @@ -70,3 +84,4 @@ private void setRunMode(DcMotor.RunMode mode) { backRight.setMode(mode); } } + From e2449e66754c15566421778ff0892188e7ca1ae5 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sun, 19 Jan 2025 14:22:04 +0800 Subject: [PATCH 136/143] Feat:Advanced Autonomous. --- .../ftc/teamcode/ConceptTelemetry.java | 177 ++++++++++++++++++ .../firstinspires/ftc/teamcode/RightAuto.java | 30 ++- .../ftc/teamcode/test/Voltagesensor.java | 50 +++++ 3 files changed, 239 insertions(+), 18 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTelemetry.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Voltagesensor.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTelemetry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTelemetry.java new file mode 100644 index 000000000000..54cb49fbf7ad --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTelemetry.java @@ -0,0 +1,177 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode; + +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.VoltageSensor; +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.Func; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +/* + * This OpMode illustrates various ways in which telemetry can be + * transmitted from the robot controller to the driver station. The sample illustrates + * numeric and text data, formatted output, and optimized evaluation of expensive-to-acquire + * information. The telemetry log is illustrated by scrolling a poem + * to the driver station. + * + * Also see the Telemetry javadocs. + */ +@TeleOp(name = "Concept: Telemetry", group = "Concept") +@Disabled +public class ConceptTelemetry extends LinearOpMode { + /** Keeps track of the line of the poem which is to be emitted next */ + int poemLine = 0; + + /** Keeps track of how long it's been since we last emitted a line of poetry */ + ElapsedTime poemElapsed = new ElapsedTime(); + + static final String[] poem = new String[] { + + "Mary had a little lamb,", + "His fleece was white as snow,", + "And everywhere that Mary went,", + "The lamb was sure to go.", + "", + "He followed her to school one day,", + "Which was against the rule,", + "It made the children laugh and play", + "To see a lamb at school.", + "", + "And so the teacher turned it out,", + "But still it lingered near,", + "And waited patiently about,", + "Till Mary did appear.", + "", + "\"Why does the lamb love Mary so?\"", + "The eager children cry.", + "\"Why, Mary loves the lamb, you know,\"", + "The teacher did reply.", + "", + "" + }; + + @Override public void runOpMode() { + + /* we keep track of how long it's been since the OpMode was started, just + * to have some interesting data to show */ + ElapsedTime opmodeRunTime = new ElapsedTime(); + + // We show the log in oldest-to-newest order, as that's better for poetry + telemetry.log().setDisplayOrder(Telemetry.Log.DisplayOrder.OLDEST_FIRST); + // We can control the number of lines shown in the log + telemetry.log().setCapacity(6); + // The interval between lines of poetry, in seconds + double sPoemInterval = 0.6; + + /* + * Wait until we've been given the ok to go. For something to do, we emit the + * elapsed time as we sit here and wait. If we didn't want to do anything while + * we waited, we would just call waitForStart(). + */ + while (!isStarted()) { + telemetry.addData("time", "%.1f seconds", opmodeRunTime.seconds()); + telemetry.update(); + idle(); + } + + // Ok, we've been given the ok to go + + /* + * As an illustration, the first line on our telemetry display will display the battery voltage. + * The idea here is that it's expensive to compute the voltage (at least for purposes of illustration) + * so you don't want to do it unless the data is _actually_ going to make it to the + * driver station (recall that telemetry transmission is throttled to reduce bandwidth use. + * Note that getBatteryVoltage() below returns 'Infinity' if there's no voltage sensor attached. + * + * @see Telemetry#getMsTransmissionInterval() + */ + telemetry.addData("voltage", "%.1f volts", new Func() { + @Override public Double value() { + return getBatteryVoltage(); + } + }); + + // Reset to keep some timing stats for the post-'start' part of the OpMode + opmodeRunTime.reset(); + int loopCount = 1; + + // Go go gadget robot! + while (opModeIsActive()) { + + // Emit poetry if it's been a while + if (poemElapsed.seconds() > sPoemInterval) { + emitPoemLine(); + } + + // As an illustration, show some loop timing information + telemetry.addData("loop count", loopCount); + telemetry.addData("ms/loop", "%.3f ms", opmodeRunTime.milliseconds() / loopCount); + + // Show joystick information as some other illustrative data + telemetry.addLine("left joystick | ") + .addData("x", gamepad1.left_stick_x) + .addData("y", gamepad1.left_stick_y); + telemetry.addLine("right joystick | ") + .addData("x", gamepad1.right_stick_x) + .addData("y", gamepad1.right_stick_y); + + /* + * Transmit the telemetry to the driver station, subject to throttling. + * See the documentation for Telemetry.getMsTransmissionInterval() for more information. + */ + telemetry.update(); + + // Update loop info + loopCount++; + } + } + + // emits a line of poetry to the telemetry log + void emitPoemLine() { + telemetry.log().add(poem[poemLine]); + poemLine = (poemLine+1) % poem.length; + poemElapsed.reset(); + } + + // Computes the current battery voltage + double getBatteryVoltage() { + double result = Double.POSITIVE_INFINITY; + for (VoltageSensor sensor : hardwareMap.voltageSensor) { + double voltage = sensor.getVoltage(); + if (voltage > 0) { + result = Math.min(result, voltage); + } + } + return result; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java index 3e85d4e3b81a..7c6632da2f86 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RightAuto.java @@ -39,22 +39,21 @@ public void runOpMode() { waitForStart(); //tighten - liftServo.setPosition(0.95); - sleep(700); + liftServo.setPosition(0.6); //left rafe frontLeft.setPower(-0.7); backLeft.setPower(0.7); frontRight.setPower(0.7); backRight.setPower(-0.7); - sleep(700); + sleep(500); //forward frontLeft.setPower(0.7); backLeft.setPower(0.7); frontRight.setPower(0.7); backRight.setPower(0.7); - sleep(600); + sleep(500); frontLeft.setPower(0); backLeft.setPower(0); frontRight.setPower(0); @@ -71,6 +70,9 @@ public void runOpMode() { leftLift.setPower(0.5); rightLift.setPower(0.5); while(leftLift.getCurrentPosition() < 1000){} + leftLift.setPower(0.5); + rightLift.setPower(0.5); + sleep(200); leftLift.setPower(0); rightLift.setPower(0); @@ -79,7 +81,7 @@ public void runOpMode() { backLeft.setPower(0.5); frontRight.setPower(0.5); backRight.setPower(0.5); - sleep(250); + sleep(300); frontLeft.setPower(0); backLeft.setPower(0); frontRight.setPower(0); @@ -92,10 +94,6 @@ public void runOpMode() { leftLift.setPower(0); rightLift.setPower(0); - //Release specimen - liftServo.setPosition(0.6); - sleep(500); - //drawback arm // armStretchMotor.setPower(-0.5); // sleep(1400); @@ -133,21 +131,17 @@ public void runOpMode() { backLeft.setPower(0.5); frontRight.setPower(0.5); backRight.setPower(0.5); - sleep(200); + sleep(700); frontLeft.setPower(0); backLeft.setPower(0); frontRight.setPower(0); backRight.setPower(0); //grab - liftServo.setPosition(0.95); - sleep(700); + liftServo.setPosition(0.6); + sleep(100); + liftServo.setPosition(0.6); + - //right rafe -// frontLeft.setPower(0.7); -// backLeft.setPower(-0.7); -// frontRight.setPower(-0.7); -// backRight.setPower(0.7); -// sleep(2500); fd } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Voltagesensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Voltagesensor.java new file mode 100644 index 000000000000..db58783632ef --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/Voltagesensor.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode.test; + +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; + +@Autonomous +public class Voltagesensor extends LinearOpMode { + + private VoltageSensor voltageSensor; + + @Override + public void runOpMode() { + // 初始化硬件 + DcMotor FLmotor = hardwareMap.get(DcMotor.class, "FL"); + DcMotor FRmotor = hardwareMap.get(DcMotor.class, "FR"); + DcMotor BLmotor = hardwareMap.get(DcMotor.class, "BL"); + DcMotor BRmotor = hardwareMap.get(DcMotor.class, "BR"); + FRmotor.setDirection(DcMotor.Direction.REVERSE); + voltageSensor = hardwareMap.voltageSensor.iterator().next(); + + // 等待用户按下 "Start" 按钮 + waitForStart(); + + // 主控制逻辑 + while (opModeIsActive()) { + double targetVoltage = 14.0; // 理想电压 + double currentVoltage = voltageSensor.getVoltage(); + double compensationFactor = targetVoltage / currentVoltage; + + double rawPower = 0.5; // 原始设定功率 + double adjustedPower = rawPower * compensationFactor; + + // 确保功率范围在 [-1, 1] + adjustedPower = Math.max(-1, Math.min(1, adjustedPower)); + + FLmotor.setPower(adjustedPower); + FRmotor.setPower(adjustedPower); + BLmotor.setPower(adjustedPower); + BRmotor.setPower(adjustedPower); + + // 输出回显信息 + + telemetry.addData("Battery Voltage1", currentVoltage); + telemetry.addData("Adjusted Motor Power", adjustedPower); + telemetry.update(); // 更新 Telemetry + } + } +} From caec3777d113b624a74e5d9fde621f2d98128057 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sun, 19 Jan 2025 14:44:20 +0800 Subject: [PATCH 137/143] Auto basic methods complete --- .../ftc/teamcode/ManualOpMode.java | 7 +- .../ftc/teamcode/hardware/AutoEasy | 72 ------------------- .../ftc/teamcode/hardware/RobotAuto.java | 69 ++++++++++++------ .../ftc/teamcode/hardware/RobotChassis.java | 31 +++++++- .../RobotVision/RobotVisionColor.java | 1 - .../ftc/teamcode/test/AutoTest.java | 2 +- .../ftc/teamcode/test/AutoTest2.java | 38 ++++++++++ .../ftc/teamcode/test/MotorTest.java | 32 +++++++++ .../ftc/teamcode/test/PIDControllerTest.java | 2 +- .../ftc/teamcode/test/PIDControllerTest2.java | 2 +- .../test/TurnServoTest.java | 6 +- .../teamcode/test/VisionCameraAngleTest.java | 2 + .../teamcode/test/VisionCameraColorTest.java | 1 + .../test/VisionCameraDistanceTest.java | 1 + 14 files changed, 162 insertions(+), 104 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest2.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MotorTest.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{previousCode => }/test/TurnServoTest.java (88%) 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 22e1b5be8c04..fefcf12eb6f9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -120,6 +120,10 @@ public void runOpMode() { handleRunningState(); break; } + telemetry.addData("encoder", robotChassis.getCurrentPosition()[0]); + telemetry.addData("encoder", robotChassis.getCurrentPosition()[1]); + telemetry.addData("encoder", robotChassis.getCurrentPosition()[2]); + telemetry.addData("encoder", robotChassis.getCurrentPosition()[3]); telemetry.addData("arm", armState); telemetry.addData("lift", liftState); telemetry.addData("armPos", robotTop.getTurnPosition()); @@ -201,9 +205,8 @@ protected void handleTurnedState() { if (gamepad1.b && !previousGamepad1.b) { if (isGrabbing) { robotTop.setTurnPosition(TURN_DOWN_POSITION); - sleep(500); robotTop.setArmGrabPosition(GRAB_CLOSE_POSITION); - sleep(500); + sleep(300); robotTop.setTurnPosition(TURN_HOVERING_POSITION); isGrabbing = !isGrabbing; } else { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy deleted file mode 100644 index 13f37c54c914..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/AutoEasy +++ /dev/null @@ -1,72 +0,0 @@ -package org.firstinspires.ftc.teamcode.hardware; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Servo; - -public class AutoEasy { - LinearOpMode opMode; - DcMotor frontLeft; - DcMotor backLeft; - DcMotor frontRight; - DcMotor backRight; - - public AutoEasy(LinearOpMode opMode) { - this.opMode = opMode; - this.init(); - } - - private void init() { - frontLeft = opMode.hardwareMap.get(DcMotor.class, "FL"); - backLeft = opMode.hardwareMap.get(DcMotor.class, "BL"); - frontRight = opMode.hardwareMap.get(DcMotor.class, "FR"); - backRight = opMode.hardwareMap.get(DcMotor.class, "BR"); - - frontRight.setDirection(DcMotor.Direction.REVERSE); - setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - setRunMode(DcMotor.RunMode.RUN_USING_ENCODER); - } - - public void forward(double distance) { - double power = 0.7; - long sleepTime = (long) (distance * 1000); // Example conversion factor - - setDrivePower(power, power, power, power); - opMode.sleep(sleepTime); - stopMotors(); - } - - public void spin(double degree) { - double power = 0.7; - long sleepTime = (long) (degree * 10); // Example conversion factor - - setDrivePower(power, -power, power, -power); - opMode.sleep(sleepTime); - stopMotors(); - } - - private void setDrivePower(double leftFront, double leftBack, double rightFront, double rightBack) { - frontLeft.setPower(leftFront); - backLeft.setPower(leftBack); - frontRight.setPower(rightFront); - backRight.setPower(rightBack); - } - - private void stopMotors() { - setDrivePower(0, 0, 0, 0); - } - - private void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior behavior) { - frontLeft.setZeroPowerBehavior(behavior); - backLeft.setZeroPowerBehavior(behavior); - frontRight.setZeroPowerBehavior(behavior); - backRight.setZeroPowerBehavior(behavior); - } - - private void setRunMode(DcMotor.RunMode mode) { - frontLeft.setMode(mode); - backLeft.setMode(mode); - frontRight.setMode(mode); - backRight.setMode(mode); - } -} 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 a5225541836f..8df46d12fc8b 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 @@ -12,6 +12,7 @@ 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; @SuppressWarnings(value = "unused") @@ -33,6 +34,7 @@ public class RobotAuto { 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; @@ -44,20 +46,23 @@ public RobotAuto(LinearOpMode opMode) { imu = opMode.hardwareMap.get(IMU.class, "imu"); imu.initialize(new IMU.Parameters(orientationOnRobot)); imu.resetYaw(); + robotChassis.setTargetPosition(new int[]{0, 0, 0, 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 = 15.0; + static final double DRIVE_GEAR_REDUCTION = (double) 3 / 5; // wheel diameter in inches - static final double WHEEL_DIAMETER_INCHES = 3.0; + static final double WHEEL_DIAMETER_INCHES = 2.55; static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * 3.1415); + static final double STRIFE_REDUCTION = 1.25; + 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 @@ -75,6 +80,7 @@ public double getSteeringCorrection(double desiredHeading, double proportionalGa // 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. * @@ -83,6 +89,7 @@ public double getSteeringCorrection(double desiredHeading, double proportionalGa public double getHeading() { return getHeading(AngleUnit.DEGREES); } + /** * read the Robot heading directly from the IMU * @@ -99,8 +106,9 @@ public double getHeading(AngleUnit unit) { } public RobotAuto driveStraight(double maxDriveSpeed, - double distance, - double heading) { + double distance, + double heading) { + robotChassis.resetEncoder(); // Ensure that the OpMode is still active if (opMode.opModeIsActive()) { @@ -128,7 +136,15 @@ public RobotAuto driveStraight(double maxDriveSpeed, // 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.addData("x", "%4.2f, %4.2f, %4.2f, %4.2f, %4d", maxDriveSpeed, distance, heading, turnSpeed, moveCounts); + telemetry.addData("target", robotChassis.getTargetPosition()[0]); + telemetry.addData("target", robotChassis.getTargetPosition()[1]); + telemetry.addData("target", robotChassis.getTargetPosition()[2]); + telemetry.addData("target", robotChassis.getTargetPosition()[3]); + telemetry.addData("encoder", robotChassis.getCurrentPosition()[0]); + telemetry.addData("encoder", robotChassis.getCurrentPosition()[1]); + telemetry.addData("encoder", robotChassis.getCurrentPosition()[2]); + telemetry.addData("encoder", robotChassis.getCurrentPosition()[3]); telemetry.update(); } @@ -140,15 +156,15 @@ public RobotAuto driveStraight(double maxDriveSpeed, } public RobotAuto driveStrafe(double maxDriveSpeed, - double distance, - double heading + double distance, + double heading ) { // Ensure that the OpMode is still active if (opMode.opModeIsActive()) { // Determine new target position, and pass to motor controller - int moveCounts = (int) (distance * COUNTS_PER_INCH); + int moveCounts = (int) (distance * COUNTS_PER_INCH * STRIFE_REDUCTION); setStrafeTargetPosition(moveCounts); robotChassis.setRunMode(DcMotor.RunMode.RUN_TO_POSITION); @@ -180,21 +196,25 @@ public RobotAuto driveStrafe(double maxDriveSpeed, } return this; } + public void setStraightTargetPosition(int moveCounts) { - int[] motorPos = robotChassis.getTargetPosition(); - for (int i = 0; i < motorPos.length; i++) { - motorPos[i] += moveCounts; - } + int[] motorPos = robotChassis.getCurrentPosition(); + motorPos[0] = motorPos[0] + moveCounts; + motorPos[1] = motorPos[1] + moveCounts; + motorPos[2] = motorPos[2] + moveCounts; + motorPos[3] = motorPos[3] + moveCounts; robotChassis.setTargetPosition(motorPos); } + public void setStrafeTargetPosition(int moveCounts) { - int[] motorPos = robotChassis.getTargetPosition(); + int[] motorPos = robotChassis.getCurrentPosition(); 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 turnToHeading(double maxTurnSpeed, double heading) { // Run getSteeringCorrection() once to pre-calculate the current error @@ -254,20 +274,20 @@ 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()); } public RobotAuto backward(double d) { - return driveStraight(0.6, -d, getHeading()); + return driveStraight(0.6, d, getHeading()); } public RobotAuto fastBackward(double d) { - return driveStraight(0.9, -d, getHeading()); + return driveStraight(0.9, d, getHeading()); } public RobotAuto rightShift(double d) { @@ -296,6 +316,7 @@ protected double[] SpinVector(double[] vector, double 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]; @@ -307,6 +328,7 @@ protected double[] getDisplacement(double[] CurrentPos, double[] DesiredPos) { 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. @@ -322,6 +344,7 @@ public RobotAuto gotoPosition(double[] CurrentPos, double[] DesiredPos) { .leftShift(-Displacement[1]) .fastSpin(DesiredHeading); } + public RobotAuto gotoPosition(double x, double y, double h) { SparkFunOTOS.Pose2D CurrentPos = getPosition(); double[] DesiredPos = {x, y, h}; @@ -343,25 +366,29 @@ public RobotAuto gotoPosition2(double[] CurrentPos, double[] DesiredPos) { .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 stretchArm(){ + public RobotAuto stretchArm() { robotTop.setArmStretchPosition(0); return this; } - public RobotAuto setLiftPower(double power){ + + public RobotAuto setLiftPower(double power) { robotTop.setLeftPower(power); return this; } - public RobotAuto grab(){ + + public RobotAuto grab() { robotTop.setLiftServoPosition(0.6); return this; } - public RobotAuto release(){ + + public RobotAuto release() { robotTop.setLiftServoPosition(0.2); return this; } 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 5d29c45a8e21..acf7c3a929d1 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 @@ -35,12 +35,25 @@ public void initMovement() { rightFrontDrive.setDirection(DcMotor.Direction.REVERSE); rightBackDrive.setDirection(DcMotor.Direction.FORWARD); + resetEncoder(); + leftFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); leftBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); } + public void resetEncoder() { + leftFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightFrontDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightFrontDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + leftBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightBackDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightBackDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + public boolean isAllBusy() { return leftFrontDrive.isBusy() && leftBackDrive.isBusy() && @@ -61,14 +74,17 @@ public void driveRobot(double axial, double lateral, double yaw) { lateral = -lateral * 1.1; // Counteract imperfect strafing yaw = -yaw; - final double LEFT_REDUCTION = 0.96; +// 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(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 frontLeftPower = (axial + lateral + yaw) / denominator * LEFT_REDUCTION; +// double backLeftPower = (axial - lateral + yaw) / denominator * LEFT_REDUCTION; + double frontLeftPower = (axial + lateral + yaw) / denominator; + double backLeftPower = (axial - lateral + yaw) / denominator; double frontRightPower = (axial - lateral - yaw) / denominator; double backRightPower = (axial + lateral - yaw) / denominator; @@ -146,4 +162,13 @@ public int[] getTargetPosition() { return new int[]{lf, lb, lb, rb}; } + public int[] getCurrentPosition() { + // Set Target FIRST, then turn on RUN_TO_POSITION + int lf = leftFrontDrive.getCurrentPosition(); + int rf = rightFrontDrive.getCurrentPosition(); + int lb = leftBackDrive.getCurrentPosition(); + int rb = rightBackDrive.getCurrentPosition(); + return new int[]{lf, lb, lb, rb}; + } + } 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 index e5341c15b09f..6bd5db76a2cb 100644 --- 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 @@ -1,7 +1,6 @@ //这是第二版视觉识别,可以识别摄像头中心的颜色 //robotVision.getDetectedColor()返回的是一个字符串然后这个字符串是四个类型Unknown,Red，Blue，Yellow - package org.firstinspires.ftc.teamcode.hardware.RobotVision; import com.qualcomm.robotcore.hardware.HardwareMap; 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 33a474836c54..db8d7972112d 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 @@ -8,7 +8,7 @@ import com.qualcomm.robotcore.hardware.Servo; @Disabled -@Autonomous +@Autonomous(group = "Test") public class AutoTest extends LinearOpMode { @Override public void runOpMode() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest2.java new file mode 100644 index 000000000000..1c9b0d5fa463 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/AutoTest2.java @@ -0,0 +1,38 @@ +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 org.firstinspires.ftc.teamcode.hardware.RobotAuto; + +@Disabled +@Autonomous(group = "Test") +public class AutoTest2 extends LinearOpMode { + @Override + public void runOpMode() { + RobotAuto robotAuto = new RobotAuto(this); + waitForStart(); + while (opModeIsActive()){ + if(gamepad2.a){ + robotAuto.forward(24); + } + if(gamepad2.y){ + robotAuto.forward(-24); + } + if(gamepad2.x){ + robotAuto.leftShift(24); + } + if(gamepad2.b){ + robotAuto.rightShift(24); + } + if(gamepad2.dpad_left){ + robotAuto.spin(90); + } + if(gamepad2.dpad_right){ + robotAuto.spin(-90); + } + } + + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MotorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MotorTest.java new file mode 100644 index 000000000000..f7a9545204c5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/MotorTest.java @@ -0,0 +1,32 @@ +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; + +@Disabled +@TeleOp(group = "Test") +public class MotorTest extends LinearOpMode { + @Override + public void runOpMode(){ + waitForStart(); + + DcMotor motor = hardwareMap.get(DcMotor.class, "m"); + motor.setTargetPosition(560); + motor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motor.setPower(0.5); + while (motor.isBusy()); + motor.setPower(0); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + sleep(1000); + motor.setTargetPosition(0); + motor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + motor.setPower(0.5); + while (motor.isBusy()); + motor.setPower(0); + motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java index 5b8ec213d905..721d455a0146 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java @@ -8,7 +8,7 @@ import org.firstinspires.ftc.teamcode.hardware.RobotTop; @Disabled -@TeleOp +@TeleOp(group = "Test") public class PIDControllerTest extends LinearOpMode { @Override public void runOpMode(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java index 299c7287d9ae..7282aadf51dd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest2.java @@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.hardware.RobotTop; @Disabled -@TeleOp +@TeleOp(group = "Test") public class PIDControllerTest2 extends LinearOpMode { @Override public void runOpMode(){ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TurnServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/TurnServoTest.java similarity index 88% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TurnServoTest.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/TurnServoTest.java index 2e8d6173a516..b79853738bba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/test/TurnServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/TurnServoTest.java @@ -1,10 +1,12 @@ -package org.firstinspires.ftc.teamcode.previousCode.test; +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.Servo; -@TeleOp +@Disabled +@TeleOp(group = "Test") public class TurnServoTest extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { 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 29f4065763a9..a72b74846fe0 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 @@ -3,8 +3,10 @@ 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; + @Disabled @TeleOp(group = "Test") 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 22a21a4a9348..708d88e8c22b 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 @@ -2,6 +2,7 @@ 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; @Disabled 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 551ad97b75cb..1f924c50ebad 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 @@ -3,6 +3,7 @@ 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 From fb739a51667aab3ee3c905a5dbe7016c3eae8302 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sun, 19 Jan 2025 14:49:12 +0800 Subject: [PATCH 138/143] what is this? --- .../ftc/teamcode/ConceptTelemetry.java | 177 ------------------ 1 file changed, 177 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTelemetry.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTelemetry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTelemetry.java deleted file mode 100644 index 54cb49fbf7ad..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ConceptTelemetry.java +++ /dev/null @@ -1,177 +0,0 @@ -/* Copyright (c) 2017 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.teamcode; - -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.VoltageSensor; -import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.robotcore.external.Func; -import org.firstinspires.ftc.robotcore.external.Telemetry; - -/* - * This OpMode illustrates various ways in which telemetry can be - * transmitted from the robot controller to the driver station. The sample illustrates - * numeric and text data, formatted output, and optimized evaluation of expensive-to-acquire - * information. The telemetry log is illustrated by scrolling a poem - * to the driver station. - * - * Also see the Telemetry javadocs. - */ -@TeleOp(name = "Concept: Telemetry", group = "Concept") -@Disabled -public class ConceptTelemetry extends LinearOpMode { - /** Keeps track of the line of the poem which is to be emitted next */ - int poemLine = 0; - - /** Keeps track of how long it's been since we last emitted a line of poetry */ - ElapsedTime poemElapsed = new ElapsedTime(); - - static final String[] poem = new String[] { - - "Mary had a little lamb,", - "His fleece was white as snow,", - "And everywhere that Mary went,", - "The lamb was sure to go.", - "", - "He followed her to school one day,", - "Which was against the rule,", - "It made the children laugh and play", - "To see a lamb at school.", - "", - "And so the teacher turned it out,", - "But still it lingered near,", - "And waited patiently about,", - "Till Mary did appear.", - "", - "\"Why does the lamb love Mary so?\"", - "The eager children cry.", - "\"Why, Mary loves the lamb, you know,\"", - "The teacher did reply.", - "", - "" - }; - - @Override public void runOpMode() { - - /* we keep track of how long it's been since the OpMode was started, just - * to have some interesting data to show */ - ElapsedTime opmodeRunTime = new ElapsedTime(); - - // We show the log in oldest-to-newest order, as that's better for poetry - telemetry.log().setDisplayOrder(Telemetry.Log.DisplayOrder.OLDEST_FIRST); - // We can control the number of lines shown in the log - telemetry.log().setCapacity(6); - // The interval between lines of poetry, in seconds - double sPoemInterval = 0.6; - - /* - * Wait until we've been given the ok to go. For something to do, we emit the - * elapsed time as we sit here and wait. If we didn't want to do anything while - * we waited, we would just call waitForStart(). - */ - while (!isStarted()) { - telemetry.addData("time", "%.1f seconds", opmodeRunTime.seconds()); - telemetry.update(); - idle(); - } - - // Ok, we've been given the ok to go - - /* - * As an illustration, the first line on our telemetry display will display the battery voltage. - * The idea here is that it's expensive to compute the voltage (at least for purposes of illustration) - * so you don't want to do it unless the data is _actually_ going to make it to the - * driver station (recall that telemetry transmission is throttled to reduce bandwidth use. - * Note that getBatteryVoltage() below returns 'Infinity' if there's no voltage sensor attached. - * - * @see Telemetry#getMsTransmissionInterval() - */ - telemetry.addData("voltage", "%.1f volts", new Func() { - @Override public Double value() { - return getBatteryVoltage(); - } - }); - - // Reset to keep some timing stats for the post-'start' part of the OpMode - opmodeRunTime.reset(); - int loopCount = 1; - - // Go go gadget robot! - while (opModeIsActive()) { - - // Emit poetry if it's been a while - if (poemElapsed.seconds() > sPoemInterval) { - emitPoemLine(); - } - - // As an illustration, show some loop timing information - telemetry.addData("loop count", loopCount); - telemetry.addData("ms/loop", "%.3f ms", opmodeRunTime.milliseconds() / loopCount); - - // Show joystick information as some other illustrative data - telemetry.addLine("left joystick | ") - .addData("x", gamepad1.left_stick_x) - .addData("y", gamepad1.left_stick_y); - telemetry.addLine("right joystick | ") - .addData("x", gamepad1.right_stick_x) - .addData("y", gamepad1.right_stick_y); - - /* - * Transmit the telemetry to the driver station, subject to throttling. - * See the documentation for Telemetry.getMsTransmissionInterval() for more information. - */ - telemetry.update(); - - // Update loop info - loopCount++; - } - } - - // emits a line of poetry to the telemetry log - void emitPoemLine() { - telemetry.log().add(poem[poemLine]); - poemLine = (poemLine+1) % poem.length; - poemElapsed.reset(); - } - - // Computes the current battery voltage - double getBatteryVoltage() { - double result = Double.POSITIVE_INFINITY; - for (VoltageSensor sensor : hardwareMap.voltageSensor) { - double voltage = sensor.getVoltage(); - if (voltage > 0) { - result = Math.min(result, voltage); - } - } - return result; - } -} From 9f54f681ef2c6e885d06db1f51ad2847eb04f984 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sun, 19 Jan 2025 16:21:21 +0800 Subject: [PATCH 139/143] new driving function with IMU --- .../ftc/teamcode/ManualOpMode.java | 13 ++++----- .../ftc/teamcode/hardware/RobotAuto.java | 9 +++--- .../ftc/teamcode/hardware/RobotChassis.java | 26 +++++++++++++++++ .../ftc/teamcode/hardware/RobotTop.java | 4 +-- .../teamcode/previousCode/PreviousManual.java | 6 ++-- .../ftc/teamcode/test/DriveTest.java | 29 +++++++++++++++++++ .../ftc/teamcode/test/MecanumTeleOp.java | 1 - .../ftc/teamcode/test/PIDControllerTest.java | 4 +-- 8 files changed, 71 insertions(+), 21 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/DriveTest.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 fefcf12eb6f9..f002130ef185 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ManualOpMode.java @@ -31,6 +31,7 @@ enum LiftState { final double SPIN_HOVERING_POSITION = 0.85; final double SPIN_DOWN_POSITION = 0; final double TURN_BACK_POSITION = 0.3; + final double TURN_LOCK_POSITION = 0.42; final double TURN_HOVERING_POSITION = 0.62; final double TURN_DOWN_POSITION = 0.7; final double GRAB_OPEN_POSITION = 0.4; @@ -120,10 +121,6 @@ public void runOpMode() { handleRunningState(); break; } - telemetry.addData("encoder", robotChassis.getCurrentPosition()[0]); - telemetry.addData("encoder", robotChassis.getCurrentPosition()[1]); - telemetry.addData("encoder", robotChassis.getCurrentPosition()[2]); - telemetry.addData("encoder", robotChassis.getCurrentPosition()[3]); telemetry.addData("arm", armState); telemetry.addData("lift", liftState); telemetry.addData("armPos", robotTop.getTurnPosition()); @@ -316,7 +313,7 @@ protected void handleDisabledState() { } if (gamepad1.right_trigger != 0 || gamepad1.left_trigger != 0) { // robotTop.setTurnPosition(TURN_DOWN_POSITION - 0.1); - targetTurnPosition = TURN_DOWN_POSITION - 0.1; + targetTurnPosition = TURN_LOCK_POSITION; armState = ArmState.LOCKING; liftState = LiftState.RUNNING; } @@ -324,18 +321,18 @@ protected void handleDisabledState() { protected void handleRunningState() { if (gamepad1.right_trigger != 0) { - robotTop.setLeftPower(0.5); + robotTop.setLiftPower(0.5); robotTop.setTopServoPosition(TOP_BACK); robotTop.setLiftTargetPos(robotTop.getLiftPosition()); } else if (gamepad1.left_trigger != 0) { - robotTop.setLeftPower(-0.5); + robotTop.setLiftPower(-0.5); robotTop.setTopServoPosition(TOP_BACK); robotTop.setLiftTargetPos(robotTop.getLiftPosition()); } else { if (robotTop.getLiftPosition() >= 200) { robotTop.updateLiftPID(); } else { - robotTop.setLeftPower(0); + robotTop.setLiftPower(0); } } if (gamepad2.left_bumper && !previousGamepad2.left_bumper) { 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 8df46d12fc8b..3294282b9737 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,7 +3,6 @@ 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; @@ -373,13 +372,13 @@ public RobotAuto gotoPosition2(double x, double y, double h) { return gotoPosition2(new double[]{CurrentPos.x, CurrentPos.y, CurrentPos.h}, DesiredPos); } - public RobotAuto stretchArm() { - robotTop.setArmStretchPosition(0); + public RobotAuto setLiftPower(double power) { + robotTop.setLiftPower(power); return this; } - public RobotAuto setLiftPower(double power) { - robotTop.setLeftPower(power); + public RobotAuto setTurnPosition(double p){ + robotTop.setTurnPosition(p); return this; } 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 acf7c3a929d1..07e360617998 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,10 +1,13 @@ package org.firstinspires.ftc.teamcode.hardware; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @SuppressWarnings(value = "unused") public class RobotChassis { @@ -15,6 +18,7 @@ public class RobotChassis { protected DcMotor leftBackDrive; protected DcMotor rightFrontDrive; protected DcMotor rightBackDrive; + protected IMU imu; public RobotChassis(LinearOpMode opMode) { this.opMode = opMode; @@ -41,6 +45,18 @@ public void initMovement() { leftBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightFrontDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightBackDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + //IMU + imu = opMode.hardwareMap.get(IMU.class, "imu"); + resetIMU(); + } + + public void resetIMU(){ + RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.FORWARD; + RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.LEFT; + RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + imu.resetYaw(); } public void resetEncoder() { @@ -94,6 +110,16 @@ public void driveRobot(double axial, double lateral, double yaw) { rightBackDrive.setPower(backRightPower); } + public void absoluteDriveRobot(double axial, double lateral, double yaw){ + double botHeading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + + // Rotate the movement direction counter to the bot's rotation + double rotX = axial * Math.cos(-botHeading) - lateral * Math.sin(-botHeading); + double rotY = axial * Math.sin(-botHeading) + lateral * Math.cos(-botHeading); + + driveRobot(rotX, rotY, yaw); + } + /** * Set the power of every motor. * 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 accf69eb99c9..78a7ae790a1b 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 @@ -64,7 +64,7 @@ public void init() { armStretchMotor.setDirection(DcMotor.Direction.REVERSE); } - public void setLeftPower(double power) { + public void setLiftPower(double power) { leftLiftMotor.setPower(power); rightLiftMotor.setPower(power); } @@ -190,7 +190,7 @@ else if (delta < -ticks_per_rev * 0.6) else if (power < -power_max) power = -power_max; } - setLeftPower(power); + setLiftPower(power); telemetry.addData("power", power); telemetry.addData("p", p); telemetry.addData("i", i); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/PreviousManual.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/PreviousManual.java index 841c6bde8c57..48f1138a2722 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/PreviousManual.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/previousCode/PreviousManual.java @@ -109,16 +109,16 @@ public void runOpMode() { } if (gamepad1.right_trigger != 0) { - robotTop.setLeftPower(0.5); + robotTop.setLiftPower(0.5); robotTop.setLiftTargetPos(robotTop.getLiftPosition()); } else if (gamepad1.left_trigger != 0) { - robotTop.setLeftPower(-0.5); + robotTop.setLiftPower(-0.5); robotTop.setLiftTargetPos(robotTop.getLiftPosition()); } else { if (robotTop.getLiftPosition() >= 200) { robotTop.updateLiftPID(); } else { - robotTop.setLeftPower(0); + robotTop.setLiftPower(0); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/DriveTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/DriveTest.java new file mode 100644 index 000000000000..20482acec80c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/DriveTest.java @@ -0,0 +1,29 @@ +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 org.firstinspires.ftc.teamcode.hardware.RobotAuto; +import org.firstinspires.ftc.teamcode.hardware.RobotChassis; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; + +@TeleOp(group = "Test") +public class DriveTest extends LinearOpMode { + @Override + public void runOpMode() { + RobotChassis robotChassis = new RobotChassis(this); + + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + if(gamepad2.a){ + robotChassis.resetIMU(); + } + robotChassis.absoluteDriveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); + sleep(10); + } + } +} \ No newline at end of file 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 424872db219a..194167867955 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 @@ -6,7 +6,6 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; -@Disabled @TeleOp(group = "Test") public class MecanumTeleOp extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java index 721d455a0146..afef95a60ff5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/PIDControllerTest.java @@ -34,7 +34,7 @@ public void runOpMode(){ i += Ki * delta; d = Kd * (currentPos - previousPos); power = Math.min(p + i + d, 1); - robotTop.setLeftPower(power); + robotTop.setLiftPower(power); previousGamepad1.copy(gamepad1); telemetry.addData("power",power); telemetry.addData("p",p); @@ -43,7 +43,7 @@ public void runOpMode(){ telemetry.update(); sleep(10); }else { - robotTop.setLeftPower(0); + robotTop.setLiftPower(0); } } } From f12179c1531a0e3a9e9aec1cf981283e92c00ae5 Mon Sep 17 00:00:00 2001 From: Eternity0 Date: Sun, 19 Jan 2025 17:39:41 +0800 Subject: [PATCH 140/143] some minor changes --- .../ftc/teamcode/hardware/RobotAuto.java | 30 ++++++++++++------- .../ftc/teamcode/hardware/RobotChassis.java | 5 ++++ .../ftc/teamcode/test/DriveTest.java | 2 +- 3 files changed, 26 insertions(+), 11 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 3294282b9737..1f760f773e7d 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 @@ -135,16 +135,16 @@ public RobotAuto driveStraight(double maxDriveSpeed, // 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.addData("target", robotChassis.getTargetPosition()[0]); - telemetry.addData("target", robotChassis.getTargetPosition()[1]); - telemetry.addData("target", robotChassis.getTargetPosition()[2]); - telemetry.addData("target", robotChassis.getTargetPosition()[3]); - telemetry.addData("encoder", robotChassis.getCurrentPosition()[0]); - telemetry.addData("encoder", robotChassis.getCurrentPosition()[1]); - telemetry.addData("encoder", robotChassis.getCurrentPosition()[2]); - telemetry.addData("encoder", robotChassis.getCurrentPosition()[3]); - telemetry.update(); +// telemetry.addData("x", "%4.2f, %4.2f, %4.2f, %4.2f, %4d", maxDriveSpeed, distance, heading, turnSpeed, moveCounts); +// telemetry.addData("target", robotChassis.getTargetPosition()[0]); +// telemetry.addData("target", robotChassis.getTargetPosition()[1]); +// telemetry.addData("target", robotChassis.getTargetPosition()[2]); +// telemetry.addData("target", robotChassis.getTargetPosition()[3]); +// telemetry.addData("encoder", robotChassis.getCurrentPosition()[0]); +// telemetry.addData("encoder", robotChassis.getCurrentPosition()[1]); +// telemetry.addData("encoder", robotChassis.getCurrentPosition()[2]); +// telemetry.addData("encoder", robotChassis.getCurrentPosition()[3]); +// telemetry.update(); } // Stop all motion & Turn off RUN_TO_POSITION @@ -158,6 +158,7 @@ public RobotAuto driveStrafe(double maxDriveSpeed, double distance, double heading ) { + robotChassis.resetEncoder(); // Ensure that the OpMode is still active if (opMode.opModeIsActive()) { @@ -391,4 +392,13 @@ public RobotAuto release() { robotTop.setLiftServoPosition(0.2); return this; } + + public RobotAuto topOut() { + robotTop.setTopServoPosition(0.66); + return this; + } + public RobotAuto topBack(){ + robotTop.setTopServoPosition(0.03); + return this; + } } 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 07e360617998..052ac07dae4d 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 @@ -103,6 +103,11 @@ public void driveRobot(double axial, double lateral, double yaw) { double backLeftPower = (axial - lateral + yaw) / denominator; double frontRightPower = (axial - lateral - yaw) / denominator; double backRightPower = (axial + lateral - yaw) / denominator; +// telemetry.addData("fl",frontLeftPower); +// telemetry.addData("bl",backLeftPower); +// telemetry.addData("fr",frontRightPower); +// telemetry.addData("br",backRightPower); +// telemetry.update(); leftFrontDrive.setPower(frontLeftPower); leftBackDrive.setPower(backLeftPower); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/DriveTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/DriveTest.java index 20482acec80c..364dc4b1e6b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/DriveTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/test/DriveTest.java @@ -23,7 +23,7 @@ public void runOpMode() { robotChassis.resetIMU(); } robotChassis.absoluteDriveRobot(gamepad2.left_stick_y, gamepad2.left_stick_x, gamepad2.right_stick_x); - sleep(10); + sleep(50); } } } \ No newline at end of file From 1ddd05c646f652d593542dddb82f5c6efa835613 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Sun, 19 Jan 2025 18:29:45 +0800 Subject: [PATCH 141/143] Feat:Advanced Autonomous in a new way. --- .../ftc/teamcode/test/AutoTest.java | 143 ++++++++---------- 1 file changed, 62 insertions(+), 81 deletions(-) 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 db8d7972112d..370e7c1a7da5 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 @@ -1,13 +1,14 @@ 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; -import com.qualcomm. robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; +import com.sun.tools.javac.comp.Todo; + +import org.firstinspires.ftc.teamcode.hardware.RobotAuto; +import org.firstinspires.ftc.teamcode.hardware.RobotTop; -@Disabled @Autonomous(group = "Test") public class AutoTest extends LinearOpMode { @Override @@ -28,107 +29,87 @@ public void runOpMode() { frontLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); backLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER); frontRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - leftLift.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - rightLift.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + leftLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + rightLift.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); backRight.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - frontRight.setDirection(DcMotorSimple.Direction.REVERSE); + frontRight.setDirection(DcMotor.Direction.REVERSE); rightLift.setDirection(DcMotor.Direction.REVERSE); armStretchMotor.setDirection(DcMotor.Direction.REVERSE); + RobotAuto robotAuto = new RobotAuto(this); + RobotTop robotTop = new RobotTop(this); + + waitForStart(); - //tighten - liftServo.setPosition(0.95); - sleep(700); - - frontLeft.setPower(-0.7); - backLeft.setPower(0.7); - frontRight.setPower(0.7); - backRight.setPower(-0.7); - sleep(700); - - //forward - frontLeft.setPower(0.7); - backLeft.setPower(0.7); - frontRight.setPower(0.7); - backRight.setPower(0.7); - sleep(600); - frontLeft.setPower(0); - backLeft.setPower(0); - frontRight.setPower(0); - backRight.setPower(0); - - //stretch arm - armStretchMotor.setPower(0.5); - sleep(1400); - armStretchMotor.setPower(0); - - //lift + liftServo.setPosition(0.6); + sleep(700);//0.6 close + robotAuto.rightShift(16); + robotAuto.backward(28); + + robotTop.setTurnPosition(0.5);//0.5 out + sleep(500); + leftLift.setPower(0.5); rightLift.setPower(0.5); - sleep(1700); + sleep(1100); leftLift.setPower(0); rightLift.setPower(0); - frontLeft.setPower(0.5); - backLeft.setPower(0.5); - frontRight.setPower(0.5); - backRight.setPower(0.5); - sleep(130); - frontLeft.setPower(0); - backLeft.setPower(0); - frontRight.setPower(0); - backRight.setPower(0); + robotAuto.backward(3); leftLift.setPower(-0.5); rightLift.setPower(-0.5); - while(leftLift.getCurrentPosition() < 800); + sleep(250); + leftLift.setPower(0); + rightLift.setPower(0); + + robotAuto.fastForward(20); + + robotAuto.leftShift(40); + robotAuto.spin(180); + liftServo.setPosition(0.1);//0.1 open + robotAuto.backward(12); + liftServo.setPosition(0.6);//0.6 close + + leftLift.setPower(0.5); + rightLift.setPower(0.5); + sleep(200); leftLift.setPower(0); rightLift.setPower(0); - //Release - liftServo.setPosition(0.2); +// robotTop.setTurnPosition(0.25);//0.25 in +// sleep(500); + + robotAuto.forward(10); + robotAuto.leftShift(40); + robotAuto.spin(0); + robotAuto.fastBackward(10); + + robotTop.setTurnPosition(0.45);//0.45 out sleep(500); - armStretchMotor.setPower(-0.5); - sleep(1400); - armStretchMotor.setPower(0); + leftLift.setPower(0.5); + rightLift.setPower(0.5); + sleep(1100); + leftLift.setPower(0); + rightLift.setPower(0); + + robotAuto.backward(3); + + leftLift.setPower(-0.5); + rightLift.setPower(-0.5); + sleep(250); + leftLift.setPower(0); + rightLift.setPower(0); - //back1 - frontLeft.setPower(-0.7); - backLeft.setPower(-0.7); - frontRight.setPower(-0.7); - backRight.setPower(-0.7); + robotTop.setTurnPosition(0.25);//0.25 in sleep(500); - //rafe - frontLeft.setPower(0.7); - backLeft.setPower(-0.7); - frontRight.setPower(-0.7); - backRight.setPower(0.7); - sleep(1500); - - //spin - frontLeft.setPower(-0.7); - backLeft.setPower(-0.7); - frontRight.setPower(0.7); - backRight.setPower(0.7); - sleep(1320); - frontLeft.setPower(0); - backLeft.setPower(0); - frontRight.setPower(0); - backRight.setPower(0); - - //back2 - frontLeft.setPower(0.7); - backLeft.setPower(0.7); - frontRight.setPower(0.7); - backRight.setPower(0.7); - sleep(100); - - liftServo.setPosition(0.95); - sleep(700); + robotAuto.fastForward(5); + robotAuto.leftShift(60); +// robotAuto.fastForward(5); } } \ No newline at end of file From 9d3bf4977bc74b43c1ec965d4b27d2ba9831fc62 Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Tue, 21 Jan 2025 14:54:20 +0800 Subject: [PATCH 142/143] Feat:Advanced Autonomous in a new way. --- .../ftc/teamcode/test/AutoTest.java | 44 ++++++++++++++----- 1 file changed, 34 insertions(+), 10 deletions(-) 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 370e7c1a7da5..4c46c3a76319 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 @@ -43,35 +43,47 @@ public void runOpMode() { waitForStart(); - liftServo.setPosition(0.6); - sleep(700);//0.6 close + liftServo.setPosition(0.7);//0.7 back close + sleep(700); robotAuto.rightShift(16); robotAuto.backward(28); - robotTop.setTurnPosition(0.5);//0.5 out + robotTop.setTurnPosition(0.45);//0.45 arm out sleep(500); + //lift up1 leftLift.setPower(0.5); rightLift.setPower(0.5); sleep(1100); leftLift.setPower(0); rightLift.setPower(0); + //close to target robotAuto.backward(3); + //lift down1 leftLift.setPower(-0.5); rightLift.setPower(-0.5); sleep(250); leftLift.setPower(0); rightLift.setPower(0); + leftLift.setPower(-0.5); + rightLift.setPower(-0.5); + sleep(100); + liftServo.setPosition(0.1);//back open + sleep(100); + leftLift.setPower(0); + rightLift.setPower(0); + robotAuto.fastForward(20); robotAuto.leftShift(40); robotAuto.spin(180); - liftServo.setPosition(0.1);//0.1 open + liftServo.setPosition(0.1);//0.1 back open robotAuto.backward(12); - liftServo.setPosition(0.6);//0.6 close + liftServo.setPosition(0.7);//0.7 back close + sleep(150); leftLift.setPower(0.5); rightLift.setPower(0.5); @@ -87,29 +99,41 @@ public void runOpMode() { robotAuto.spin(0); robotAuto.fastBackward(10); - robotTop.setTurnPosition(0.45);//0.45 out + robotTop.setTurnPosition(0.45);//0.45 arm out sleep(500); - leftLift.setPower(0.5); + //lift up2 rightLift.setPower(0.5); + leftLift.setPower(0.5); sleep(1100); leftLift.setPower(0); rightLift.setPower(0); - robotAuto.backward(3); + //close to target + robotAuto.backward(5); + //lift down2 leftLift.setPower(-0.5); rightLift.setPower(-0.5); sleep(250); leftLift.setPower(0); rightLift.setPower(0); - robotTop.setTurnPosition(0.25);//0.25 in + leftLift.setPower(-0.5); + rightLift.setPower(-0.5); + sleep(100); + liftServo.setPosition(0.1);//back open + sleep(100); + leftLift.setPower(0); + rightLift.setPower(0); + sleep(100); + + robotTop.setTurnPosition(0.25);//0.25 arm in sleep(500); robotAuto.fastForward(5); robotAuto.leftShift(60); -// robotAuto.fastForward(5); + robotAuto.fastForward(15); } } \ No newline at end of file From e591859468c7268dd3319a8d9e44b71d84d152cd Mon Sep 17 00:00:00 2001 From: LeoLBWK Date: Tue, 21 Jan 2025 17:32:45 +0800 Subject: [PATCH 143/143] Feat:Advanced One of Autonomous in the new way. --- .../ftc/teamcode/test/AutoTest.java | 31 ++++++++++++------- 1 file changed, 19 insertions(+), 12 deletions(-) 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 4c46c3a76319..ba5d553d4888 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 @@ -78,26 +78,32 @@ public void runOpMode() { robotAuto.fastForward(20); - robotAuto.leftShift(40); + robotAuto.spin(90); + robotAuto.fastForward(45); robotAuto.spin(180); liftServo.setPosition(0.1);//0.1 back open + robotAuto.spin(180); + sleep(600); robotAuto.backward(12); + robotAuto.forward(0.8); + liftServo.setPosition(0.7);//0.7 back close + sleep(500); liftServo.setPosition(0.7);//0.7 back close - sleep(150); leftLift.setPower(0.5); rightLift.setPower(0.5); - sleep(200); + sleep(400); leftLift.setPower(0); rightLift.setPower(0); + sleep(300); -// robotTop.setTurnPosition(0.25);//0.25 in -// sleep(500); - + sleep(500); robotAuto.forward(10); - robotAuto.leftShift(40); + robotAuto.spin(270); + robotAuto.fastForward(40); robotAuto.spin(0); - robotAuto.fastBackward(10); + robotAuto.backward(17.5); + robotAuto.spin(1); robotTop.setTurnPosition(0.45);//0.45 arm out sleep(500); @@ -110,7 +116,7 @@ public void runOpMode() { rightLift.setPower(0); //close to target - robotAuto.backward(5); + robotAuto.backward(3); //lift down2 leftLift.setPower(-0.5); @@ -131,9 +137,10 @@ public void runOpMode() { robotTop.setTurnPosition(0.25);//0.25 arm in sleep(500); - robotAuto.fastForward(5); - robotAuto.leftShift(60); - robotAuto.fastForward(15); + robotAuto.fastForward(10); + robotAuto.spin(90); + robotAuto.fastForward(60); + robotAuto.rightShift(20); } } \ No newline at end of file