Add examples 2, 3, and 4

Units, calibration, and offset

Author: sfe-SparkFro

examples/Example1_BasicReadings/Example1_BasicReadings.ino

@@ -1,7 +1,7 @@
 #include "SparkFun_Qwiic_OTOS_Arduino_Library.h"
 #include "Wire.h"
 
-// Create an OTOS object
+// Create an Optical Tracking Odometry Sensor object
 QwiicOTOS myOtos;
 
 void setup()
@@ -21,47 +21,36 @@ void setup()
 
     Serial.println("OTOS connected!");
 
-    // The IMU on the OTOS includes a gyroscope and accelerometer, which could
-    // have an offset. This needs to be calibrated out in order to get accurate
-    // tracking, which must be done while the OTOS is stationary and flat!
-    Serial.println("Calibrating IMU...");
-    myOtos.calibrateImu();
-
-    // We'll also reset the tracking to start at the origin
-    Serial.println("Resetting pose...");
-    otos_pose2d_t currentPose = {0, 0, 0};
-    myOtos.setPosition(currentPose);
-
-    myOtos.setLinearUnit(kOtosLinearUnitInches);
-    myOtos.setAngularUnit(kOtosAngularUnitDegrees);
+    // Reset the tracking algorithm, making the sensor report it's at the origin
+    myOtos.resetTracking();
 }
 
 void loop()
 {
-    // Get latest pose from sensor, which includes the x and y coordinates, plus
+    // Get the latest sensor pose, which includes the x and y coordinates, plus
     // the heading angle
     otos_pose2d_t otosPose;
     myOtos.getPosition(otosPose);
 
     // Print measurement
     Serial.println();
-    Serial.println("Current pose:");
-    Serial.print("X (Meters): ");
-    Serial.println(otosPose.x, 3);
-    Serial.print("Y (Meters): ");
-    Serial.println(otosPose.y, 3);
-    Serial.print("Heading (Radians): ");
-    Serial.println(otosPose.h, 3);
+    Serial.println("Sensor pose:");
+    Serial.print("X (Inches): ");
+    Serial.println(otosPose.x);
+    Serial.print("Y (Inches): ");
+    Serial.println(otosPose.y);
+    Serial.print("Heading (Degrees): ");
+    Serial.println(otosPose.h);
 
     // Wait a bit so we don't spam the serial port
     delay(500);
 
     // Alternatively, you can comment out the print and delay code above, and
-    // instead use the following code to refresh the data more quickly
-    // Serial.print(otosPose.x, 3);
-    // Serial.print(", ");
-    // Serial.print(otosPose.y, 3);
-    // Serial.print(", ");
-    // Serial.println(otosPose.h, 3);
+    // instead use the following code to rapidly refresh the data
+    // Serial.print(otosPose.x);
+    // Serial.print("\t");
+    // Serial.print(otosPose.y);
+    // Serial.print("\t");
+    // Serial.println(otosPose.h);
     // delay(10);
 }

examples/Example2_SetUnits/Example2_SetUnits.ino

@@ -0,0 +1,55 @@
+#include "SparkFun_Qwiic_OTOS_Arduino_Library.h"
+#include "Wire.h"
+
+// Create an Optical Tracking Odometry Sensor object
+QwiicOTOS myOtos;
+
+void setup()
+{
+    // Start serial
+    Serial.begin(115200);
+    Serial.println("Qwiic OTOS Example 2 - Set Units");
+
+    Wire.begin();
+
+    // Attempt to begin the sensor
+    while (myOtos.begin() == false)
+    {
+        Serial.println("OTOS not connected, check your wiring and I2C address!");
+        delay(1000);
+    }
+
+    Serial.println("OTOS connected!");
+    
+    // Set the desired units for linear and angular measurements. Can be either
+    // meters or inches for linear, and radians or degrees for angular. If not
+    // set, the default is inches and degrees.
+    myOtos.setLinearUnit(kOtosLinearUnitMeters);
+    // myOtos.setLinearUnit(kOtosLinearUnitInches);
+    myOtos.setAngularUnit(kOtosAngularUnitRadians);
+    // myOtos.setAngularUnit(kOtosAngularUnitDegrees);
+
+    // Reset the tracking algorithm, making the sensor report it's at the origin
+    myOtos.resetTracking();
+}
+
+void loop()
+{
+    // Get the latest sensor pose, which includes the x and y coordinates, plus
+    // the heading angle
+    otos_pose2d_t otosPose;
+    myOtos.getPosition(otosPose);
+
+    // Print measurement
+    Serial.println();
+    Serial.println("Sensor pose:");
+    Serial.print("X (Meters): ");
+    Serial.println(otosPose.x, 4);
+    Serial.print("Y (Meters): ");
+    Serial.println(otosPose.y, 4);
+    Serial.print("Heading (Radians): ");
+    Serial.println(otosPose.h, 4);
+
+    // Wait a bit so we don't spam the serial port
+    delay(500);
+}

examples/Example3_Calibration/Example3_Calibration.ino

