diff --git a/app/src/routes/peripherals/i2c/i2c.svelte b/app/src/routes/peripherals/i2c/i2c.svelte index 084f08d..18dcef2 100644 --- a/app/src/routes/peripherals/i2c/i2c.svelte +++ b/app/src/routes/peripherals/i2c/i2c.svelte @@ -16,6 +16,7 @@ part_number: 'MPU6050', name: 'Six-Axis (Gyro + Accelerometer) MEMS MotionTracking™ Devices' }, + { address: 115, part_number: 'PAJ7620U2', name: 'Gesture sensor' }, { address: 119, part_number: 'BMP085', name: 'Temp/Barometric' } ] diff --git a/esp32/features.ini b/esp32/features.ini index 3427e8e..f3e545e 100644 --- a/esp32/features.ini +++ b/esp32/features.ini @@ -19,4 +19,5 @@ build_flags = -D USE_WS2812=1 -D USE_BNO055=0 -D USE_USS=0 - -D USE_PCA9685=1 \ No newline at end of file + -D USE_PCA9685=1 + -D USE_PAJ7620U2=0 diff --git a/esp32/include/motion.h b/esp32/include/motion.h index 0bb9871..594fe50 100644 --- a/esp32/include/motion.h +++ b/esp32/include/motion.h @@ -105,7 +105,23 @@ class MotionService { _servoController->setAngles(angles); } + void handleGestures() { + const gesture_t ges = _peripherals->getGesture(); + if (ges != gesture_t::eGestureNone) { + ESP_LOGI("Motion", "Gesture: %d", ges); + switch (ges) { + case gesture_t::eGestureDown: setState(&restState); break; + case gesture_t::eGestureUp: setState(&standState); break; + case gesture_t::eGestureLeft: + case gesture_t::eGestureRight: setState(&walkState); break; + + default: break; + } + } + } + bool updateMotion() { + handleGestures(); if (!state) return false; unsigned long now = millis(); float dt = (now - lastUpdate) / 1000.0f; diff --git a/esp32/include/peripherals/gesture.h b/esp32/include/peripherals/gesture.h new file mode 100644 index 0000000..78e71a9 --- /dev/null +++ b/esp32/include/peripherals/gesture.h @@ -0,0 +1,151 @@ +#pragma once + +#include +#include + +enum gesture_t : uint16_t { + eGestureNone = 0x0000, + eGestureRight = 0x0001, + eGestureLeft = 0x0002, + eGestureUp = 0x0004, + eGestureDown = 0x0008, + eGestureForward = 0x0010, + eGestureBackward = 0x0020, + eGestureClockwise = 0x0040, + eGestureAntiClockwise = 0x0080, + eGestureWave = 0x0100 +}; + +struct GestureMsg { + gesture_t gesture {eGestureNone}; + bool success {false}; + friend void toJson(JsonVariant v, GestureMsg const& a) { + JsonArray arr = v.to(); + arr.add(static_cast(a.gesture)); + arr.add(a.success); + } + void fromJson(JsonVariantConst o) { + JsonArrayConst arr = o.as(); + gesture = static_cast(arr[0].as()); + success = arr[1].as(); + } +}; + +class PAJ7620U2 { + public: + PAJ7620U2(TwoWire* w = &Wire, uint8_t addr = 0x73) : wire(w), dev(addr) {} + int begin() { + uint16_t id = 0; + wire->begin(); + selectBank(0x00); + if (readReg(0x00, &id, 2) != 2) return -1; + if (id != 0x7620) return -2; + for (size_t i = 0; i < sizeof(initReg) / 2; i++) writeReg(initReg[i][0], &initReg[i][1], 1); + selectBank(0x00); + return 0; + } + + void setGestureHighRate(bool b) { highRate = b; } + + gesture_t getGesture() { + uint8_t f1 = 0, f0 = 0, t0 = 0; + if (readReg(0x44, &f1, 1) != 1) return eGestureNone; + if (f1) { + delay(highRate ? 200 : 1000); + return eGestureWave; + } + if (readReg(0x43, &f0, 1) != 1) return eGestureNone; + if (!highRate) { + delay(800); + if (readReg(0x43, &t0, 1) == 1) f0 |= t0; + } + if (f0 & 0x01) return eGestureRight; + if (f0 & 0x02) return eGestureLeft; + if (f0 & 0x04) return eGestureUp; + if (f0 & 0x08) return eGestureDown; + if (f0 & 0x10) return eGestureForward; + if (f0 & 0x20) return eGestureBackward; + if (f0 & 0x40) return eGestureClockwise; + if (f0 & 0x80) return eGestureAntiClockwise; + return eGestureNone; + } + + private: + void selectBank(uint8_t b) { writeReg(0xEF, &b, 1); } + + void writeReg(uint8_t reg, const void* p, size_t n) { + if (!p || !n) return; + const uint8_t* d = static_cast(p); + wire->beginTransmission(dev); + wire->write(reg); + for (size_t i = 0; i < n; i++) wire->write(d[i]); + wire->endTransmission(); + } + + uint8_t readReg(uint8_t reg, void* p, size_t n) { + if (!p || !n) return 0; + uint8_t* d = static_cast(p); + wire->beginTransmission(dev); + wire->write(reg); + if (wire->endTransmission(false) != 0) return 0; + uint8_t r = wire->requestFrom(dev, static_cast(n)); + for (uint8_t i = 0; i < r; i++) d[i] = wire->read(); + return r; + } + + TwoWire* wire; + uint8_t dev; + bool highRate = false; + + static inline const uint8_t initReg[219][2] = { + {0xEF, 0x00}, {0x32, 0x29}, {0x33, 0x01}, {0x34, 0x00}, {0x35, 0x01}, {0x36, 0x00}, {0x37, 0x07}, {0x38, 0x17}, + {0x39, 0x06}, {0x3A, 0x12}, {0x3F, 0x00}, {0x40, 0x02}, {0x41, 0xFF}, {0x42, 0x01}, {0x46, 0x2D}, {0x47, 0x0F}, + {0x48, 0x3C}, {0x49, 0x00}, {0x4A, 0x1E}, {0x4B, 0x00}, {0x4C, 0x20}, {0x4D, 0x00}, {0x4E, 0x1A}, {0x4F, 0x14}, + {0x50, 0x00}, {0x51, 0x10}, {0x52, 0x00}, {0x5C, 0x02}, {0x5D, 0x00}, {0x5E, 0x10}, {0x5F, 0x3F}, {0x60, 0x27}, + {0x61, 0x28}, {0x62, 0x00}, {0x63, 0x03}, {0x64, 0xF7}, {0x65, 0x03}, {0x66, 0xD9}, {0x67, 0x03}, {0x68, 0x01}, + {0x69, 0xC8}, {0x6A, 0x40}, {0x6D, 0x04}, {0x6E, 0x00}, {0x6F, 0x00}, {0x70, 0x80}, {0x71, 0x00}, {0x72, 0x00}, + {0x73, 0x00}, {0x74, 0xF0}, {0x75, 0x00}, {0x80, 0x42}, {0x81, 0x44}, {0x82, 0x04}, {0x83, 0x20}, {0x84, 0x20}, + {0x85, 0x00}, {0x86, 0x10}, {0x87, 0x00}, {0x88, 0x05}, {0x89, 0x18}, {0x8A, 0x10}, {0x8B, 0x01}, {0x8C, 0x37}, + {0x8D, 0x00}, {0x8E, 0xF0}, {0x8F, 0x81}, {0x90, 0x06}, {0x91, 0x06}, {0x92, 0x1E}, {0x93, 0x0D}, {0x94, 0x0A}, + {0x95, 0x0A}, {0x96, 0x0C}, {0x97, 0x05}, {0x98, 0x0A}, {0x99, 0x41}, {0x9A, 0x14}, {0x9B, 0x0A}, {0x9C, 0x3F}, + {0x9D, 0x33}, {0x9E, 0xAE}, {0x9F, 0xF9}, {0xA0, 0x48}, {0xA1, 0x13}, {0xA2, 0x10}, {0xA3, 0x08}, {0xA4, 0x30}, + {0xA5, 0x19}, {0xA6, 0x10}, {0xA7, 0x08}, {0xA8, 0x24}, {0xA9, 0x04}, {0xAA, 0x1E}, {0xAB, 0x1E}, {0xCC, 0x19}, + {0xCD, 0x0B}, {0xCE, 0x13}, {0xCF, 0x64}, {0xD0, 0x21}, {0xD1, 0x0F}, {0xD2, 0x88}, {0xE0, 0x01}, {0xE1, 0x04}, + {0xE2, 0x41}, {0xE3, 0xD6}, {0xE4, 0x00}, {0xE5, 0x0C}, {0xE6, 0x0A}, {0xE7, 0x00}, {0xE8, 0x00}, {0xE9, 0x00}, + {0xEE, 0x07}, {0xEF, 0x01}, {0x00, 0x1E}, {0x01, 0x1E}, {0x02, 0x0F}, {0x03, 0x10}, {0x04, 0x02}, {0x05, 0x00}, + {0x06, 0xB0}, {0x07, 0x04}, {0x08, 0x0D}, {0x09, 0x0E}, {0x0A, 0x9C}, {0x0B, 0x04}, {0x0C, 0x05}, {0x0D, 0x0F}, + {0x0E, 0x02}, {0x0F, 0x12}, {0x10, 0x02}, {0x11, 0x02}, {0x12, 0x00}, {0x13, 0x01}, {0x14, 0x05}, {0x15, 0x07}, + {0x16, 0x05}, {0x17, 0x07}, {0x18, 0x01}, {0x19, 0x04}, {0x1A, 0x05}, {0x1B, 0x0C}, {0x1C, 0x2A}, {0x1D, 0x01}, + {0x1E, 0x00}, {0x21, 0x00}, {0x22, 0x00}, {0x23, 0x00}, {0x25, 0x01}, {0x26, 0x00}, {0x27, 0x39}, {0x28, 0x7F}, + {0x29, 0x08}, {0x30, 0x03}, {0x31, 0x00}, {0x32, 0x1A}, {0x33, 0x1A}, {0x34, 0x07}, {0x35, 0x07}, {0x36, 0x01}, + {0x37, 0xFF}, {0x38, 0x36}, {0x39, 0x07}, {0x3A, 0x00}, {0x3E, 0xFF}, {0x3F, 0x00}, {0x40, 0x77}, {0x41, 0x40}, + {0x42, 0x00}, {0x43, 0x30}, {0x44, 0xA0}, {0x45, 0x5C}, {0x46, 0x00}, {0x47, 0x00}, {0x48, 0x58}, {0x4A, 0x1E}, + {0x4B, 0x1E}, {0x4C, 0x00}, {0x4D, 0x00}, {0x4E, 0xA0}, {0x4F, 0x80}, {0x50, 0x00}, {0x51, 0x00}, {0x52, 0x00}, + {0x53, 0x00}, {0x54, 0x00}, {0x57, 0x80}, {0x59, 0x10}, {0x5A, 0x08}, {0x5B, 0x94}, {0x5C, 0xE8}, {0x5D, 0x08}, + {0x5E, 0x3D}, {0x5F, 0x99}, {0x60, 0x45}, {0x61, 0x40}, {0x63, 0x2D}, {0x64, 0x02}, {0x65, 0x96}, {0x66, 0x00}, + {0x67, 0x97}, {0x68, 0x01}, {0x69, 0xCD}, {0x6A, 0x01}, {0x6B, 0xB0}, {0x6C, 0x04}, {0x6D, 0x2C}, {0x6E, 0x01}, + {0x6F, 0x32}, {0x71, 0x00}, {0x72, 0x01}, {0x73, 0x35}, {0x74, 0x00}, {0x75, 0x33}, {0x76, 0x31}, {0x77, 0x01}, + {0x7C, 0x84}, {0x7D, 0x03}, {0x7E, 0x01}}; +}; + +class GestureSensor { + public: + GestureSensor() {} + bool initialize() { + msg.success = paj.begin() == 0; + if (msg.success) paj.setGestureHighRate(true); + return msg.success; + } + bool readGesture() { + if (!msg.success) return false; + msg.gesture = paj.getGesture(); + return msg.gesture != eGestureNone; + } + gesture_t getGesture() { return msg.gesture; } + GestureMsg getGestureMsg() { return msg; } + bool isActive() { return msg.success; } + + private: + PAJ7620U2 paj; + GestureMsg msg; +}; \ No newline at end of file diff --git a/esp32/include/peripherals/peripherals.h b/esp32/include/peripherals/peripherals.h index 2e65996..a6cf44b 100644 --- a/esp32/include/peripherals/peripherals.h +++ b/esp32/include/peripherals/peripherals.h @@ -19,6 +19,7 @@ #include #include #include +#include #define EVENT_CONFIGURATION_SETTINGS "peripheralSettings" @@ -69,6 +70,9 @@ class Peripherals : public StatefulService { #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(USS_LEFT_PIN, USS_LEFT_PIN, MAX_DISTANCE); _right_sonar = std::make_unique(USS_RIGHT_PIN, USS_RIGHT_PIN, MAX_DISTANCE); @@ -155,6 +159,16 @@ class Peripherals : public StatefulService { return updated; } + bool readGesture() { + bool updated = false; +#if FT_ENABLED(USE_PAJ7620U2) + beginTransaction(); + updated = _gesture.readGesture(); + endTransaction(); +#endif + return updated; + } + void readSonar() { #if FT_ENABLED(USE_USS) _left_distance = _left_sonar->ping_cm(); @@ -183,6 +197,15 @@ class Peripherals : public StatefulService { // float angleZ() { return _imu.getAngleZ(); } + gesture_t getGesture() { + return +#if FT_ENABLED(USE_PAJ7620U2) + _gesture.getGesture(); +#else + gesture_t::eGestureNone; +#endif + } + float leftDistance() { return _left_distance; } float rightDistance() { return _right_distance; } @@ -236,6 +259,9 @@ class Peripherals : public StatefulService { #if FT_ENABLED(USE_BMP180) Barometer _bmp; #endif +#if FT_ENABLED(USE_PAJ7620U2) + GestureSensor _gesture; +#endif #if FT_ENABLED(USE_USS) std::unique_ptr _left_sonar; std::unique_ptr _right_sonar; diff --git a/esp32/include/spot.h b/esp32/include/spot.h index d8ea2bc..d9fe41e 100644 --- a/esp32/include/spot.h +++ b/esp32/include/spot.h @@ -49,6 +49,7 @@ class Spot { _peripherals.readIMU(); _peripherals.readMag(); _peripherals.readBMP(); + EXECUTE_EVERY_N_MS(100, { _peripherals.readGesture(); }); } // plan