在 Sensors 中加入获取三个编码器的Ticks的方法

6-BennyLi-9 · Aug 27, 2024 · 5830104 · 5830104
1 parent be3d16b
commit 5830104
Show file tree

Hide file tree

Showing 5 changed files with 27 additions and 7 deletions.
diff --git a/...pires/ftc/teamcode/DriveControls/Localizers/plugins/DeadWheelVectorPositionLocalizer.java b/...pires/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
@@ -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
@@ -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
@@ -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
@@ -18,7 +18,7 @@ public void runOpMode() {
 
 		while (!isStopRequested()){
 			robot.update();
-			robot.changeData("Ticks",robot.classic.encoders.TurningTicks);
+			robot.changeData("Ticks",robot.sensors.getDeltaT());
 		}
 	}
 }