From 139a7b59a989c5a2b981999d910a8a13b33bd23d Mon Sep 17 00:00:00 2001
From: missing <piecenotfound@gmail.com>
Date: Thu, 26 Jan 2023 13:56:23 -0600
Subject: [PATCH] DashboardPoser

---
 .../ftc/teamcode/auto/DashboardPoser.java     | 57 +++++++++++++++++++
 .../ftc/teamcode/kinematics/FieldVector.java  |  4 ++
 .../ftc/teamcode/kinematics/Poser.java        | 18 +++++-
 3 files changed, 78 insertions(+), 1 deletion(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/DashboardPoser.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/DashboardPoser.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/DashboardPoser.java
new file mode 100644
index 0000000..2f36c80
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/DashboardPoser.java
@@ -0,0 +1,57 @@
+package org.firstinspires.ftc.teamcode.auto;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.reflection.FieldProvider;
+import com.acmerobotics.dashboard.config.variable.BasicVariable;
+import com.acmerobotics.dashboard.config.variable.CustomVariable;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Hardware;
+import org.firstinspires.ftc.teamcode.kinematics.FieldVector;
+import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
+import org.firstinspires.ftc.teamcode.kinematics.Poser;
+
+@TeleOp(name = "Dashboard Poser")
+public class DashboardPoser extends LinearOpMode {
+    private double targetXPos = 0;
+    private double targetYPos = 0;
+    private double targetYaw = 0;
+    private LiftPosition targetLiftPos = LiftPosition.FLOOR;
+    private double speed = 0.2;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Hardware hardware = new Hardware(this);
+        Poser poser = new Poser(hardware, speed, FieldVector.inTiles(targetXPos, targetYPos), targetYaw);
+
+        DashboardPoser me = this;
+        FtcDashboard.getInstance().withConfigRoot(root -> {
+            CustomVariable subVar = new CustomVariable();
+            me.addFieldVariable(subVar, "Target X Position", "targetXPos");
+            me.addFieldVariable(subVar, "Target Y Position", "targetYPos");
+            me.addFieldVariable(subVar, "Target Yaw", "targetYaw");
+            me.addFieldVariable(subVar, "Target Lift Height", "targetLiftPos");
+            me.addFieldVariable(subVar, "Speed", "speed");
+
+            root.putVariable(me.getClass().getSimpleName(), subVar);
+        });
+
+        waitForStart();
+
+        while (!isStopRequested()) {
+            poser.setSpeed(speed);
+            poser.goTo(FieldVector.inTiles(targetXPos, targetYPos), targetYaw, targetLiftPos);
+        }
+    }
+
+    private <T> void addFieldVariable(CustomVariable root, String name, String field) {
+        try {
+            root.putVariable(name, new BasicVariable<T>(
+                    new FieldProvider<>(this.getClass().getField(field), this)
+            ));
+        } catch (NoSuchFieldException e) {
+            telemetry.log().add("warning: field `" + field + "` not found on " + this.getClass().getSimpleName());
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/FieldVector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/FieldVector.java
index bec854f..e29dd3e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/FieldVector.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/FieldVector.java
@@ -28,6 +28,10 @@ public class FieldVector {
         return "(" + this.x + ", " + this.y + ")";
     }
 
+    public boolean equals(FieldVector rhs) {
+        return this.x == rhs.x && this.y == rhs.y;
+    }
+
     public FieldVector add(FieldVector rhs) {
         return new FieldVector(this.x.add(rhs.x), this.y.add(rhs.y));
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java
index e48cc2d..82a4e89 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java
@@ -40,6 +40,14 @@ public class Poser {
         this.lastTargetLiftPos = hardware.getLiftEncoder();
     }
 
+    public double getSpeed() {
+        return this.speed;
+    }
+
+    public void setSpeed(double speed) {
+        this.speed = speed;
+    }
+
     private Pose getPose() {
         return new Pose(
                 integrator.pos,
@@ -56,6 +64,14 @@ public class Poser {
         }
     }
 
+    private static double modAngle(double angle) {
+        angle %= 360;
+        if (angle > 180) {
+            angle -= 360;
+        }
+        return angle;
+    }
+
     public void goTo(FieldVector pos, double yaw, int liftPos) {
         this.goTo(new Pose(pos, yaw, liftPos));
     }
@@ -109,7 +125,7 @@ public class Poser {
             Pose currentPose = this.getPose();
 
             FieldVector posDiff = targetPose.pos.sub(currentPose.pos);
-            double yawDiff = targetPose.yaw - currentPose.yaw;
+            double yawDiff = modAngle(targetPose.yaw - currentPose.yaw);
             int liftDiff = targetPose.liftPos - currentPose.liftPos;
 
             if (