From 7fc79005a062e71505b561f2e2391cb446e43a16 Mon Sep 17 00:00:00 2001 From: Dryw Wade Date: Sun, 18 Aug 2024 17:39:59 -0600 Subject: [PATCH] Fix setPose() angle wrapping Fixes #1 --- SparkFunOTOS.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/SparkFunOTOS.java b/SparkFunOTOS.java index 0c2c7bd..87c3b8d 100644 --- a/SparkFunOTOS.java +++ b/SparkFunOTOS.java @@ -783,9 +783,9 @@ protected Pose2D regsToPose(byte[] rawData, double rawToXY, double rawToH) { // Function to convert a pose structure to raw pose registers protected void poseToRegs(byte[] rawData, Pose2D pose, double xyToRaw, double hToRaw) { // Convert pose units to raw data - short rawX = (short) _distanceUnit.toMeters(pose.x * xyToRaw); - short rawY = (short) _distanceUnit.toMeters(pose.y * xyToRaw); - short rawH = (short) _angularUnit.toRadians(pose.h * hToRaw); + short rawX = (short) (_distanceUnit.toMeters(pose.x) * xyToRaw); + short rawY = (short) (_distanceUnit.toMeters(pose.y) * xyToRaw); + short rawH = (short) (_angularUnit.toRadians(pose.h) * hToRaw); // Store raw data in buffer rawData[0] = (byte) (rawX & 0xFF);