Merge pull request #22 from Logan-Nash/master

Fixed loop times on OTOS localizer. Updated the pinpoint to the lates…
Pedro-Pathing · Dec 10, 2024 · a2d896b · a2d896b
2 parents d34d56d + f494271
commit a2d896b
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 3 deletions.
diff --git a/.../java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java b/.../java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java
@@ -314,9 +314,16 @@ public void setOffsets(double xOffset, double yOffset){
      * @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left
      */
     public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){
+        if (xEncoder == EncoderDirection.FORWARD){
+            writeInt(Register.DEVICE_CONTROL,1<<5);
+        }
         if (xEncoder == EncoderDirection.REVERSED) {
             writeInt(Register.DEVICE_CONTROL,1<<4);
         }
+
+        if (yEncoder == EncoderDirection.FORWARD){
+            writeInt(Register.DEVICE_CONTROL,1<<3);
+        }
         if (yEncoder == EncoderDirection.REVERSED){
             writeInt(Register.DEVICE_CONTROL,1<<2);
         }

diff --git a/...va/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/...va/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java
@@ -108,10 +108,12 @@ public OTOSLocalizer(HardwareMap map, Pose setStartPose) {
      */
     @Override
     public Pose getPose() {
-        SparkFunOTOS.Pose2D rawPose = otos.getPosition();
-        Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h);
+        Pose pose = new Pose(otosPose.x, otosPose.y, otosPose.h);
 
-        return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
+        Vector vec = pose.getVector();
+        vec.rotateVector(startPose.getHeading());
+
+        return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading()));
     }
 
     /**