From 0529359eede9aaa1bca16f5a06f9666b54b91193 Mon Sep 17 00:00:00 2001 From: BennyLi Date: Wed, 11 Sep 2024 21:55:01 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E5=85=A5=20ArcOr?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Odometry/ArcOrganizedOdometer.java | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/Odometry/ArcOrganizedOdometer.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/Odometry/ArcOrganizedOdometer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriveControls/Odometry/ArcOrganizedOdometer.java new file mode 100644 index 0000000..266ba95 --- /dev/null +++ b/TeamCode/src/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))); + } +}