@@ -0,0 +1,77 @@
+#include "SparkFun_Qwiic_OTOS_Arduino_Library.h"
+#include "Wire.h"
+
+// Create an Optical Tracking Odometry Sensor object
+QwiicOTOS myOtos;
+
+void setup()
+{
+    // Start serial
+    Serial.begin(115200);
+    Serial.println("Qwiic OTOS Example 3 - Calibration");
+
+    Wire.begin();
+
+    // Attempt to begin the sensor
+    while (myOtos.begin() == false)
+    {
+        Serial.println("OTOS not connected, check your wiring and I2C address!");
+        delay(1000);
+    }
+
+    Serial.println("OTOS connected!");
+    
+    // The IMU on the OTOS includes a gyroscope and accelerometer, which could
+    // have an offset. The OTOS performs a quick calibration when it powers up,
+    // but it is recommended to perform a more thorough calibration at the start
+    // of your programs. Note that the sensor must be completely stationary and
+    // flat during calibration! When calling calibrateImu(), you can specify the
+    // number of samples to take and whether to wait until the calibration is
+    // complete. If no parameters are provided, it will take 255 samples and
+    // wait until done; each sample takes about 1.2ms, so about 306ms total
+    myOtos.calibrateImu();
+    // myOtos.calibrateImu(255, true);
+
+    // Here we can set the linear and angular scalars, which can compensate for
+    // scaling issues with the sensor measurements. Note that as of firmware
+    // version 1.0, these values will be lost after a power cycle, so you will
+    // need to set them each time you power up the sensor. They can be any value
+    // from 0.872 to 1.127 in increments of 0.001 (0.1%). It is recommended to
+    // first set both scalars to 1.0, then calibrate the angular scalar, then
+    // the linear scalar. To calibrate the angular scalar, spin the robot by
+    // multiple rotations (eg. 10) to get a precise error, then set the scalar
+    // to the inverse of the error. Remember that the angle wraps from -180 to
+    // 180 degrees, so for example, if after 10 rotations counterclockwise
+    // (positive rotation), the sensor reports -15 degrees, the required scalar
+    // would be 3600/3585 = 1.004. To calibrate the linear scalar, move the
+    // robot a known distance and measure the error; do this multiple times at
+    // multiple speeds to get an average, then set the linear scalar to the
+    // inverse of the error. For example, if you move the robot 100 inches and
+    // the sensor reports 103 inches, set the linear scalar to 100/103 = 0.971
+    myOtos.setLinearScalar(1.0);
+    myOtos.setAngularScalar(1.0);
+
+    // Reset the tracking algorithm, making the sensor report it's at the origin
+    myOtos.resetTracking();
+}
+
+void loop()
+{
+    // Get the latest sensor pose, which includes the x and y coordinates, plus
+    // the heading angle
+    otos_pose2d_t otosPose;
+    myOtos.getPosition(otosPose);
+
+    // Print measurement
+    Serial.println();
+    Serial.println("Sensor pose:");
+    Serial.print("X (Inches): ");
+    Serial.println(otosPose.x);
+    Serial.print("Y (Inches): ");
+    Serial.println(otosPose.y);
+    Serial.print("Heading (Degrees): ");
+    Serial.println(otosPose.h);
+
+    // Wait a bit so we don't spam the serial port
+    delay(500);
+}

examples/Example4_SetOffsetAndPosition/Example4_SetOffsetAndPosition.ino

@@ -0,0 +1,69 @@
+#include "SparkFun_Qwiic_OTOS_Arduino_Library.h"
+#include "Wire.h"
+
+// Create an Optical Tracking Odometry Sensor object
+QwiicOTOS myOtos;
+
+void setup()
+{
+    // Start serial
+    Serial.begin(115200);
+    Serial.println("Qwiic OTOS Example 4 - Set Offset and Position");
+
+    Wire.begin();
+
+    // Attempt to begin the sensor
+    while (myOtos.begin() == false)
+    {
+        Serial.println("OTOS not connected, check your wiring and I2C address!");
+        delay(1000);
+    }
+
+    Serial.println("OTOS connected!");
+    
+    // Here you can set the offset for the sensor relative to the center of the
+    // robot. The units default to inches and degrees, but if you want to use
+    // different units, specify them before setting the offset! For example, if
+    // the sensor is mounted 5 inches to the left (negative X) and 10 inches
+    // forward (positive Y) of the center of the robot, and mounted 90 degrees
+    // clockwise (negative rotation) from the robot's orientation, the offset
+    // would be {-5, 10, -90}. These can be any value, even the angle can be
+    // tweaked slightly to compensate for imperfect mounting (eg. 1.3 degrees).
+    otos_pose2d_t offset = {-5, 10, -90};
+    myOtos.setOffset(offset);
+
+    // Reset the tracking algorithm, making the sensor report it's at the origin
+    myOtos.resetTracking();
+
+    // The OTOS sensor itself does not know about the offset defined above, it
+    // only tracks its own location; the offset is applied by the library when
+    // getPosition() is called. After resetting the tracking, the sensor will
+    // report that it's at the origin, but getPosition() will return the inverse
+    // of the offset pose. This can be resolved by calling setPosition(), which
+    // take into account the offset and set the sensor's location accordingly.
+    // Here we set the location to the origin, but any location is valid within
+    // the sensor's tracking range.
+    otos_pose2d_t newPose = {0, 0, 0};
+    myOtos.setPosition(newPose);
+}
+
+void loop()
+{
+    // Get the latest robot pose, which includes the x and y coordinates, plus
+    // the heading angle
+    otos_pose2d_t robotPose;
+    myOtos.getPosition(robotPose);
+
+    // Print measurement
+    Serial.println();
+    Serial.println("Robot pose:");
+    Serial.print("X (Inches): ");
+    Serial.println(robotPose.x);
+    Serial.print("Y (Inches): ");
+    Serial.println(robotPose.y);
+    Serial.print("Heading (Degrees): ");
+    Serial.println(robotPose.h);
+
+    // Wait a bit so we don't spam the serial port
+    delay(500);
+}