├── LICENSE.md ├── README.md ├── SensorSparkFunOTOS.java └── SparkFunOTOS.java /LICENSE.md: -------------------------------------------------------------------------------- 1 | SparkFun License Information 2 | ============================ 3 | 4 | SparkFun uses two different licenses for our files — one for hardware and one for code. 5 | 6 | Hardware 7 | --------- 8 | 9 | **SparkFun hardware is released under [Creative Commons Share-alike 4.0 International](http://creativecommons.org/licenses/by-sa/4.0/).** 10 | 11 | Note: This is a human-readable summary of (and not a substitute for) the [license](http://creativecommons.org/licenses/by-sa/4.0/legalcode). 12 | 13 | You are free to: 14 | 15 | Share — copy and redistribute the material in any medium or format 16 | Adapt — remix, transform, and build upon the material 17 | for any purpose, even commercially. 18 | The licensor cannot revoke these freedoms as long as you follow the license terms. 19 | Under the following terms: 20 | 21 | Attribution — You must give appropriate credit, provide a link to the license, and indicate if changes were made. You may do so in any reasonable manner, but not in any way that suggests the licensor endorses you or your use. 22 | ShareAlike — If you remix, transform, or build upon the material, you must distribute your contributions under the same license as the original. 23 | No additional restrictions — You may not apply legal terms or technological measures that legally restrict others from doing anything the license permits. 24 | Notices: 25 | 26 | You do not have to comply with the license for elements of the material in the public domain or where your use is permitted by an applicable exception or limitation. 27 | No warranties are given. The license may not give you all of the permissions necessary for your intended use. For example, other rights such as publicity, privacy, or moral rights may limit how you use the material. 28 | 29 | 30 | Code 31 | -------- 32 | Library contains code from multiple sources, see top of each file for specific licensing information. 33 | 34 | **SparkFun code, firmware, and software is released under the MIT License(http://opensource.org/licenses/MIT).** 35 | 36 | The MIT License (MIT) 37 | 38 | Copyright (c) 2020 SparkFun Electronics 39 | 40 | Permission is hereby granted, free of charge, to any person obtaining a copy 41 | of this software and associated documentation files (the "Software"), to deal 42 | in the Software without restriction, including without limitation the rights 43 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 44 | copies of the Software, and to permit persons to whom the Software is 45 | furnished to do so, subject to the following conditions: 46 | 47 | The above copyright notice and this permission notice shall be included in all 48 | copies or substantial portions of the Software. 49 | 50 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 51 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 52 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 53 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 54 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 55 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 56 | SOFTWARE. 57 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SparkFun Optical Tracking Odometry Sensor FTC Java Library 2 | ======================================== 3 | 4 | 5 | [![SparkFun Optical Tracking Odometry Sensor](https://cdn.sparkfun.com/assets/parts/2/5/2/0/9/SEN-24904-Optical-Tracking-Odometry-Sensor-Feature.jpg)](https://www.sparkfun.com/products/24904) 6 | 7 | [*SparkFun Optical Tracking Odometry Sensor [SEN-24904]*](https://www.sparkfun.com/products/24904) 8 | 9 | FTC Java Library for the SparkFun Optical Tracking Odometry Sensor. This has been integrated into the [FTC SDK](https://github.com/FIRST-Tech-Challenge/FtcRobotController), so you can ignore this repository unless you know what you are doing! 10 | 11 | Repository Contents 12 | ------------------- 13 | 14 | * **SparkFunOTOS.java** - Java driver for the FTC control system. 15 | * **SensorSparkFunOTOS.java** - Sample OpMode that demonstrates basic usage of the Optical Tracking Odometry Sensor. 16 | 17 | Documentation 18 | -------------- 19 | * **[GitHub Repo](https://github.com/sparkfun/SparkFun_Optical_Tracking_Odometry_Sensor)** - Hardware repository for the SparkFun Optical Tracking Odometry Sensor 20 | * **[Hookup Guide](https://docs.sparkfun.com/SparkFun_Optical_Tracking_Odometry_Sensor)** - Hookup Guide for the SparkFun Optical Tracking Odometry Sensor 21 | 22 | License Information 23 | ------------------- 24 | 25 | This product is _**open source**_! 26 | 27 | Please review the LICENSE.md file for license information. 28 | 29 | If you have any questions or concerns on licensing, please contact technical support on our [SparkFun forums](https://forum.sparkfun.com/viewforum.php?f=152). 30 | 31 | Distributed as-is; no warranty is given. 32 | 33 | - Your friends at SparkFun. 34 | 35 | __ 36 | -------------------------------------------------------------------------------- /SensorSparkFunOTOS.java: -------------------------------------------------------------------------------- 1 | /* 2 | SPDX-License-Identifier: MIT 3 | 4 | Copyright (c) 2024 SparkFun Electronics 5 | */ 6 | package org.firstinspires.ftc.robotcontroller.external.samples; 7 | 8 | import com.qualcomm.robotcore.eventloop.opmode.Disabled; 9 | import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; 10 | import com.qualcomm.robotcore.eventloop.opmode.TeleOp; 11 | 12 | import com.qualcomm.hardware.sparkfun.SparkFunOTOS; 13 | 14 | import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; 15 | import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; 16 | 17 | /* 18 | * This OpMode illustrates how to use the SparkFun Qwiic Optical Tracking Odometry Sensor (OTOS) 19 | * 20 | * The OpMode assumes that the sensor is configured with a name of "sensor_otos". 21 | * 22 | * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. 23 | * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list 24 | * 25 | * See the sensor's product page: https://www.sparkfun.com/products/24904 26 | */ 27 | @TeleOp(name = "Sensor: SparkFun OTOS", group = "Sensor") 28 | @Disabled 29 | public class SensorSparkFunOTOS extends LinearOpMode { 30 | // Create an instance of the sensor 31 | SparkFunOTOS myOtos; 32 | 33 | @Override 34 | public void runOpMode() throws InterruptedException { 35 | // Get a reference to the sensor 36 | myOtos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos"); 37 | 38 | // All the configuration for the OTOS is done in this helper method, check it out! 39 | configureOtos(); 40 | 41 | // Wait for the start button to be pressed 42 | waitForStart(); 43 | 44 | // Loop until the OpMode ends 45 | while (opModeIsActive()) { 46 | // Get the latest position, which includes the x and y coordinates, plus the 47 | // heading angle 48 | SparkFunOTOS.Pose2D pos = myOtos.getPosition(); 49 | 50 | // Reset the tracking if the user requests it 51 | if (gamepad1.y) { 52 | myOtos.resetTracking(); 53 | } 54 | 55 | // Re-calibrate the IMU if the user requests it 56 | if (gamepad1.x) { 57 | myOtos.calibrateImu(); 58 | } 59 | 60 | // Inform user of available controls 61 | telemetry.addLine("Press Y (triangle) on Gamepad to reset tracking"); 62 | telemetry.addLine("Press X (square) on Gamepad to calibrate the IMU"); 63 | telemetry.addLine(); 64 | 65 | // Log the position to the telemetry 66 | telemetry.addData("X coordinate", pos.x); 67 | telemetry.addData("Y coordinate", pos.y); 68 | telemetry.addData("Heading angle", pos.h); 69 | 70 | // Update the telemetry on the driver station 71 | telemetry.update(); 72 | } 73 | } 74 | 75 | private void configureOtos() { 76 | telemetry.addLine("Configuring OTOS..."); 77 | telemetry.update(); 78 | 79 | // Set the desired units for linear and angular measurements. Can be either 80 | // meters or inches for linear, and radians or degrees for angular. If not 81 | // set, the default is inches and degrees. Note that this setting is not 82 | // persisted in the sensor, so you need to set at the start of all your 83 | // OpModes if using the non-default value. 84 | // myOtos.setLinearUnit(DistanceUnit.METER); 85 | myOtos.setLinearUnit(DistanceUnit.INCH); 86 | // myOtos.setAngularUnit(AnguleUnit.RADIANS); 87 | myOtos.setAngularUnit(AngleUnit.DEGREES); 88 | 89 | // Assuming you've mounted your sensor to a robot and it's not centered, 90 | // you can specify the offset for the sensor relative to the center of the 91 | // robot. The units default to inches and degrees, but if you want to use 92 | // different units, specify them before setting the offset! Note that as of 93 | // firmware version 1.0, these values will be lost after a power cycle, so 94 | // you will need to set them each time you power up the sensor. For example, if 95 | // the sensor is mounted 5 inches to the left (negative X) and 10 inches 96 | // forward (positive Y) of the center of the robot, and mounted 90 degrees 97 | // clockwise (negative rotation) from the robot's orientation, the offset 98 | // would be {-5, 10, -90}. These can be any value, even the angle can be 99 | // tweaked slightly to compensate for imperfect mounting (eg. 1.3 degrees). 100 | SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0, 0, 0); 101 | myOtos.setOffset(offset); 102 | 103 | // Here we can set the linear and angular scalars, which can compensate for 104 | // scaling issues with the sensor measurements. Note that as of firmware 105 | // version 1.0, these values will be lost after a power cycle, so you will 106 | // need to set them each time you power up the sensor. They can be any value 107 | // from 0.872 to 1.127 in increments of 0.001 (0.1%). It is recommended to 108 | // first set both scalars to 1.0, then calibrate the angular scalar, then 109 | // the linear scalar. To calibrate the angular scalar, spin the robot by 110 | // multiple rotations (eg. 10) to get a precise error, then set the scalar 111 | // to the inverse of the error. Remember that the angle wraps from -180 to 112 | // 180 degrees, so for example, if after 10 rotations counterclockwise 113 | // (positive rotation), the sensor reports -15 degrees, the required scalar 114 | // would be 3600/3585 = 1.004. To calibrate the linear scalar, move the 115 | // robot a known distance and measure the error; do this multiple times at 116 | // multiple speeds to get an average, then set the linear scalar to the 117 | // inverse of the error. For example, if you move the robot 100 inches and 118 | // the sensor reports 103 inches, set the linear scalar to 100/103 = 0.971 119 | myOtos.setLinearScalar(1.0); 120 | myOtos.setAngularScalar(1.0); 121 | 122 | // The IMU on the OTOS includes a gyroscope and accelerometer, which could 123 | // have an offset. Note that as of firmware version 1.0, the calibration 124 | // will be lost after a power cycle; the OTOS performs a quick calibration 125 | // when it powers up, but it is recommended to perform a more thorough 126 | // calibration at the start of all your OpModes. Note that the sensor must 127 | // be completely stationary and flat during calibration! When calling 128 | // calibrateImu(), you can specify the number of samples to take and whether 129 | // to wait until the calibration is complete. If no parameters are provided, 130 | // it will take 255 samples and wait until done; each sample takes about 131 | // 2.4ms, so about 612ms total 132 | myOtos.calibrateImu(); 133 | 134 | // Reset the tracking algorithm - this resets the position to the origin, 135 | // but can also be used to recover from some rare tracking errors 136 | myOtos.resetTracking(); 137 | 138 | // After resetting the tracking, the OTOS will report that the robot is at 139 | // the origin. If your robot does not start at the origin, or you have 140 | // another source of location information (eg. vision odometry), you can set 141 | // the OTOS location to match and it will continue to track from there. 142 | SparkFunOTOS.Pose2D currentPosition = new SparkFunOTOS.Pose2D(0, 0, 0); 143 | myOtos.setPosition(currentPosition); 144 | 145 | // Get the hardware and firmware version 146 | SparkFunOTOS.Version hwVersion = new SparkFunOTOS.Version(); 147 | SparkFunOTOS.Version fwVersion = new SparkFunOTOS.Version(); 148 | myOtos.getVersionInfo(hwVersion, fwVersion); 149 | 150 | telemetry.addLine("OTOS configured! Press start to get position data!"); 151 | telemetry.addLine(); 152 | telemetry.addLine(String.format("OTOS Hardware Version: v%d.%d", hwVersion.major, hwVersion.minor)); 153 | telemetry.addLine(String.format("OTOS Firmware Version: v%d.%d", fwVersion.major, fwVersion.minor)); 154 | telemetry.update(); 155 | } 156 | } 157 | -------------------------------------------------------------------------------- /SparkFunOTOS.java: -------------------------------------------------------------------------------- 1 | /* 2 | SPDX-License-Identifier: MIT 3 | 4 | Copyright (c) 2024 SparkFun Electronics 5 | */ 6 | package com.qualcomm.hardware.sparkfun; 7 | 8 | import java.nio.ByteBuffer; 9 | import java.util.Arrays; 10 | 11 | import com.qualcomm.robotcore.hardware.I2cAddr; 12 | import com.qualcomm.robotcore.hardware.I2cDeviceSynch; 13 | import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; 14 | import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; 15 | import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; 16 | import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; 17 | 18 | import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; 19 | import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; 20 | 21 | /** 22 | * {@link SparkFunOTOS} is the Java driver for the SparkFun Qwiic Optical Tracking Odometry Sensor 23 | * (OTOS). This is a port of the Arduino library. 24 | * 25 | * @see SparkFun OTOS Product Page 26 | * @see Arduino Library 27 | */ 28 | @I2cDeviceType 29 | @DeviceProperties( 30 | name = "SparkFun OTOS", 31 | xmlTag = "SparkFunOTOS", 32 | description = "SparkFun Qwiic Optical Tracking Odometry Sensor" 33 | ) 34 | public class SparkFunOTOS extends I2cDeviceSynchDevice { 35 | // Default I2C addresses of the Qwiic OTOS 36 | public static final byte DEFAULT_ADDRESS = 0x17; 37 | // Minimum scalar value for the linear and angular scalars 38 | public static final double MIN_SCALAR = 0.872; 39 | 40 | // Maximum scalar value for the linear and angular scalars 41 | public static final double MAX_SCALAR = 1.127; 42 | 43 | // OTOS register map 44 | protected static final byte REG_PRODUCT_ID = 0x00; 45 | protected static final byte REG_HW_VERSION = 0x01; 46 | protected static final byte REG_FW_VERSION = 0x02; 47 | protected static final byte REG_SCALAR_LINEAR = 0x04; 48 | protected static final byte REG_SCALAR_ANGULAR = 0x05; 49 | protected static final byte REG_IMU_CALIB = 0x06; 50 | protected static final byte REG_RESET = 0x07; 51 | protected static final byte REG_SIGNAL_PROCESS = 0x0E; 52 | protected static final byte REG_SELF_TEST = 0x0F; 53 | protected static final byte REG_OFF_XL = 0x10; 54 | protected static final byte REG_OFF_XH = 0x11; 55 | protected static final byte REG_OFF_YL = 0x12; 56 | protected static final byte REG_OFF_YH = 0x13; 57 | protected static final byte REG_OFF_HL = 0x14; 58 | protected static final byte REG_OFF_HH = 0x15; 59 | protected static final byte REG_STATUS = 0x1F; 60 | protected static final byte REG_POS_XL = 0x20; 61 | protected static final byte REG_POS_XH = 0x21; 62 | protected static final byte REG_POS_YL = 0x22; 63 | protected static final byte REG_POS_YH = 0x23; 64 | protected static final byte REG_POS_HL = 0x24; 65 | protected static final byte REG_POS_HH = 0x25; 66 | protected static final byte REG_VEL_XL = 0x26; 67 | protected static final byte REG_VEL_XH = 0x27; 68 | protected static final byte REG_VEL_YL = 0x28; 69 | protected static final byte REG_VEL_YH = 0x29; 70 | protected static final byte REG_VEL_HL = 0x2A; 71 | protected static final byte REG_VEL_HH = 0x2B; 72 | protected static final byte REG_ACC_XL = 0x2C; 73 | protected static final byte REG_ACC_XH = 0x2D; 74 | protected static final byte REG_ACC_YL = 0x2E; 75 | protected static final byte REG_ACC_YH = 0x2F; 76 | protected static final byte REG_ACC_HL = 0x30; 77 | protected static final byte REG_ACC_HH = 0x31; 78 | protected static final byte REG_POS_STD_XL = 0x32; 79 | protected static final byte REG_POS_STD_XH = 0x33; 80 | protected static final byte REG_POS_STD_YL = 0x34; 81 | protected static final byte REG_POS_STD_YH = 0x35; 82 | protected static final byte REG_POS_STD_HL = 0x36; 83 | protected static final byte REG_POS_STD_HH = 0x37; 84 | protected static final byte REG_VEL_STD_XL = 0x38; 85 | protected static final byte REG_VEL_STD_XH = 0x39; 86 | protected static final byte REG_VEL_STD_YL = 0x3A; 87 | protected static final byte REG_VEL_STD_YH = 0x3B; 88 | protected static final byte REG_VEL_STD_HL = 0x3C; 89 | protected static final byte REG_VEL_STD_HH = 0x3D; 90 | protected static final byte REG_ACC_STD_XL = 0x3E; 91 | protected static final byte REG_ACC_STD_XH = 0x3F; 92 | protected static final byte REG_ACC_STD_YL = 0x40; 93 | protected static final byte REG_ACC_STD_YH = 0x41; 94 | protected static final byte REG_ACC_STD_HL = 0x42; 95 | protected static final byte REG_ACC_STD_HH = 0x43; 96 | 97 | // Product ID register value 98 | protected static final byte PRODUCT_ID = 0x5F; 99 | 100 | // Conversion factors 101 | protected static final double RADIAN_TO_DEGREE = 180.0 / Math.PI; 102 | protected static final double DEGREE_TO_RADIAN = Math.PI / 180.0; 103 | 104 | // Conversion factor for the linear position registers. 16-bit signed 105 | // registers with a max value of 10 meters (394 inches) gives a resolution 106 | // of about 0.0003 mps (0.012 ips) 107 | protected static final double METER_TO_INT16 = 32768.0 / 10.0; 108 | protected static final double INT16_TO_METER = 1.0 / METER_TO_INT16; 109 | 110 | // Conversion factor for the linear velocity registers. 16-bit signed 111 | // registers with a max value of 5 mps (197 ips) gives a resolution of about 112 | // 0.00015 mps (0.006 ips) 113 | protected static final double MPS_TO_INT16 = 32768.0 / 5.0; 114 | protected static final double INT16_TO_MPS = 1.0 / MPS_TO_INT16; 115 | 116 | // Conversion factor for the linear acceleration registers. 16-bit signed 117 | // registers with a max value of 157 mps^2 (16 g) gives a resolution of 118 | // about 0.0048 mps^2 (0.49 mg) 119 | protected static final double MPSS_TO_INT16 = 32768.0 / (16.0 * 9.80665); 120 | protected static final double INT16_TO_MPSS = 1.0 / MPSS_TO_INT16; 121 | 122 | // Conversion factor for the angular position registers. 16-bit signed 123 | // registers with a max value of pi radians (180 degrees) gives a resolution 124 | // of about 0.00096 radians (0.0055 degrees) 125 | protected static final double RAD_TO_INT16 = 32768.0 / Math.PI; 126 | protected static final double INT16_TO_RAD = 1.0 / RAD_TO_INT16; 127 | 128 | // Conversion factor for the angular velocity registers. 16-bit signed 129 | // registers with a max value of 34.9 rps (2000 dps) gives a resolution of 130 | // about 0.0011 rps (0.061 degrees per second) 131 | protected static final double RPS_TO_INT16 = 32768.0 / (2000.0 * DEGREE_TO_RADIAN); 132 | protected static final double INT16_TO_RPS = 1.0 / RPS_TO_INT16; 133 | 134 | // Conversion factor for the angular acceleration registers. 16-bit signed 135 | // registers with a max value of 3141 rps^2 (180000 dps^2) gives a 136 | // resolution of about 0.096 rps^2 (5.5 dps^2) 137 | protected static final double RPSS_TO_INT16 = 32768.0 / (Math.PI * 1000.0); 138 | protected static final double INT16_TO_RPSS = 1.0 / RPSS_TO_INT16; 139 | 140 | // 2D pose structure, including x and y coordinates and heading angle. 141 | // Although pose is traditionally used for position and orientation, this 142 | // structure is also used for velocity and accleration by the OTOS driver 143 | public static class Pose2D { 144 | public double x; 145 | public double y; 146 | public double h; 147 | 148 | public Pose2D() { 149 | x = 0.0; 150 | y = 0.0; 151 | h = 0.0; 152 | } 153 | 154 | public Pose2D(double x, double y, double h) { 155 | this.x = x; 156 | this.y = y; 157 | this.h = h; 158 | } 159 | 160 | public void set(Pose2D pose) { 161 | this.x = pose.x; 162 | this.y = pose.y; 163 | this.h = pose.h; 164 | } 165 | } 166 | 167 | // Version register structure 168 | public static class Version { 169 | public byte minor; 170 | public byte major; 171 | 172 | public Version() { 173 | set((byte) 0); 174 | } 175 | 176 | public Version(byte value) { 177 | set(value); 178 | } 179 | 180 | public void set(byte value) { 181 | minor = (byte) (value & 0x0F); 182 | major = (byte) ((value >> 4) & 0x0F); 183 | } 184 | 185 | public byte get() { 186 | return (byte) ((major << 4) | minor); 187 | } 188 | } 189 | 190 | // Signal process config register structure 191 | public static class SignalProcessConfig { 192 | public boolean enLut; 193 | public boolean enAcc; 194 | public boolean enRot; 195 | public boolean enVar; 196 | 197 | public SignalProcessConfig() { 198 | set((byte) 0); 199 | } 200 | 201 | public SignalProcessConfig(byte value) { 202 | set(value); 203 | } 204 | 205 | public void set(byte value) { 206 | enLut = (value & 0x01) != 0; 207 | enAcc = (value & 0x02) != 0; 208 | enRot = (value & 0x04) != 0; 209 | enVar = (value & 0x08) != 0; 210 | } 211 | 212 | public byte get() { 213 | return (byte) ((enLut ? 0x01 : 0) | (enAcc ? 0x02 : 0) | (enRot ? 0x04 : 0) | (enVar ? 0x08 : 0)); 214 | } 215 | } 216 | 217 | // Self-test config register structure 218 | public static class SelfTestConfig { 219 | public boolean start; 220 | public boolean inProgress; 221 | public boolean pass; 222 | public boolean fail; 223 | 224 | public SelfTestConfig() { 225 | set((byte) 0); 226 | } 227 | 228 | public SelfTestConfig(byte value) { 229 | set(value); 230 | } 231 | 232 | public void set(byte value) { 233 | start = (value & 0x01) != 0; 234 | inProgress = (value & 0x02) != 0; 235 | pass = (value & 0x04) != 0; 236 | fail = (value & 0x08) != 0; 237 | } 238 | 239 | public byte get() { 240 | return (byte) ((start ? 0x01 : 0) | (inProgress ? 0x02 : 0) | (pass ? 0x04 : 0) | (fail ? 0x08 : 0)); 241 | } 242 | } 243 | 244 | // Status register structure 245 | public static class Status { 246 | public boolean warnTiltAngle; 247 | public boolean warnOpticalTracking; 248 | public boolean errorPaa; 249 | public boolean errorLsm; 250 | 251 | public Status() { 252 | set((byte) 0); 253 | } 254 | 255 | public Status(byte value) { 256 | set(value); 257 | } 258 | 259 | public void set(byte value) { 260 | warnTiltAngle = (value & 0x01) != 0; 261 | warnOpticalTracking = (value & 0x02) != 0; 262 | errorPaa = (value & 0x40) != 0; 263 | errorLsm = (value & 0x80) != 0; 264 | } 265 | 266 | public byte get() { 267 | return (byte) ((warnTiltAngle ? 0x01 : 0) | (warnOpticalTracking ? 0x02 : 0) | (errorPaa ? 0x40 : 0) | (errorLsm ? 0x80 : 0)); 268 | } 269 | } 270 | 271 | protected DistanceUnit _distanceUnit; 272 | protected AngleUnit _angularUnit; 273 | 274 | public SparkFunOTOS(I2cDeviceSynch deviceClient) 275 | { 276 | // Initialize the base class 277 | super(deviceClient, true); 278 | 279 | // Set the I2C address to the default 280 | deviceClient.setI2cAddress(I2cAddr.create7bit(DEFAULT_ADDRESS)); 281 | 282 | super.registerArmingStateCallback(false); 283 | this.deviceClient.engage(); 284 | } 285 | 286 | @Override 287 | protected boolean doInitialize() 288 | { 289 | // Set default units to inches and degrees 290 | _distanceUnit = DistanceUnit.INCH; 291 | _angularUnit = AngleUnit.DEGREES; 292 | 293 | // Check if the device is connected 294 | return isConnected(); 295 | } 296 | 297 | @Override 298 | public Manufacturer getManufacturer() 299 | { 300 | return Manufacturer.SparkFun; 301 | } 302 | 303 | @Override 304 | public String getDeviceName() 305 | { 306 | return "SparkFun Qwiic Optical Tracking Odometry Sensor"; 307 | } 308 | 309 | /** 310 | * Begins the Qwiic OTOS and verifies it is connected 311 | * @return True if successful, false otherwise 312 | */ 313 | public boolean begin() { 314 | // Just check if the device is connected, no other setup is needed 315 | return isConnected(); 316 | } 317 | 318 | /** 319 | * Checks if the OTOS is connected to the I2C bus 320 | * @return True if the OTOS is connected, false otherwise 321 | */ 322 | public boolean isConnected() { 323 | // Read the product ID 324 | byte prodId = deviceClient.read8(REG_PRODUCT_ID); 325 | 326 | // Check if the product ID is correct 327 | if (prodId != PRODUCT_ID) 328 | return false; 329 | 330 | // Everything checks out, we must be connected! 331 | return true; 332 | } 333 | 334 | /** 335 | * Gets the hardware and firmware version of the OTOS 336 | * @param hwVersion Hardware version number 337 | * @param fwVersion Firmware version number 338 | */ 339 | public void getVersionInfo(Version hwVersion, Version fwVersion) { 340 | // Read hardware and firmware version registers 341 | byte[] rawData = deviceClient.read(REG_HW_VERSION, 2); 342 | 343 | // Store the version info 344 | hwVersion.set(rawData[0]); 345 | fwVersion.set(rawData[1]); 346 | } 347 | 348 | /** 349 | * Performs a self-test on the OTOS 350 | * @return True if the self-test passed, false otherwise 351 | */ 352 | public boolean selfTest() { 353 | // Write the self-test register to start the test 354 | SelfTestConfig selfTest = new SelfTestConfig(); 355 | selfTest.set((byte) 1); 356 | deviceClient.write8(REG_SELF_TEST, selfTest.get()); 357 | 358 | // Loop until self-test is done, should only take ~20ms as of firmware v1.0 359 | for (int i = 0; i < 10; i++) { 360 | // Give a short delay between reads 361 | try { 362 | Thread.sleep(5); 363 | } catch (InterruptedException e) { 364 | Thread.currentThread().interrupt(); 365 | return false; 366 | } 367 | 368 | // Read the self-test register 369 | selfTest.set(deviceClient.read8(REG_SELF_TEST)); 370 | 371 | // Check if the self-test is done 372 | if (selfTest.inProgress == false) { 373 | break; 374 | } 375 | } 376 | 377 | // Check if the self-test passed 378 | return selfTest.pass; 379 | } 380 | 381 | /** 382 | * Calibrates the IMU on the OTOS, which removes the accelerometer and 383 | * gyroscope offsets. This will do the full 255 samples and wait until 384 | * the calibration is done, which takes about 612ms as of firmware v1.0 385 | * @return True if the calibration was successful, false otherwise 386 | */ 387 | public boolean calibrateImu() { 388 | return calibrateImu(255, true); 389 | } 390 | 391 | /** 392 | * Calibrates the IMU on the OTOS, which removes the accelerometer and 393 | * gyroscope offsets 394 | * @param numSamples Number of samples to take for calibration. Each sample 395 | * takes about 2.4ms, so fewer samples can be taken for faster calibration 396 | * @param waitUntilDone Whether to wait until the calibration is complete. 397 | * Set false to calibrate asynchronously, see getImuCalibrationProgress() 398 | * @return True if the calibration was successful, false otherwise 399 | */ 400 | public boolean calibrateImu(int numSamples, boolean waitUntilDone) { 401 | // Check if the number of samples is out of bounds 402 | if (numSamples < 1 || numSamples > 255) 403 | return false; 404 | 405 | // Write the number of samples to the device 406 | deviceClient.write8(REG_IMU_CALIB, numSamples); 407 | 408 | // Wait 1 sample period (2.4ms) to ensure the register updates 409 | try { 410 | Thread.sleep(3); 411 | } catch (InterruptedException e) { 412 | Thread.currentThread().interrupt(); 413 | return false; 414 | } 415 | 416 | // Do we need to wait until the calibration finishes? 417 | if (!waitUntilDone) 418 | return true; 419 | 420 | // Wait for the calibration to finish, which is indicated by the IMU 421 | // calibration register reading zero, or until we reach the maximum number 422 | // of read attempts 423 | for (int numAttempts = numSamples; numAttempts > 0; numAttempts--) { 424 | // Read the gryo calibration register value 425 | byte calibrationValue = deviceClient.read8(REG_IMU_CALIB); 426 | 427 | // Check if calibration is done 428 | if (calibrationValue == 0) 429 | return true; 430 | 431 | // Give a short delay between reads. As of firmware v1.0, samples take 432 | // 2.4ms each, so 3ms should guarantee the next sample is done. This 433 | // also ensures the max attempts is not exceeded in normal operation 434 | try { 435 | Thread.sleep(3); 436 | } catch (InterruptedException e) { 437 | Thread.currentThread().interrupt(); 438 | return false; 439 | } 440 | } 441 | 442 | // Max number of attempts reached, calibration failed 443 | return false; 444 | } 445 | 446 | /** 447 | * Gets the progress of the IMU calibration. Used for asynchronous 448 | * calibration with calibrateImu() 449 | * @return Number of samples remaining for calibration 450 | */ 451 | public int getImuCalibrationProgress() { 452 | // Read the IMU calibration register 453 | return deviceClient.read8(REG_IMU_CALIB) & 0xFF; 454 | } 455 | 456 | /** 457 | * Gets the linear unit used by all methods using a pose 458 | * @return Linear unit 459 | */ 460 | public DistanceUnit getLinearUnit() { 461 | return _distanceUnit; 462 | } 463 | 464 | /** 465 | * Sets the linear unit used by all methods using a pose 466 | * @param unit Linear unit 467 | */ 468 | public void setLinearUnit(DistanceUnit unit) { 469 | // Check if this unit is already set 470 | if (unit == _distanceUnit) 471 | return; 472 | 473 | // Store new unit 474 | _distanceUnit = unit; 475 | } 476 | 477 | /** 478 | * Gets the angular unit used by all methods using a pose 479 | * @return Angular unit 480 | */ 481 | public AngleUnit getAngularUnit() { 482 | return _angularUnit; 483 | } 484 | 485 | /** 486 | * Sets the angular unit used by all methods using a pose 487 | * @param unit Angular unit 488 | */ 489 | public void setAngularUnit(AngleUnit unit) { 490 | // Check if this unit is already set 491 | if (unit == _angularUnit) 492 | return; 493 | 494 | // Store new unit 495 | _angularUnit = unit; 496 | } 497 | 498 | /** 499 | * Gets the linear scalar used by the OTOS 500 | * @return Linear scalar 501 | */ 502 | public double getLinearScalar() { 503 | // Read the linear scalar from the device 504 | byte rawScalar = deviceClient.read8(REG_SCALAR_LINEAR); 505 | 506 | // Convert to double, multiples of 0.1% 507 | return (rawScalar * 0.001) + 1.0; 508 | } 509 | 510 | /** 511 | * Sets the linear scalar used by the OTOS. Can be used to 512 | * compensate for scaling issues with the sensor measurements 513 | * @param scalar Linear scalar, must be between 0.872 and 1.127 514 | * @return True if the scalar was set successfully, false otherwise 515 | */ 516 | public boolean setLinearScalar(double scalar) { 517 | // Check if the scalar is out of bounds 518 | if (scalar < MIN_SCALAR || scalar > MAX_SCALAR) 519 | return false; 520 | 521 | // Convert to integer, multiples of 0.1% (+0.5 to round instead of truncate) 522 | byte rawScalar = (byte) ((scalar - 1.0) * 1000 + 0.5); 523 | 524 | // Write the scalar to the device 525 | deviceClient.write8(REG_SCALAR_LINEAR, rawScalar); 526 | 527 | // Done! 528 | return true; 529 | } 530 | 531 | /** 532 | * Gets the angular scalar used by the OTOS 533 | * @return Angular scalar 534 | */ 535 | public double getAngularScalar() { 536 | // Read the angular scalar from the device 537 | byte rawScalar = deviceClient.read8(REG_SCALAR_ANGULAR); 538 | 539 | // Convert to double, multiples of 0.1% 540 | return (rawScalar * 0.001) + 1.0; 541 | } 542 | 543 | /** 544 | * Sets the angular scalar used by the OTOS. Can be used to 545 | * compensate for scaling issues with the sensor measurements 546 | * @param scalar Angular scalar, must be between 0.872 and 1.127 547 | * @return True if the scalar was set successfully, false otherwise 548 | */ 549 | public boolean setAngularScalar(double scalar) { 550 | // Check if the scalar is out of bounds 551 | if (scalar < MIN_SCALAR || scalar > MAX_SCALAR) 552 | return false; 553 | 554 | // Convert to integer, multiples of 0.1% (+0.5 to round instead of truncate) 555 | byte rawScalar = (byte) ((scalar - 1.0) * 1000 + 0.5); 556 | 557 | // Write the scalar to the device 558 | deviceClient.write8(REG_SCALAR_ANGULAR, rawScalar); 559 | 560 | // Done! 561 | return true; 562 | } 563 | 564 | /** 565 | * Resets the tracking algorithm, which resets the position to the 566 | * origin, but can also be used to recover from some rare tracking errors 567 | */ 568 | public void resetTracking() { 569 | // Set tracking reset bit 570 | deviceClient.write8(REG_RESET, 0x01); 571 | } 572 | 573 | /** 574 | * Gets the signal processing configuration from the OTOS 575 | * @return Signal processing configuration 576 | */ 577 | public SignalProcessConfig getSignalProcessConfig() { 578 | // Read the signal process register 579 | return new SignalProcessConfig(deviceClient.read8(REG_SIGNAL_PROCESS)); 580 | } 581 | 582 | /** 583 | * Sets the signal processing configuration on the OTOS. This is 584 | * primarily useful for creating and testing a new lookup table calibration 585 | * @param config Signal processing configuration 586 | */ 587 | public void setSignalProcessConfig(SignalProcessConfig config) { 588 | // Write the signal process register 589 | deviceClient.write8(REG_SIGNAL_PROCESS, config.get()); 590 | } 591 | 592 | /** 593 | * Gets the status register from the OTOS, which includes warnings 594 | * and errors reported by the sensor 595 | * @return Status register value 596 | */ 597 | public Status getStatus() { 598 | return new Status(deviceClient.read8(REG_STATUS)); 599 | } 600 | 601 | /** 602 | * Gets the offset of the OTOS 603 | * @return Offset of the sensor relative to the center of the robot 604 | */ 605 | public Pose2D getOffset() { 606 | return readPoseRegs(REG_OFF_XL, INT16_TO_METER, INT16_TO_RAD); 607 | } 608 | 609 | /** 610 | * Sets the offset of the OTOS. This is useful if your sensor is 611 | * mounted off-center from a robot. Rather than returning the position of 612 | * the sensor, the OTOS will return the position of the robot 613 | * @param pose Offset of the sensor relative to the center of the robot 614 | */ 615 | public void setOffset(Pose2D pose) { 616 | writePoseRegs(REG_OFF_XL, pose, METER_TO_INT16, RAD_TO_INT16); 617 | } 618 | 619 | /** 620 | * Gets the position measured by the OTOS 621 | * @return Position measured by the OTOS 622 | */ 623 | public Pose2D getPosition() { 624 | return readPoseRegs(REG_POS_XL, INT16_TO_METER, INT16_TO_RAD); 625 | } 626 | 627 | /** 628 | * Sets the position measured by the OTOS. This is useful if your 629 | * robot does not start at the origin, or you have another source of 630 | * location information (eg. vision odometry); the OTOS will continue 631 | * tracking from this position 632 | * @param pose New position for the OTOS to track from 633 | */ 634 | public void setPosition(Pose2D pose) { 635 | writePoseRegs(REG_POS_XL, pose, METER_TO_INT16, RAD_TO_INT16); 636 | } 637 | 638 | /** 639 | * Gets the velocity measured by the OTOS 640 | * @return Velocity measured by the OTOS 641 | */ 642 | public Pose2D getVelocity() { 643 | return readPoseRegs(REG_VEL_XL, INT16_TO_MPS, INT16_TO_RPS); 644 | } 645 | 646 | /** 647 | * Gets the acceleration measured by the OTOS 648 | * @return Acceleration measured by the OTOS 649 | */ 650 | public Pose2D getAcceleration() { 651 | return readPoseRegs(REG_ACC_XL, INT16_TO_MPSS, INT16_TO_RPSS); 652 | } 653 | 654 | /** 655 | * Gets the standard deviation of the measured position 656 | * These values are just the square root of the diagonal elements 657 | * of the covariance matrices of the Kalman filters used in the firmware, so 658 | * they are just statistical quantities and do not represent actual error! 659 | * @return Standard deviation of the position measured by the OTOS 660 | */ 661 | public Pose2D getPositionStdDev() { 662 | return readPoseRegs(REG_POS_STD_XL, INT16_TO_METER, INT16_TO_RAD); 663 | } 664 | 665 | /** 666 | * Gets the standard deviation of the measured velocity 667 | * These values are just the square root of the diagonal elements 668 | * of the covariance matrices of the Kalman filters used in the firmware, so 669 | * they are just statistical quantities and do not represent actual error! 670 | * @return Standard deviation of the velocity measured by the OTOS 671 | */ 672 | public Pose2D getVelocityStdDev() { 673 | return readPoseRegs(REG_VEL_STD_XL, INT16_TO_MPS, INT16_TO_RPS); 674 | } 675 | 676 | /** 677 | * Gets the standard deviation of the measured acceleration 678 | * These values are just the square root of the diagonal elements 679 | * of the covariance matrices of the Kalman filters used in the firmware, so 680 | * they are just statistical quantities and do not represent actual error! 681 | * @return Standard deviation of the acceleration measured by the OTOS 682 | */ 683 | public Pose2D getAccelerationStdDev() { 684 | return readPoseRegs(REG_ACC_STD_XL, INT16_TO_MPSS, INT16_TO_RPSS); 685 | } 686 | 687 | /** 688 | * Gets the position, velocity, and acceleration measured by the 689 | * OTOS in a single burst read 690 | * @param pos Position measured by the OTOS 691 | * @param vel Velocity measured by the OTOS 692 | * @param acc Acceleration measured by the OTOS 693 | */ 694 | public void getPosVelAcc(Pose2D pos, Pose2D vel, Pose2D acc) { 695 | // Read all pose registers 696 | byte[] rawData = deviceClient.read(REG_POS_XL, 18); 697 | 698 | // Convert raw data to pose units 699 | pos.set(regsToPose(Arrays.copyOfRange(rawData, 0, 6), INT16_TO_METER, INT16_TO_RAD)); 700 | vel.set(regsToPose(Arrays.copyOfRange(rawData, 6, 12), INT16_TO_MPS, INT16_TO_RPS)); 701 | acc.set(regsToPose(Arrays.copyOfRange(rawData, 12, 18), INT16_TO_MPSS, INT16_TO_RPSS)); 702 | } 703 | 704 | /** 705 | * Gets the standard deviation of the measured position, velocity, 706 | * and acceleration in a single burst read 707 | * @param pos Standard deviation of the position measured by the OTOS 708 | * @param vel Standard deviation of the velocity measured by the OTOS 709 | * @param acc Standard deviation of the acceleration measured by the OTOS 710 | */ 711 | public void getPosVelAccStdDev(Pose2D pos, Pose2D vel, Pose2D acc) { 712 | // Read all pose registers 713 | byte[] rawData = deviceClient.read(REG_POS_STD_XL, 18); 714 | 715 | // Convert raw data to pose units 716 | pos.set(regsToPose(Arrays.copyOfRange(rawData, 0, 6), INT16_TO_METER, INT16_TO_RAD)); 717 | vel.set(regsToPose(Arrays.copyOfRange(rawData, 6, 12), INT16_TO_MPS, INT16_TO_RPS)); 718 | acc.set(regsToPose(Arrays.copyOfRange(rawData, 12, 18), INT16_TO_MPSS, INT16_TO_RPSS)); 719 | } 720 | 721 | /** 722 | * Gets the position, velocity, acceleration, and standard deviation 723 | * of each in a single burst read 724 | * @param pos Position measured by the OTOS 725 | * @param vel Velocity measured by the OTOS 726 | * @param acc Acceleration measured by the OTOS 727 | * @param posStdDev Standard deviation of the position measured by the OTOS 728 | * @param velStdDev Standard deviation of the velocity measured by the OTOS 729 | * @param accStdDev Standard deviation of the acceleration measured by the OTOS 730 | */ 731 | public void getPosVelAccAndStdDev(Pose2D pos, Pose2D vel, Pose2D acc, 732 | Pose2D posStdDev, Pose2D velStdDev, Pose2D accStdDev) { 733 | // Read all pose registers 734 | byte[] rawData = deviceClient.read(REG_POS_XL, 36); 735 | 736 | // Convert raw data to pose units 737 | pos.set(regsToPose(Arrays.copyOfRange(rawData, 0, 6), INT16_TO_METER, INT16_TO_RAD)); 738 | vel.set(regsToPose(Arrays.copyOfRange(rawData, 6, 12), INT16_TO_MPS, INT16_TO_RPS)); 739 | acc.set(regsToPose(Arrays.copyOfRange(rawData, 12, 18), INT16_TO_MPSS, INT16_TO_RPSS)); 740 | posStdDev.set(regsToPose(Arrays.copyOfRange(rawData, 18, 24), INT16_TO_METER, INT16_TO_RAD)); 741 | velStdDev.set(regsToPose(Arrays.copyOfRange(rawData, 24, 30), INT16_TO_MPS, INT16_TO_RPS)); 742 | accStdDev.set(regsToPose(Arrays.copyOfRange(rawData, 30, 36), INT16_TO_MPSS, INT16_TO_RPSS)); 743 | } 744 | 745 | // Function to read raw pose registers and convert to specified units 746 | protected Pose2D readPoseRegs(byte reg, double rawToXY, double rawToH) { 747 | // Attempt to read the raw pose data 748 | byte[] rawData = deviceClient.read(reg, 6); 749 | 750 | return regsToPose(rawData, rawToXY, rawToH); 751 | } 752 | 753 | // Function to write raw pose registers and convert from specified units 754 | protected void writePoseRegs(byte reg, Pose2D pose, double xyToRaw, double hToRaw) { 755 | // Store raw data in a temporary buffer 756 | byte[] rawData = new byte[6]; 757 | poseToRegs(rawData, pose, xyToRaw, hToRaw); 758 | 759 | // Write the raw data to the device 760 | deviceClient.write(reg, rawData); 761 | } 762 | 763 | // Function to convert raw pose registers to a pose structure 764 | protected Pose2D regsToPose(byte[] rawData, double rawToXY, double rawToH) { 765 | // Wrap raw data in a buffer to handle endianness and signed values 766 | ByteBuffer data = ByteBuffer.wrap(rawData); 767 | data.order(java.nio.ByteOrder.LITTLE_ENDIAN); 768 | 769 | // Store raw data 770 | short rawX = data.getShort(0); 771 | short rawY = data.getShort(2); 772 | short rawH = data.getShort(4); 773 | 774 | // Store in pose and convert to units 775 | Pose2D pose = new Pose2D(); 776 | pose.x = _distanceUnit.fromMeters(rawX * rawToXY); 777 | pose.y = _distanceUnit.fromMeters(rawY * rawToXY); 778 | pose.h = _angularUnit.fromRadians(rawH * rawToH); 779 | 780 | return pose; 781 | } 782 | 783 | // Function to convert a pose structure to raw pose registers 784 | protected void poseToRegs(byte[] rawData, Pose2D pose, double xyToRaw, double hToRaw) { 785 | // Convert pose units to raw data 786 | short rawX = (short) _distanceUnit.toMeters(pose.x * xyToRaw); 787 | short rawY = (short) _distanceUnit.toMeters(pose.y * xyToRaw); 788 | short rawH = (short) _angularUnit.toRadians(pose.h * hToRaw); 789 | 790 | // Store raw data in buffer 791 | rawData[0] = (byte) (rawX & 0xFF); 792 | rawData[1] = (byte) ((rawX >> 8) & 0xFF); 793 | rawData[2] = (byte) (rawY & 0xFF); 794 | rawData[3] = (byte) ((rawY >> 8) & 0xFF); 795 | rawData[4] = (byte) (rawH & 0xFF); 796 | rawData[5] = (byte) ((rawH >> 8) & 0xFF); 797 | } 798 | } 799 | --------------------------------------------------------------------------------