♻️ Moves peripherals to source file, add sensor base

esp32/include/peripherals/barometer.h

@@ -9,10 +9,35 @@
 #include <Adafruit_BMP085_U.h>
 #include <Adafruit_Sensor.h>
 
+#include <peripherals/sensor.hpp>
+
+struct BarometerMsg : public SensorMessageBase {
+    float pressure {-1};
+    float altitude {-1};
+    float temperature {-1};
+    bool success {false};
+
+    void toJson(JsonVariant v) const override {
+        JsonArray arr = v.to<JsonArray>();
+        arr.add(pressure);
+        arr.add(altitude);
+        arr.add(temperature);
+        arr.add(success);
+    }
+
+    void fromJson(JsonVariantConst v) override {
+        JsonArrayConst arr = v.as<JsonArrayConst>();
+        pressure = arr[0] | -1.0f;
+        altitude = arr[1] | -1.0f;
+        temperature = arr[2] | -1.0f;
+        success = arr[3] | false;
+    }
+
+    friend void toJson(JsonVariant v, BarometerMsg const& a) { a.toJson(v); }
+};
+
 class Barometer {
   public:
-    Barometer() : _bmp(10085) {}
-
     bool initialize() {
         bmp_success = _bmp.begin();
         return bmp_success;
@@ -44,7 +69,7 @@ class Barometer {
     bool active() { return bmp_success; }
 
   private:
-    Adafruit_BMP085_Unified _bmp;
+    Adafruit_BMP085_Unified _bmp {10085};
     bool bmp_success {false};
     float pressure {0};
     float altitude {0};

esp32/include/peripherals/imu.h

@@ -14,43 +14,41 @@
 #include <Adafruit_BNO055.h>
 #endif
 
-struct IMUAnglesMsg {
+#include <peripherals/sensor.hpp>
+
+struct IMUAnglesMsg : public SensorMessageBase {
     float rpy[3] {0, 0, 0};
     float temperature {-1};
     bool success {false};
 
-    friend void toJson(JsonVariant v, IMUAnglesMsg const& a) {
+    void toJson(JsonVariant v) const override {
         JsonArray arr = v.to<JsonArray>();
-        arr.add(a.rpy[0]);
+        arr.add(rpy[0]);
-        arr.add(a.rpy[1]);
+        arr.add(rpy[1]);
-        arr.add(a.rpy[2]);
+        arr.add(rpy[2]);
-        arr.add(a.temperature);
+        arr.add(temperature);
-        arr.add(a.success);
+        arr.add(success);
     }
 
-    void fromJson(JsonVariantConst o) {
+    void fromJson(JsonVariantConst v) override {
-        JsonArrayConst arr = o.as<JsonArrayConst>();
+        JsonArrayConst arr = v.as<JsonArrayConst>();
-        rpy[0] = arr[0].as<float>();
+        rpy[0] = arr[0] | -1.0f;
-        rpy[1] = arr[1].as<float>();
+        rpy[1] = arr[1] | -1.0f;
-        rpy[2] = arr[2].as<float>();
+        rpy[2] = arr[2] | -1.0f;
-        temperature = arr[3].as<float>();
+        temperature = arr[3] | -1.0f;
-        success = arr[4].as<bool>();
+        success = arr[4] | false;
     }
+
+    friend void toJson(JsonVariant v, IMUAnglesMsg const& a) { a.toJson(v); }
 };
 
-class IMU {
+class IMU : public SensorBase<IMUAnglesMsg> {
   public:
-    IMU()
+    bool initialize() override {
-#if FT_ENABLED(USE_BNO055)
-        : _imu(55, 0x29)
-#endif
-    {
-    }
-    bool initialize() {
 #if FT_ENABLED(USE_MPU6050)
         _imu.initialize();
-        imuMsg.success = _imu.testConnection();
+        _msg.success = _imu.testConnection();
-        if (!imuMsg.success) return false;
+        if (!_msg.success) return false;
         devStatus = _imu.dmpInitialize();
         if (devStatus == 0) {
             _imu.setDMPEnabled(false);
@@ -64,8 +62,8 @@ class IMU {
         }
 #endif
 #if FT_ENABLED(USE_BNO055)
-        imuMsg.success = _imu.begin();
+        _msg.success = _imu.begin();
-        if (!imuMsg.success) {
+        if (!_msg.success) {
             return false;
         }
         _imu.setExtCrystalUse(true);
@@ -73,14 +71,14 @@ class IMU {
         return true;
     }
 
-    bool readIMU() {
+    bool update() override {
-        if (!imuMsg.success) return false;
+        if (!_msg.success) return false;
 #if FT_ENABLED(USE_MPU6050)
         if (_imu.dmpPacketAvailable()) {
             if (_imu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
                 _imu.dmpGetQuaternion(&q, fifoBuffer);
                 _imu.dmpGetGravity(&gravity, &q);
-                _imu.dmpGetYawPitchRoll(imuMsg.rpy, &q, &gravity);
+                _imu.dmpGetYawPitchRoll(_msg.rpy, &q, &gravity);
                 return true;
             }
         }
@@ -89,31 +87,20 @@ class IMU {
 #if FT_ENABLED(USE_BNO055)
         sensors_event_t event;
         _imu.getEvent(&event);
-        imuMsg.rpy[0] = event.orientation.x;
+        _msg.rpy[0] = event.orientation.x;
-        imuMsg.rpy[1] = event.orientation.y;
+        _msg.rpy[1] = event.orientation.y;
-        imuMsg.rpy[2] = event.orientation.z;
+        _msg.rpy[2] = event.orientation.z;
 #endif
         return true;
     }
 
-    float getTemperature() { return imuMsg.temperature; }
+    float getTemperature() { return _msg.temperature; }
 
-    float getAngleX() { return imuMsg.rpy[2]; }
+    float getAngleX() { return _msg.rpy[2]; }
 
-    float getAngleY() { return imuMsg.rpy[1]; }
+    float getAngleY() { return _msg.rpy[1]; }
 
-    float getAngleZ() { return imuMsg.rpy[0]; }
+    float getAngleZ() { return _msg.rpy[0]; }
-
-    bool isActive() { return imuMsg.success; }
-
-    IMUAnglesMsg getIMUAngles() { return imuMsg; }
-
-    void readIMU(JsonObject& root) {
-        if (!imuMsg.success) return;
-        root["x"] = round2(getAngleX());
-        root["y"] = round2(getAngleY());
-        root["z"] = round2(getAngleZ());
-    }
 
   private:
 #if FT_ENABLED(USE_MPU6050)
@@ -124,7 +111,6 @@ class IMU {
     VectorFloat gravity;
 #endif
 #if FT_ENABLED(USE_BNO055)
-    Adafruit_BNO055 _imu;
+    Adafruit_BNO055 _imu {55, 0x29};
 #endif
-    IMUAnglesMsg imuMsg;
 };

esp32/include/peripherals/magnetometer.h

@@ -9,40 +9,41 @@
 #include <Adafruit_HMC5883_U.h>
 #include <Adafruit_Sensor.h>
 
-struct MagnetometerMsg {
+#include <peripherals/sensor.hpp>
+
+struct MagnetometerMsg : public SensorMessageBase {
     float rpy[3] {0, 0, 0};
     float heading {-1};
-    bool success {false};
 
-    friend void toJson(JsonVariant v, MagnetometerMsg const& a) {
+    void toJson(JsonVariant v) const override {
         JsonArray arr = v.to<JsonArray>();
-        arr.add(a.rpy[0]);
+        arr.add(rpy[0]);
-        arr.add(a.rpy[1]);
+        arr.add(rpy[1]);
-        arr.add(a.rpy[2]);
+        arr.add(rpy[2]);
-        arr.add(a.heading);
+        arr.add(heading);
-        arr.add(a.success);
+        arr.add(success);
     }
 
-    void fromJson(JsonVariantConst o) {
+    void fromJson(JsonVariantConst v) override {
-        JsonArrayConst arr = o.as<JsonArrayConst>();
+        JsonArrayConst arr = v.as<JsonArrayConst>();
-        rpy[0] = arr[0].as<float>();
+        rpy[0] = arr[0] | 0.0f;
-        rpy[1] = arr[1].as<float>();
+        rpy[1] = arr[1] | 0.0f;
-        rpy[2] = arr[2].as<float>();
+        rpy[2] = arr[2] | 0.0f;
-        heading = arr[3].as<float>();
+        heading = arr[3] | -1.0f;
-        success = arr[4].as<bool>();
+        success = arr[4] | false;
     }
+
+    friend void toJson(JsonVariant v, MagnetometerMsg const& a) { a.toJson(v); }
 };
 
-class Magnetometer {
+class Magnetometer : public SensorBase<MagnetometerMsg> {
   public:
-    Magnetometer() : _mag(12345) {}
-
     bool initialize() {
         msg.success = _mag.begin();
         return msg.success;
     }
 
-    bool readMagnetometer() {
+    bool update() {
         if (!msg.success) return false;
         sensors_event_t event;
         bool updated = _mag.getEvent(&event);
@@ -66,17 +67,8 @@ class Magnetometer {
 
     float getHeading() { return msg.heading; }
 
-    MagnetometerMsg getMagnetometerMsg() { return msg; }
-
-    void readMagnetometer(JsonObject& root) {
-        if (!msg.success) return;
-        root["heading"] = round2(getHeading());
-    }
-
-    bool isActive() { return msg.success; }
-
   private:
-    Adafruit_HMC5883_Unified _mag;
+    Adafruit_HMC5883_Unified _mag {12345};
     MagnetometerMsg msg;
     const float declinationAngle = 0.22;
 };

esp32/include/peripherals/peripherals.h

@@ -36,216 +36,46 @@
 
 class Peripherals : public StatefulService<PeripheralsConfiguration> {
   public:
-    Peripherals()
+    Peripherals();
-        : endpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this),
-          _eventEndpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this,
-                         EVENT_CONFIGURATION_SETTINGS),
-          _persistence(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this, DEVICE_CONFIG_FILE) {
-        _accessMutex = xSemaphoreCreateMutex();
-        addUpdateHandler([&](const String &originId) { updatePins(); }, false);
-    };
 
-    void begin() {
+    void begin();
-        _eventEndpoint.begin();
-        _persistence.readFromFS();
 
-        // socket.onEvent(EVENT_I2C_SCAN, [&](JsonVariant &root, int originId) {
+    void update();
-        //     scanI2C();
-        //     emitI2C();
-        // });
 
-        // socket.onSubscribe(EVENT_I2C_SCAN, [&](const String &originId, bool sync) {
+    void updatePins();
-        //     scanI2C();
-        //     emitI2C(originId, sync);
-        // });
 
-        updatePins();
+    void scanI2C(uint8_t lower = 1, uint8_t higher = 127);
 
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+    void getI2CResult(JsonVariant &root);
-        if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
-#endif
-#if FT_ENABLED(USE_HMC5883)
-        if (!_mag.initialize()) ESP_LOGE("IMUService", "MAG initialize failed");
-#endif
-#if FT_ENABLED(USE_BMP180)
-        if (!_bmp.initialize()) ESP_LOGE("IMUService", "BMP initialize failed");
-#endif
-#if FT_ENABLED(USE_PAJ7620U2)
-        if (!_gesture.initialize()) ESP_LOGE("IMUService", "Gesture initialize failed");
-#endif
-#if FT_ENABLED(USE_USS)
-        _left_sonar = std::make_unique<NewPing>(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
-        _right_sonar = std::make_unique<NewPing>(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE);
-#endif
-    };
 
-    void update() {
+    void getIMUResult(JsonVariant &root);
-        readIMU();
-        readMag();
-        // _peripherals.readBMP();
-        EXECUTE_EVERY_N_MS(100, { readGesture(); });
-    }
 
-    void loop() {
+    void getSonarResult(JsonVariant &root);
-        EXECUTE_EVERY_N_MS(_updateInterval, {
-            beginTransaction();
-            emitIMU();
-            readSonar();
-            emitSonar();
-            endTransaction();
-        });
-    }
-
-    void updatePins() {
-        if (i2c_active) {
-            Wire.end();
-        }
-
-        if (state().sda != -1 && state().scl != -1) {
-            Wire.begin(state().sda, state().scl, state().frequency);
-            i2c_active = true;
-        }
-    }
-
-    void emitI2C(const String &originId = "", bool sync = false) {
-        char output[150];
-        JsonDocument doc;
-        JsonObject root = doc.to<JsonObject>();
-        root["sda"] = state().sda;
-        root["scl"] = state().scl;
-        JsonArray addresses = root["addresses"].to<JsonArray>();
-        for (auto &address : addressList) {
-            addresses.add(address);
-        }
-        ESP_LOGI("Peripherals", "Emitting I2C scan results, %s %d", originId.c_str(), sync);
-        JsonVariant data = doc.as<JsonVariant>();
-        // socket.emit(EVENT_I2C_SCAN, data, originId.c_str(), sync);
-    }
-
-    void scanI2C(uint8_t lower = 1, uint8_t higher = 127) {
-        addressList.clear();
-        for (uint8_t address = lower; address < higher; address++) {
-            Wire.beginTransmission(address);
-            if (Wire.endTransmission() == 0) {
-                addressList.emplace_back(address);
-                ESP_LOGI("Peripherals", "I2C device found at address 0x%02X", address);
-            }
-        }
-        uint8_t nDevices = addressList.size();
-        ESP_LOGI("Peripherals", "Scan complete - Found %d device(s)", nDevices);
-    }
 
     /* IMU FUNCTIONS */
-    bool readIMU() {
+    bool readImu();
-        bool updated = false;
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
-        beginTransaction();
-        updated = _imu.readIMU();
-        endTransaction();
-#endif
-        return updated;
-    }
 
-    bool readMag() {
+    bool readMag();
-        bool updated = false;
-#if FT_ENABLED(USE_HMC5883)
-        beginTransaction();
-        updated = _mag.readMagnetometer();
-        endTransaction();
-#endif
-        return updated;
-    }
 
-    bool readBMP() {
+    bool readBMP();
-        bool updated = false;
-#if FT_ENABLED(USE_BMP180)
-        beginTransaction();
-        updated = _bmp.readBarometer();
-        endTransaction();
-#endif
-        return updated;
-    }
 
-    bool readGesture() {
+    bool readGesture();
-        bool updated = false;
-#if FT_ENABLED(USE_PAJ7620U2)
-        beginTransaction();
-        updated = _gesture.readGesture();
-        endTransaction();
-#endif
-        return updated;
-    }
 
-    void readSonar() {
+    void readSonar();
-#if FT_ENABLED(USE_USS)
-        _left_distance = _left_sonar->ping_cm();
-        delay(50);
-        _right_distance = _right_sonar->ping_cm();
-#endif
-    }
 
-    float angleX() {
+    float angleX();
-        return
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
-            _imu.getAngleX();
-#else
-            0;
-#endif
-    }
 
-    float angleY() {
+    float angleY();
-        return
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
-            _imu.getAngleY();
-#else
-            0;
-#endif
-    }
 
-    // float angleZ() { return _imu.getAngleZ(); }
+    float angleZ();
 
-    gesture_t const takeGesture() {
+    gesture_t const takeGesture();
-        return
-#if FT_ENABLED(USE_PAJ7620U2)
-            _gesture.takeGesture();
-#else
-            gesture_t::eGestureNone;
-#endif
-    }
 
-    float leftDistance() { return _left_distance; }
+    float leftDistance();
-    float rightDistance() { return _right_distance; }
+    float rightDistance();
 
     StatefulHttpEndpoint<PeripheralsConfiguration> endpoint;
 
-    void emitIMU() {
-        doc.clear();
-        JsonObject root = doc.to<JsonObject>();
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
-        _imu.readIMU(root);
-#endif
-#if FT_ENABLED(USE_HMC5883)
-        _mag.readMagnetometer(root);
-#endif
-#if FT_ENABLED(USE_BMP180)
-        _bmp.readBarometer(root);
-#endif
-#if FT_ENABLED(USE_MPU6050 || USE_BNO055) || FT_ENABLED(USE_HMC5883)
-        JsonVariant data = doc.as<JsonVariant>();
-        // socket.emit(EVENT_IMU, data);
-#endif
-    }
-
-    void emitSonar() {
-#if FT_ENABLED(USE_USS)
-        doc.clear();
-        JsonArray root = doc.to<JsonArray>();
-        root[0] = _left_distance, root[1] = _right_distance;
-        JsonVariant data = doc.as<JsonVariant>();
-        // socket.emit("sonar", data);
-#endif
-    }
-
   private:
     EventEndpoint<PeripheralsConfiguration> _eventEndpoint;
     FSPersistence<PeripheralsConfiguration> _persistence;

esp32/include/peripherals/sensor.hpp

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <ArduinoJson.h>
+
+struct SensorMessageBase {
+    bool success;
+    virtual void toJson(JsonVariant v) const = 0;
+    virtual void fromJson(JsonVariantConst v) = 0;
+
+    virtual ~SensorMessageBase() = default;
+};
+
+template <class T>
+class SensorBase {
+    static_assert(std::is_base_of<SensorMessageBase, T>::value, "T must inherit from SensorMessageBase");
+
+  public:
+    SensorBase() {}
+
+    virtual bool initialize() = 0;
+
+    virtual bool update() = 0;
+
+    virtual void getResults(JsonVariant &root) { _msg.toJson(root); }
+
+    virtual T getResult() { return _msg; }
+
+    virtual bool isActive() { return _msg.success; }
+
+  protected:
+    T _msg;
+};

esp32/src/main.cpp

@@ -75,8 +75,10 @@ void setupServer() {
 #define INPUT_EVENT "input"
 #define MODE_EVENT "mode"
 #define WALK_GAIT_EVENT "walk_gait"
+#define EVENT_I2C_SCAN "i2cScan"
 
 void setupEventSocket() {
+    // Motion events
     socket.onEvent(INPUT_EVENT, [&](JsonVariant &root, int originId) { motionService.handleInput(root, originId); });
 
     socket.onEvent(MODE_EVENT, [&](JsonVariant &root, int originId) { motionService.handleMode(root, originId); });
@@ -85,6 +87,15 @@ void setupEventSocket() {
                    [&](JsonVariant &root, int originId) { motionService.handleWalkGait(root, originId); });
 
     socket.onEvent(ANGLES_EVENT, [&](JsonVariant &root, int originId) { motionService.anglesEvent(root, originId); });
+
+    // Peripherals events
+    socket.onEvent(EVENT_I2C_SCAN, [&](JsonVariant &root, int originId) {
+        peripherals.scanI2C();
+        JsonDocument doc;
+        JsonVariant results = doc.to<JsonVariant>();
+        peripherals.getI2CResult(results);
+        socket.emit(EVENT_I2C_SCAN, results);
+    });
 }
 
 void IRAM_ATTR SpotControlLoopEntry(void *) {

esp32/src/peripherals/peripherals.cpp

@@ -0,0 +1,185 @@
+#include <peripherals/peripherals.h>
+
+Peripherals::Peripherals()
+    : endpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this),
+      _eventEndpoint(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this,
+                     EVENT_CONFIGURATION_SETTINGS),
+      _persistence(PeripheralsConfiguration::read, PeripheralsConfiguration::update, this, DEVICE_CONFIG_FILE) {
+    _accessMutex = xSemaphoreCreateMutex();
+    addUpdateHandler([&](const String &originId) { updatePins(); }, false);
+}
+
+void Peripherals::begin() {
+    _eventEndpoint.begin();
+    _persistence.readFromFS();
+
+    updatePins();
+
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+    if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
+#endif
+#if FT_ENABLED(USE_HMC5883)
+    if (!_mag.initialize()) ESP_LOGE("IMUService", "MAG initialize failed");
+#endif
+#if FT_ENABLED(USE_BMP180)
+    if (!_bmp.initialize()) ESP_LOGE("IMUService", "BMP initialize failed");
+#endif
+#if FT_ENABLED(USE_PAJ7620U2)
+    if (!_gesture.initialize()) ESP_LOGE("IMUService", "Gesture initialize failed");
+#endif
+#if FT_ENABLED(USE_USS)
+    _left_sonar = std::make_unique<NewPing>(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE);
+    _right_sonar = std::make_unique<NewPing>(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE);
+#endif
+};
+
+void Peripherals::update() {
+    readImu();
+    readMag();
+    EXECUTE_EVERY_N_MS(100, { readGesture(); });
+    EXECUTE_EVERY_N_MS(500, { readBMP(); });
+    EXECUTE_EVERY_N_MS(500, { readSonar(); });
+}
+
+void Peripherals::updatePins() {
+    if (i2c_active) {
+        Wire.end();
+    }
+
+    if (state().sda != -1 && state().scl != -1) {
+        Wire.begin(state().sda, state().scl, state().frequency);
+        i2c_active = true;
+    }
+}
+
+void Peripherals::getI2CResult(JsonVariant &root) {
+    char output[150];
+    root["sda"] = state().sda;
+    root["scl"] = state().scl;
+    JsonArray addresses = root["addresses"].to<JsonArray>();
+    for (auto &address : addressList) {
+        addresses.add(address);
+    }
+    ESP_LOGI("Peripherals", "Emitting I2C scan results: %d", addressList.size());
+}
+
+void Peripherals::scanI2C(uint8_t lower, uint8_t higher) {
+    addressList.clear();
+    for (uint8_t address = lower; address < higher; address++) {
+        Wire.beginTransmission(address);
+        if (Wire.endTransmission() == 0) {
+            addressList.emplace_back(address);
+            ESP_LOGI("Peripherals", "I2C device found at address 0x%02X", address);
+        }
+    }
+    uint8_t nDevices = addressList.size();
+    ESP_LOGI("Peripherals", "Scan complete - Found %d device(s)", nDevices);
+}
+
+/* IMU FUNCTIONS */
+bool Peripherals::readImu() {
+    bool updated = false;
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+    beginTransaction();
+    updated = _imu.update();
+    endTransaction();
+#endif
+    return updated;
+}
+
+bool Peripherals::readMag() {
+    bool updated = false;
+#if FT_ENABLED(USE_HMC5883)
+    beginTransaction();
+    updated = _mag.update();
+    endTransaction();
+#endif
+    return updated;
+}
+
+bool Peripherals::readBMP() {
+    bool updated = false;
+#if FT_ENABLED(USE_BMP180)
+    beginTransaction();
+    updated = _bmp.readBarometer();
+    endTransaction();
+#endif
+    return updated;
+}
+
+bool Peripherals::readGesture() {
+    bool updated = false;
+#if FT_ENABLED(USE_PAJ7620U2)
+    beginTransaction();
+    updated = _gesture.readGesture();
+    endTransaction();
+#endif
+    return updated;
+}
+
+void Peripherals::readSonar() {
+#if FT_ENABLED(USE_USS)
+    _left_distance = _left_sonar->ping_cm();
+    delay(50);
+    _right_distance = _right_sonar->ping_cm();
+#endif
+}
+
+float Peripherals::angleX() {
+    return
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+        _imu.getAngleX();
+#else
+        0;
+#endif
+}
+
+float Peripherals::angleY() {
+    return
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+        _imu.getAngleY();
+#else
+        0;
+#endif
+}
+
+float Peripherals::angleZ() {
+    return
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+        _imu.getAngleZ();
+#else
+        0;
+#endif
+}
+
+gesture_t const Peripherals::takeGesture() {
+    return
+#if FT_ENABLED(USE_PAJ7620U2)
+        _gesture.takeGesture();
+#else
+        gesture_t::eGestureNone;
+#endif
+}
+
+float Peripherals::leftDistance() { return _left_distance; }
+float Peripherals::rightDistance() { return _right_distance; }
+
+void Peripherals::getIMUResult(JsonVariant &root) {
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
+    _imu.getResults(root);
+#endif
+#if FT_ENABLED(USE_HMC5883)
+    _mag.getResults(root);
+#endif
+#if FT_ENABLED(USE_BMP180)
+    _bmp.getResults(root);
+#endif
+}
+
+void Peripherals::getSonarResult(JsonVariant &root) {
+#if FT_ENABLED(USE_USS)
+    JsonArray array = root.to<JsonArray>();
+    array[0] = _left_distance;
+    array[1] = _right_distance;
+#endif
+}