From 5830104446920e6a26ada5a6389c40ef2918fa80 Mon Sep 17 00:00:00 2001 From: Benny0Li Date: Tue, 27 Aug 2024 11:12:33 +0800 Subject: [PATCH] =?UTF-8?q?=E5=9C=A8=20Sensors=20=E4=B8=AD=E5=8A=A0?= =?UTF-8?q?=E5=85=A5=E8=8E=B7=E5=8F=96=E4=B8=89=E4=B8=AA=E7=BC=96=E7=A0=81?= =?UTF-8?q?=E5=99=A8=E7=9A=84Ticks=E7=9A=84=E6=96=B9=E6=B3=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../DeadWheelVectorPositionLocalizer.java | 10 ++++------ .../ftc/teamcode/Hardwares/basic/Sensors.java | 20 +++++++++++++++++++ .../RIC_tuning/AxialInchPerTickTest.java | 1 + .../RIC_tuning/LateralInchPerTickTest.java | 1 + .../RIC_tuning/TurningDegPerTickTest.java | 2 +- 5 files changed, 27 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/Localizers/plugins/DeadWheelVectorPositionLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/Localizers/plugins/DeadWheelVectorPositionLocalizer.java index 8d6a2c6..07de2d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/Localizers/plugins/DeadWheelVectorPositionLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/Localizers/plugins/DeadWheelVectorPositionLocalizer.java @@ -1,9 +1,7 @@ package org.firstinspires.ftc.teamcode.DriveControls.Localizers.plugins; import static org.firstinspires.ftc.teamcode.Params.AxialInchPerTick; -import static org.firstinspires.ftc.teamcode.Params.AxialPosition; import static org.firstinspires.ftc.teamcode.Params.LateralInchPerTick; -import static org.firstinspires.ftc.teamcode.Params.LateralPosition; import androidx.annotation.NonNull; import androidx.annotation.Nullable; @@ -59,13 +57,13 @@ public Vector2d getDelta(){ case TwoDeadWheels: return new Vector2d( 0, - sensors.LeftTick/2* AxialInchPerTick+sensors.RightTick/2* AxialInchPerTick + sensors.getDeltaA()* AxialInchPerTick ); case ThreeDeadWheels: return new Vector2d( - (((sensors.LeftTick * LateralPosition) / 2) / LateralPosition - (sensors.LeftTick * AxialPosition) / LateralPosition - (sensors.RightTick * AxialPosition) / LateralPosition) * 2 * LateralInchPerTick, - sensors.LeftTick/2* AxialInchPerTick+sensors.RightTick/2* AxialInchPerTick - ); + sensors.getDeltaL()*LateralInchPerTick, + sensors.getDeltaA()* AxialInchPerTick + ); } return null; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardwares/basic/Sensors.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardwares/basic/Sensors.java index 838782f..3e7977b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardwares/basic/Sensors.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hardwares/basic/Sensors.java @@ -16,6 +16,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; import org.firstinspires.ftc.teamcode.Hardwares.namespace.DeviceMap; import org.firstinspires.ftc.teamcode.Hardwares.namespace.HardwareDevices; +import org.firstinspires.ftc.teamcode.Params; import org.firstinspires.ftc.teamcode.utils.Enums.DeadWheelsType; public class Sensors { @@ -93,4 +94,23 @@ public void update(){ break; } } + + /** + * @return 机器前进的TICK数 + */ + public double getDeltaA(){ + return (LeftTick-LastLeftTick+RightTick-LastRightTick)/2; + } + /** + * @return 机器平移的TICK数 + */ + public double getDeltaL(){ + return MiddleTick-LastMiddleTick-getDeltaA()*Params.AxialPosition/Params.LateralPosition*4; + } + /** + * @return 机器旋转的TICK数 + */ + public double getDeltaT(){ + return (LeftTick-LastLeftTick-RightTick+LastRightTick)/2; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_tuning/AxialInchPerTickTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_tuning/AxialInchPerTickTest.java index db8b1c4..c6a482a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_tuning/AxialInchPerTickTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_tuning/AxialInchPerTickTest.java @@ -18,6 +18,7 @@ public void runOpMode() { while (!isStopRequested()){ robot.update(); + robot.changeData("Ticks",robot.sensors.getDeltaA()); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_tuning/LateralInchPerTickTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_tuning/LateralInchPerTickTest.java index 9dbaf10..252acfe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_tuning/LateralInchPerTickTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_tuning/LateralInchPerTickTest.java @@ -18,6 +18,7 @@ public void runOpMode() { while (!isStopRequested()){ robot.update(); + robot.changeData("Ticks",robot.sensors.getDeltaL()); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_tuning/TurningDegPerTickTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_tuning/TurningDegPerTickTest.java index 77f4c50..5589884 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_tuning/TurningDegPerTickTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RIC_tuning/TurningDegPerTickTest.java @@ -18,7 +18,7 @@ public void runOpMode() { while (!isStopRequested()){ robot.update(); - robot.changeData("Ticks",robot.classic.encoders.TurningTicks); + robot.changeData("Ticks",robot.sensors.getDeltaT()); } } }