From f4942715d984666bd8d1c0fb57fdaba1b0cec02b Mon Sep 17 00:00:00 2001 From: Logan-Nash Date: Tue, 10 Dec 2024 10:40:22 -0500 Subject: [PATCH] Fixed loop times on OTOS localizer. Updated the pinpoint to the latest driver --- .../pedroPathing/localization/GoBildaPinpointDriver.java | 7 +++++++ .../localization/localizers/OTOSLocalizer.java | 8 +++++--- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java index a2bc9d85..882ed564 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java +++ b/TeamCode/src/main/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/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java index a700e3b2..0f309a5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java +++ b/TeamCode/src/main/java/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())); } /**