加入 ArcOr

6-BennyLi-9 · Sep 11, 2024 · 0529359 · 0529359
1 parent e5878db
commit 0529359
Showing 1 changed file with 31 additions and 0 deletions.
diff --git a/...main/java/org/firstinspires/ftc/teamcode/DriveControls/Odometry/ArcOrganizedOdometer.java b/...main/java/org/firstinspires/ftc/teamcode/DriveControls/Odometry/ArcOrganizedOdometer.java
@@ -0,0 +1,31 @@
+package org.firstinspires.ftc.teamcode.DriveControls.Odometry;
+
+import org.firstinspires.ftc.teamcode.Utils.Annotations.OdometerPrograms;
+import org.firstinspires.ftc.teamcode.Utils.Clients.Client;
+
+@OdometerPrograms
+public class ArcOrganizedOdometer extends ClassicOdometer{
+	ArcOrganizedOdometer(Client client) {
+		super(client);
+	}
+
+	@Override
+	public void ProcessDeltaRelPose(double relX, double relY, double relTheta) {
+		double REL_X=GetArcRelX(relX,relY,relTheta);
+		double REL_Y=GetArcRelY(relX,relY,relTheta);
+		AddDelta(
+				REL_X*Math.cos(Math.toRadians(relTheta))-REL_Y*Math.sin(Math.toRadians(relTheta)),
+				REL_Y*Math.cos(Math.toRadians(relTheta))+REL_X*Math.sin(Math.toRadians(relTheta)),
+				relTheta
+		);
+	}
+
+	protected double GetArcRelX(double relX,double relY,double relTheta){
+		double r0=relX/relTheta,r1=relY/relTheta;
+		return r0*Math.sin(Math.toRadians(relTheta))-r1*(1-Math.cos(Math.toRadians(relTheta)));
+	}
+	protected double GetArcRelY(double relX,double relY,double relTheta){
+		double r0=relX/relTheta,r1=relY/relTheta;
+		return r1*Math.sin(Math.toRadians(relTheta))+r0*(1-Math.cos(Math.toRadians(relTheta)));
+	}
+}