From e450352511fc1ad047f1de16ab7169fc66b3262d Mon Sep 17 00:00:00 2001 From: GavinsMJ Date: Fri, 11 Nov 2022 18:34:45 +0300 Subject: [PATCH] changes --- HandMotion/include/MPU6050.h | 258 +++++++++++ HandMotion/include/definitions.h | 9 +- HandMotion/src/MPU6050.cpp | 749 ++++++++++++++++++++++++++++++ handmotion/src/main.cpp | 768 +++++++++++++++++++++++-------- 4 files changed, 1583 insertions(+), 201 deletions(-) create mode 100644 HandMotion/include/MPU6050.h create mode 100644 HandMotion/src/MPU6050.cpp diff --git a/HandMotion/include/MPU6050.h b/HandMotion/include/MPU6050.h new file mode 100644 index 0000000..fbf077e --- /dev/null +++ b/HandMotion/include/MPU6050.h @@ -0,0 +1,258 @@ +/* +MPU6050.h - Header file for the MPU6050 Triple Axis Gyroscope & Accelerometer Arduino Library. + +Version: 1.0.3 +(c) 2014-2015 Korneliusz Jarzebski +www.jarzebski.pl + +This program is free software: you can redistribute it and/or modify +it under the terms of the version 3 GNU General Public License as +published by the Free Software Foundation. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . +*/ + +#ifndef MPU6050_h +#define MPU6050_h + +#if ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +#define MPU6050_ADDRESS (0x68) // 0x69 when AD0 pin to Vcc + +#define MPU6050_REG_ACCEL_XOFFS_H (0x06) +#define MPU6050_REG_ACCEL_XOFFS_L (0x07) +#define MPU6050_REG_ACCEL_YOFFS_H (0x08) +#define MPU6050_REG_ACCEL_YOFFS_L (0x09) +#define MPU6050_REG_ACCEL_ZOFFS_H (0x0A) +#define MPU6050_REG_ACCEL_ZOFFS_L (0x0B) +#define MPU6050_REG_GYRO_XOFFS_H (0x13) +#define MPU6050_REG_GYRO_XOFFS_L (0x14) +#define MPU6050_REG_GYRO_YOFFS_H (0x15) +#define MPU6050_REG_GYRO_YOFFS_L (0x16) +#define MPU6050_REG_GYRO_ZOFFS_H (0x17) +#define MPU6050_REG_GYRO_ZOFFS_L (0x18) +#define MPU6050_REG_CONFIG (0x1A) +#define MPU6050_REG_GYRO_CONFIG (0x1B) // Gyroscope Configuration +#define MPU6050_REG_ACCEL_CONFIG (0x1C) // Accelerometer Configuration +#define MPU6050_REG_FF_THRESHOLD (0x1D) +#define MPU6050_REG_FF_DURATION (0x1E) +#define MPU6050_REG_MOT_THRESHOLD (0x1F) +#define MPU6050_REG_MOT_DURATION (0x20) +#define MPU6050_REG_ZMOT_THRESHOLD (0x21) +#define MPU6050_REG_ZMOT_DURATION (0x22) +#define MPU6050_REG_INT_PIN_CFG (0x37) // INT Pin. Bypass Enable Configuration +#define MPU6050_REG_INT_ENABLE (0x38) // INT Enable +#define MPU6050_REG_INT_STATUS (0x3A) +#define MPU6050_REG_ACCEL_XOUT_H (0x3B) +#define MPU6050_REG_ACCEL_XOUT_L (0x3C) +#define MPU6050_REG_ACCEL_YOUT_H (0x3D) +#define MPU6050_REG_ACCEL_YOUT_L (0x3E) +#define MPU6050_REG_ACCEL_ZOUT_H (0x3F) +#define MPU6050_REG_ACCEL_ZOUT_L (0x40) +#define MPU6050_REG_TEMP_OUT_H (0x41) +#define MPU6050_REG_TEMP_OUT_L (0x42) +#define MPU6050_REG_GYRO_XOUT_H (0x43) +#define MPU6050_REG_GYRO_XOUT_L (0x44) +#define MPU6050_REG_GYRO_YOUT_H (0x45) +#define MPU6050_REG_GYRO_YOUT_L (0x46) +#define MPU6050_REG_GYRO_ZOUT_H (0x47) +#define MPU6050_REG_GYRO_ZOUT_L (0x48) +#define MPU6050_REG_MOT_DETECT_STATUS (0x61) +#define MPU6050_REG_MOT_DETECT_CTRL (0x69) +#define MPU6050_REG_USER_CTRL (0x6A) // User Control +#define MPU6050_REG_PWR_MGMT_1 (0x6B) // Power Management 1 +#define MPU6050_REG_WHO_AM_I (0x75) // Who Am I + +#ifndef VECTOR_STRUCT_H +#define VECTOR_STRUCT_H +struct Vector +{ + float XAxis; + float YAxis; + float ZAxis; +}; +#endif + +struct Activites +{ + bool isOverflow; + bool isFreeFall; + bool isInactivity; + bool isActivity; + bool isPosActivityOnX; + bool isPosActivityOnY; + bool isPosActivityOnZ; + bool isNegActivityOnX; + bool isNegActivityOnY; + bool isNegActivityOnZ; + bool isDataReady; +}; + +typedef enum +{ + MPU6050_CLOCK_KEEP_RESET = 0b111, + MPU6050_CLOCK_EXTERNAL_19MHZ = 0b101, + MPU6050_CLOCK_EXTERNAL_32KHZ = 0b100, + MPU6050_CLOCK_PLL_ZGYRO = 0b011, + MPU6050_CLOCK_PLL_YGYRO = 0b010, + MPU6050_CLOCK_PLL_XGYRO = 0b001, + MPU6050_CLOCK_INTERNAL_8MHZ = 0b000 +} mpu6050_clockSource_t; + +typedef enum +{ + MPU6050_SCALE_2000DPS = 0b11, + MPU6050_SCALE_1000DPS = 0b10, + MPU6050_SCALE_500DPS = 0b01, + MPU6050_SCALE_250DPS = 0b00 +} mpu6050_dps_t; + +typedef enum +{ + MPU6050_RANGE_16G = 0b11, + MPU6050_RANGE_8G = 0b10, + MPU6050_RANGE_4G = 0b01, + MPU6050_RANGE_2G = 0b00, +} mpu6050_range_t; + +typedef enum +{ + MPU6050_DELAY_3MS = 0b11, + MPU6050_DELAY_2MS = 0b10, + MPU6050_DELAY_1MS = 0b01, + MPU6050_NO_DELAY = 0b00, +} mpu6050_onDelay_t; + +typedef enum +{ + MPU6050_DHPF_HOLD = 0b111, + MPU6050_DHPF_0_63HZ = 0b100, + MPU6050_DHPF_1_25HZ = 0b011, + MPU6050_DHPF_2_5HZ = 0b010, + MPU6050_DHPF_5HZ = 0b001, + MPU6050_DHPF_RESET = 0b000, +} mpu6050_dhpf_t; + +typedef enum +{ + MPU6050_DLPF_6 = 0b110, + MPU6050_DLPF_5 = 0b101, + MPU6050_DLPF_4 = 0b100, + MPU6050_DLPF_3 = 0b011, + MPU6050_DLPF_2 = 0b010, + MPU6050_DLPF_1 = 0b001, + MPU6050_DLPF_0 = 0b000, +} mpu6050_dlpf_t; + +class MPU6050 +{ + public: + + bool begin(mpu6050_dps_t scale = MPU6050_SCALE_2000DPS, mpu6050_range_t range = MPU6050_RANGE_2G, int mpua = MPU6050_ADDRESS); + + void setClockSource(mpu6050_clockSource_t source); + void setScale(mpu6050_dps_t scale); + void setRange(mpu6050_range_t range); + mpu6050_clockSource_t getClockSource(void); + mpu6050_dps_t getScale(void); + mpu6050_range_t getRange(void); + void setDHPFMode(mpu6050_dhpf_t dhpf); + void setDLPFMode(mpu6050_dlpf_t dlpf); + mpu6050_onDelay_t getAccelPowerOnDelay(); + void setAccelPowerOnDelay(mpu6050_onDelay_t delay); + + uint8_t getIntStatus(void); + + bool getIntZeroMotionEnabled(void); + void setIntZeroMotionEnabled(bool state); + bool getIntMotionEnabled(void); + void setIntMotionEnabled(bool state); + bool getIntFreeFallEnabled(void); + void setIntFreeFallEnabled(bool state); + + uint8_t getMotionDetectionThreshold(void); + void setMotionDetectionThreshold(uint8_t threshold); + uint8_t getMotionDetectionDuration(void); + void setMotionDetectionDuration(uint8_t duration); + + uint8_t getZeroMotionDetectionThreshold(void); + void setZeroMotionDetectionThreshold(uint8_t threshold); + uint8_t getZeroMotionDetectionDuration(void); + void setZeroMotionDetectionDuration(uint8_t duration); + + uint8_t getFreeFallDetectionThreshold(void); + void setFreeFallDetectionThreshold(uint8_t threshold); + uint8_t getFreeFallDetectionDuration(void); + void setFreeFallDetectionDuration(uint8_t duration); + + bool getSleepEnabled(void); + void setSleepEnabled(bool state); + bool getI2CMasterModeEnabled(void); + void setI2CMasterModeEnabled(bool state); + bool getI2CBypassEnabled(void); + void setI2CBypassEnabled(bool state); + + float readTemperature(void); + Activites readActivites(void); + + int16_t getGyroOffsetX(void); + void setGyroOffsetX(int16_t offset); + int16_t getGyroOffsetY(void); + void setGyroOffsetY(int16_t offset); + int16_t getGyroOffsetZ(void); + void setGyroOffsetZ(int16_t offset); + + int16_t getAccelOffsetX(void); + void setAccelOffsetX(int16_t offset); + int16_t getAccelOffsetY(void); + void setAccelOffsetY(int16_t offset); + int16_t getAccelOffsetZ(void); + void setAccelOffsetZ(int16_t offset); + + void calibrateGyro(uint8_t samples = 50); + void setThreshold(uint8_t multiple = 1); + uint8_t getThreshold(void); + + Vector readRawGyro(void); + Vector readNormalizeGyro(void); + + Vector readRawAccel(void); + Vector readNormalizeAccel(void); + Vector readScaledAccel(void); + + private: + Vector ra, rg; // Raw vectors + Vector na, ng; // Normalized vectors + Vector tg, dg; // Threshold and Delta for Gyro + Vector th; // Threshold + Activites a; // Activities + + float dpsPerDigit, rangePerDigit; + float actualThreshold; + bool useCalibrate; + int mpuAddress; + + uint8_t fastRegister8(uint8_t reg); + + uint8_t readRegister8(uint8_t reg); + void writeRegister8(uint8_t reg, uint8_t value); + + int16_t readRegister16(uint8_t reg); + void writeRegister16(uint8_t reg, int16_t value); + + bool readRegisterBit(uint8_t reg, uint8_t pos); + void writeRegisterBit(uint8_t reg, uint8_t pos, bool state); + +}; + +#endif diff --git a/HandMotion/include/definitions.h b/HandMotion/include/definitions.h index a5dcd1e..82107aa 100644 --- a/HandMotion/include/definitions.h +++ b/HandMotion/include/definitions.h @@ -29,9 +29,14 @@ //output pins -#define LED_PIN 16//2 //LED PIN INTERNAL -#define TRIGGER_PIN 0 //Force reset +#define OnBoard_LED_PIN 2 // OnBoard_LEDLED PIN +#define LED_PIN 2//13 // LED PIN +#define TRIGGER_PIN 0 // Force reset push button pin +#define blinkRate_wm 150 // ms blink rate while waiting for wifimanager button wiFi change +#define blinkRate_ws 1000 // ms blink rate while working successfully + +#define Yaw_Offset 0.5 // yaw offset for calibration /* ======= ========== ==================================================== VCC VU (5V USB) 3.3V diff --git a/HandMotion/src/MPU6050.cpp b/HandMotion/src/MPU6050.cpp new file mode 100644 index 0000000..d9cc131 --- /dev/null +++ b/HandMotion/src/MPU6050.cpp @@ -0,0 +1,749 @@ +/* +MPU6050.cpp - Class file for the MPU6050 Triple Axis Gyroscope & Accelerometer Arduino Library. + +Version: 1.0.3 +(c) 2014-2015 Korneliusz Jarzebski +www.jarzebski.pl + +This program is free software: you can redistribute it and/or modify +it under the terms of the version 3 GNU General Public License as +published by the Free Software Foundation. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . +*/ + +#if ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +#include +#include + +#include + +bool MPU6050::begin(mpu6050_dps_t scale, mpu6050_range_t range, int mpua) +{ + // Set Address + mpuAddress = mpua; + + Wire.begin(); + + // Reset calibrate values + dg.XAxis = 0; + dg.YAxis = 0; + dg.ZAxis = 0; + useCalibrate = false; + + // Reset threshold values + tg.XAxis = 0; + tg.YAxis = 0; + tg.ZAxis = 0; + actualThreshold = 0; + + // Check MPU6050 Who Am I Register + if (fastRegister8(MPU6050_REG_WHO_AM_I) != 0x68) + { + return false; + } + + // Set Clock Source + setClockSource(MPU6050_CLOCK_PLL_XGYRO); + + // Set Scale & Range + setScale(scale); + setRange(range); + + // Disable Sleep Mode + setSleepEnabled(false); + + return true; +} + +void MPU6050::setScale(mpu6050_dps_t scale) +{ + uint8_t value; + + switch (scale) + { + case MPU6050_SCALE_250DPS: + dpsPerDigit = .007633f; + break; + case MPU6050_SCALE_500DPS: + dpsPerDigit = .015267f; + break; + case MPU6050_SCALE_1000DPS: + dpsPerDigit = .030487f; + break; + case MPU6050_SCALE_2000DPS: + dpsPerDigit = .060975f; + break; + default: + break; + } + + value = readRegister8(MPU6050_REG_GYRO_CONFIG); + value &= 0b11100111; + value |= (scale << 3); + writeRegister8(MPU6050_REG_GYRO_CONFIG, value); +} + +mpu6050_dps_t MPU6050::getScale(void) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_GYRO_CONFIG); + value &= 0b00011000; + value >>= 3; + return (mpu6050_dps_t)value; +} + +void MPU6050::setRange(mpu6050_range_t range) +{ + uint8_t value; + + switch (range) + { + case MPU6050_RANGE_2G: + rangePerDigit = .000061f; + break; + case MPU6050_RANGE_4G: + rangePerDigit = .000122f; + break; + case MPU6050_RANGE_8G: + rangePerDigit = .000244f; + break; + case MPU6050_RANGE_16G: + rangePerDigit = .0004882f; + break; + default: + break; + } + + value = readRegister8(MPU6050_REG_ACCEL_CONFIG); + value &= 0b11100111; + value |= (range << 3); + writeRegister8(MPU6050_REG_ACCEL_CONFIG, value); +} + +mpu6050_range_t MPU6050::getRange(void) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_ACCEL_CONFIG); + value &= 0b00011000; + value >>= 3; + return (mpu6050_range_t)value; +} + +void MPU6050::setDHPFMode(mpu6050_dhpf_t dhpf) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_ACCEL_CONFIG); + value &= 0b11111000; + value |= dhpf; + writeRegister8(MPU6050_REG_ACCEL_CONFIG, value); +} + +void MPU6050::setDLPFMode(mpu6050_dlpf_t dlpf) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_CONFIG); + value &= 0b11111000; + value |= dlpf; + writeRegister8(MPU6050_REG_CONFIG, value); +} + +void MPU6050::setClockSource(mpu6050_clockSource_t source) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_PWR_MGMT_1); + value &= 0b11111000; + value |= source; + writeRegister8(MPU6050_REG_PWR_MGMT_1, value); +} + +mpu6050_clockSource_t MPU6050::getClockSource(void) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_PWR_MGMT_1); + value &= 0b00000111; + return (mpu6050_clockSource_t)value; +} + +bool MPU6050::getSleepEnabled(void) +{ + return readRegisterBit(MPU6050_REG_PWR_MGMT_1, 6); +} + +void MPU6050::setSleepEnabled(bool state) +{ + writeRegisterBit(MPU6050_REG_PWR_MGMT_1, 6, state); +} + +bool MPU6050::getIntZeroMotionEnabled(void) +{ + return readRegisterBit(MPU6050_REG_INT_ENABLE, 5); +} + +void MPU6050::setIntZeroMotionEnabled(bool state) +{ + writeRegisterBit(MPU6050_REG_INT_ENABLE, 5, state); +} + +bool MPU6050::getIntMotionEnabled(void) +{ + return readRegisterBit(MPU6050_REG_INT_ENABLE, 6); +} + +void MPU6050::setIntMotionEnabled(bool state) +{ + writeRegisterBit(MPU6050_REG_INT_ENABLE, 6, state); +} + +bool MPU6050::getIntFreeFallEnabled(void) +{ + return readRegisterBit(MPU6050_REG_INT_ENABLE, 7); +} + +void MPU6050::setIntFreeFallEnabled(bool state) +{ + writeRegisterBit(MPU6050_REG_INT_ENABLE, 7, state); +} + +uint8_t MPU6050::getMotionDetectionThreshold(void) +{ + return readRegister8(MPU6050_REG_MOT_THRESHOLD); +} + +void MPU6050::setMotionDetectionThreshold(uint8_t threshold) +{ + writeRegister8(MPU6050_REG_MOT_THRESHOLD, threshold); +} + +uint8_t MPU6050::getMotionDetectionDuration(void) +{ + return readRegister8(MPU6050_REG_MOT_DURATION); +} + +void MPU6050::setMotionDetectionDuration(uint8_t duration) +{ + writeRegister8(MPU6050_REG_MOT_DURATION, duration); +} + +uint8_t MPU6050::getZeroMotionDetectionThreshold(void) +{ + return readRegister8(MPU6050_REG_ZMOT_THRESHOLD); +} + +void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold) +{ + writeRegister8(MPU6050_REG_ZMOT_THRESHOLD, threshold); +} + +uint8_t MPU6050::getZeroMotionDetectionDuration(void) +{ + return readRegister8(MPU6050_REG_ZMOT_DURATION); +} + +void MPU6050::setZeroMotionDetectionDuration(uint8_t duration) +{ + writeRegister8(MPU6050_REG_ZMOT_DURATION, duration); +} + +uint8_t MPU6050::getFreeFallDetectionThreshold(void) +{ + return readRegister8(MPU6050_REG_FF_THRESHOLD); +} + +void MPU6050::setFreeFallDetectionThreshold(uint8_t threshold) +{ + writeRegister8(MPU6050_REG_FF_THRESHOLD, threshold); +} + +uint8_t MPU6050::getFreeFallDetectionDuration(void) +{ + return readRegister8(MPU6050_REG_FF_DURATION); +} + +void MPU6050::setFreeFallDetectionDuration(uint8_t duration) +{ + writeRegister8(MPU6050_REG_FF_DURATION, duration); +} + +bool MPU6050::getI2CMasterModeEnabled(void) +{ + return readRegisterBit(MPU6050_REG_USER_CTRL, 5); +} + +void MPU6050::setI2CMasterModeEnabled(bool state) +{ + writeRegisterBit(MPU6050_REG_USER_CTRL, 5, state); +} + +void MPU6050::setI2CBypassEnabled(bool state) +{ + return writeRegisterBit(MPU6050_REG_INT_PIN_CFG, 1, state); +} + +bool MPU6050::getI2CBypassEnabled(void) +{ + return readRegisterBit(MPU6050_REG_INT_PIN_CFG, 1); +} + +void MPU6050::setAccelPowerOnDelay(mpu6050_onDelay_t delay) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_MOT_DETECT_CTRL); + value &= 0b11001111; + value |= (delay << 4); + writeRegister8(MPU6050_REG_MOT_DETECT_CTRL, value); +} + +mpu6050_onDelay_t MPU6050::getAccelPowerOnDelay(void) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_MOT_DETECT_CTRL); + value &= 0b00110000; + return (mpu6050_onDelay_t)(value >> 4); +} + +uint8_t MPU6050::getIntStatus(void) +{ + return readRegister8(MPU6050_REG_INT_STATUS); +} + +Activites MPU6050::readActivites(void) +{ + uint8_t data = readRegister8(MPU6050_REG_INT_STATUS); + + a.isOverflow = ((data >> 4) & 1); + a.isFreeFall = ((data >> 7) & 1); + a.isInactivity = ((data >> 5) & 1); + a.isActivity = ((data >> 6) & 1); + a.isDataReady = ((data >> 0) & 1); + + data = readRegister8(MPU6050_REG_MOT_DETECT_STATUS); + + a.isNegActivityOnX = ((data >> 7) & 1); + a.isPosActivityOnX = ((data >> 6) & 1); + + a.isNegActivityOnY = ((data >> 5) & 1); + a.isPosActivityOnY = ((data >> 4) & 1); + + a.isNegActivityOnZ = ((data >> 3) & 1); + a.isPosActivityOnZ = ((data >> 2) & 1); + + return a; +} + +Vector MPU6050::readRawAccel(void) +{ + Wire.beginTransmission(mpuAddress); + #if ARDUINO >= 100 + Wire.write(MPU6050_REG_ACCEL_XOUT_H); + #else + Wire.send(MPU6050_REG_ACCEL_XOUT_H); + #endif + Wire.endTransmission(); + + Wire.beginTransmission(mpuAddress); + Wire.requestFrom(mpuAddress, 6); + + while (Wire.available() < 6); + + #if ARDUINO >= 100 + uint8_t xha = Wire.read(); + uint8_t xla = Wire.read(); + uint8_t yha = Wire.read(); + uint8_t yla = Wire.read(); + uint8_t zha = Wire.read(); + uint8_t zla = Wire.read(); + #else + uint8_t xha = Wire.receive(); + uint8_t xla = Wire.receive(); + uint8_t yha = Wire.receive(); + uint8_t yla = Wire.receive(); + uint8_t zha = Wire.receive(); + uint8_t zla = Wire.receive(); + #endif + + ra.XAxis = xha << 8 | xla; + ra.YAxis = yha << 8 | yla; + ra.ZAxis = zha << 8 | zla; + + return ra; +} + +Vector MPU6050::readNormalizeAccel(void) +{ + readRawAccel(); + + na.XAxis = ra.XAxis * rangePerDigit * 9.80665f; + na.YAxis = ra.YAxis * rangePerDigit * 9.80665f; + na.ZAxis = ra.ZAxis * rangePerDigit * 9.80665f; + + return na; +} + +Vector MPU6050::readScaledAccel(void) +{ + readRawAccel(); + + na.XAxis = ra.XAxis * rangePerDigit; + na.YAxis = ra.YAxis * rangePerDigit; + na.ZAxis = ra.ZAxis * rangePerDigit; + + return na; +} + + +Vector MPU6050::readRawGyro(void) +{ + Wire.beginTransmission(mpuAddress); + #if ARDUINO >= 100 + Wire.write(MPU6050_REG_GYRO_XOUT_H); + #else + Wire.send(MPU6050_REG_GYRO_XOUT_H); + #endif + Wire.endTransmission(); + + Wire.beginTransmission(mpuAddress); + Wire.requestFrom(mpuAddress, 6); + + while (Wire.available() < 6); + + #if ARDUINO >= 100 + uint8_t xha = Wire.read(); + uint8_t xla = Wire.read(); + uint8_t yha = Wire.read(); + uint8_t yla = Wire.read(); + uint8_t zha = Wire.read(); + uint8_t zla = Wire.read(); + #else + uint8_t xha = Wire.receive(); + uint8_t xla = Wire.receive(); + uint8_t yha = Wire.receive(); + uint8_t yla = Wire.receive(); + uint8_t zha = Wire.receive(); + uint8_t zla = Wire.receive(); + #endif + + rg.XAxis = xha << 8 | xla; + rg.YAxis = yha << 8 | yla; + rg.ZAxis = zha << 8 | zla; + + return rg; +} + +Vector MPU6050::readNormalizeGyro(void) +{ + readRawGyro(); + + if (useCalibrate) + { + ng.XAxis = (rg.XAxis - dg.XAxis) * dpsPerDigit; + ng.YAxis = (rg.YAxis - dg.YAxis) * dpsPerDigit; + ng.ZAxis = (rg.ZAxis - dg.ZAxis) * dpsPerDigit; + } else + { + ng.XAxis = rg.XAxis * dpsPerDigit; + ng.YAxis = rg.YAxis * dpsPerDigit; + ng.ZAxis = rg.ZAxis * dpsPerDigit; + } + + if (actualThreshold) + { + if (abs(ng.XAxis) < tg.XAxis) ng.XAxis = 0; + if (abs(ng.YAxis) < tg.YAxis) ng.YAxis = 0; + if (abs(ng.ZAxis) < tg.ZAxis) ng.ZAxis = 0; + } + + return ng; +} + +float MPU6050::readTemperature(void) +{ + int16_t T; + T = readRegister16(MPU6050_REG_TEMP_OUT_H); + return (float)T/340 + 36.53; +} + +int16_t MPU6050::getGyroOffsetX(void) +{ + return readRegister16(MPU6050_REG_GYRO_XOFFS_H); +} + +int16_t MPU6050::getGyroOffsetY(void) +{ + return readRegister16(MPU6050_REG_GYRO_YOFFS_H); +} + +int16_t MPU6050::getGyroOffsetZ(void) +{ + return readRegister16(MPU6050_REG_GYRO_ZOFFS_H); +} + +void MPU6050::setGyroOffsetX(int16_t offset) +{ + writeRegister16(MPU6050_REG_GYRO_XOFFS_H, offset); +} + +void MPU6050::setGyroOffsetY(int16_t offset) +{ + writeRegister16(MPU6050_REG_GYRO_YOFFS_H, offset); +} + +void MPU6050::setGyroOffsetZ(int16_t offset) +{ + writeRegister16(MPU6050_REG_GYRO_ZOFFS_H, offset); +} + +int16_t MPU6050::getAccelOffsetX(void) +{ + return readRegister16(MPU6050_REG_ACCEL_XOFFS_H); +} + +int16_t MPU6050::getAccelOffsetY(void) +{ + return readRegister16(MPU6050_REG_ACCEL_YOFFS_H); +} + +int16_t MPU6050::getAccelOffsetZ(void) +{ + return readRegister16(MPU6050_REG_ACCEL_ZOFFS_H); +} + +void MPU6050::setAccelOffsetX(int16_t offset) +{ + writeRegister16(MPU6050_REG_ACCEL_XOFFS_H, offset); +} + +void MPU6050::setAccelOffsetY(int16_t offset) +{ + writeRegister16(MPU6050_REG_ACCEL_YOFFS_H, offset); +} + +void MPU6050::setAccelOffsetZ(int16_t offset) +{ + writeRegister16(MPU6050_REG_ACCEL_ZOFFS_H, offset); +} + +// Calibrate algorithm +void MPU6050::calibrateGyro(uint8_t samples) +{ + // Set calibrate + useCalibrate = true; + + // Reset values + float sumX = 0; + float sumY = 0; + float sumZ = 0; + float sigmaX = 0; + float sigmaY = 0; + float sigmaZ = 0; + + // Read n-samples + for (uint8_t i = 0; i < samples; ++i) + { + readRawGyro(); + sumX += rg.XAxis; + sumY += rg.YAxis; + sumZ += rg.ZAxis; + + sigmaX += rg.XAxis * rg.XAxis; + sigmaY += rg.YAxis * rg.YAxis; + sigmaZ += rg.ZAxis * rg.ZAxis; + + delay(5); + } + + // Calculate delta vectors + dg.XAxis = sumX / samples; + dg.YAxis = sumY / samples; + dg.ZAxis = sumZ / samples; + + // Calculate threshold vectors + th.XAxis = sqrt((sigmaX / 50) - (dg.XAxis * dg.XAxis)); + th.YAxis = sqrt((sigmaY / 50) - (dg.YAxis * dg.YAxis)); + th.ZAxis = sqrt((sigmaZ / 50) - (dg.ZAxis * dg.ZAxis)); + + // If already set threshold, recalculate threshold vectors + if (actualThreshold > 0) + { + setThreshold(actualThreshold); + } +} + +// Get current threshold value +uint8_t MPU6050::getThreshold(void) +{ + return actualThreshold; +} + +// Set treshold value +void MPU6050::setThreshold(uint8_t multiple) +{ + if (multiple > 0) + { + // If not calibrated, need calibrate + if (!useCalibrate) + { + calibrateGyro(); + } + + // Calculate threshold vectors + tg.XAxis = th.XAxis * multiple; + tg.YAxis = th.YAxis * multiple; + tg.ZAxis = th.ZAxis * multiple; + } else + { + // No threshold + tg.XAxis = 0; + tg.YAxis = 0; + tg.ZAxis = 0; + } + + // Remember old threshold value + actualThreshold = multiple; +} + +// Fast read 8-bit from register +uint8_t MPU6050::fastRegister8(uint8_t reg) +{ + uint8_t value; + + Wire.beginTransmission(mpuAddress); + #if ARDUINO >= 100 + Wire.write(reg); + #else + Wire.send(reg); + #endif + Wire.endTransmission(); + + Wire.beginTransmission(mpuAddress); + Wire.requestFrom(mpuAddress, 1); + #if ARDUINO >= 100 + value = Wire.read(); + #else + value = Wire.receive(); + #endif; + Wire.endTransmission(); + + return value; +} + +// Read 8-bit from register +uint8_t MPU6050::readRegister8(uint8_t reg) +{ + uint8_t value; + + Wire.beginTransmission(mpuAddress); + #if ARDUINO >= 100 + Wire.write(reg); + #else + Wire.send(reg); + #endif + Wire.endTransmission(); + + Wire.beginTransmission(mpuAddress); + Wire.requestFrom(mpuAddress, 1); + while(!Wire.available()) {}; + #if ARDUINO >= 100 + value = Wire.read(); + #else + value = Wire.receive(); + #endif; + Wire.endTransmission(); + + return value; +} + +// Write 8-bit to register +void MPU6050::writeRegister8(uint8_t reg, uint8_t value) +{ + Wire.beginTransmission(mpuAddress); + + #if ARDUINO >= 100 + Wire.write(reg); + Wire.write(value); + #else + Wire.send(reg); + Wire.send(value); + #endif + Wire.endTransmission(); +} + +int16_t MPU6050::readRegister16(uint8_t reg) +{ + int16_t value; + Wire.beginTransmission(mpuAddress); + #if ARDUINO >= 100 + Wire.write(reg); + #else + Wire.send(reg); + #endif + Wire.endTransmission(); + + Wire.beginTransmission(mpuAddress); + Wire.requestFrom(mpuAddress, 2); + while(!Wire.available()) {}; + #if ARDUINO >= 100 + uint8_t vha = Wire.read(); + uint8_t vla = Wire.read(); + #else + uint8_t vha = Wire.receive(); + uint8_t vla = Wire.receive(); + #endif; + Wire.endTransmission(); + + value = vha << 8 | vla; + + return value; +} + +void MPU6050::writeRegister16(uint8_t reg, int16_t value) +{ + Wire.beginTransmission(mpuAddress); + + #if ARDUINO >= 100 + Wire.write(reg); + Wire.write((uint8_t)(value >> 8)); + Wire.write((uint8_t)value); + #else + Wire.send(reg); + Wire.send((uint8_t)(value >> 8)); + Wire.send((uint8_t)value); + #endif + Wire.endTransmission(); +} + +// Read register bit +bool MPU6050::readRegisterBit(uint8_t reg, uint8_t pos) +{ + uint8_t value; + value = readRegister8(reg); + return ((value >> pos) & 1); +} + +// Write register bit +void MPU6050::writeRegisterBit(uint8_t reg, uint8_t pos, bool state) +{ + uint8_t value; + value = readRegister8(reg); + + if (state) + { + value |= (1 << pos); + } else + { + value &= ~(1 << pos); + } + + writeRegister8(reg, value); +} diff --git a/handmotion/src/main.cpp b/handmotion/src/main.cpp index 5676bf7..89db954 100644 --- a/handmotion/src/main.cpp +++ b/handmotion/src/main.cpp @@ -1,240 +1,610 @@ -#include "Arduino.h" -#include -#include -#include -#include -#include "I2Cdev.h" -#include "MPU6050_6Axis_MotionApps20.h" -#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE -#include "Wire.h" -#endif - -#define WLAN_SSID "wifi_name" -#define WLAN_PASSWORD "wifi_password" -#define MQTT_TOPIC "mqtt_topic" -#define MQTT_HOST "mqtt_host" -#define MQTT_PORT 1883 -#define SERIAL_BAUDRATE 115200 -#define M_PI 3.14159265358979323846 - -#define DEBUG 2 -#if DEBUG == 1 -#define debug(x) Serial.print(x) -#define debugln(x) Serial.println(x) -#define debugf(x, y) Serial.printf(x, y) -#else -#define debug(x) -#define debugln(x) -#define debugf(x, y) -#endif - -WiFiEventHandler _wifi_connect_handler; -WiFiEventHandler _wifi_disconnect_handler; -AsyncMqttClient _mqtt_client; -Ticker _mqtt_reconnect_timer; -Ticker _wifi_reconnect_timer; -MPU6050 _mpu; -// MPU control/status vars -bool _dmp_ready; // set true if DMP init was successful -uint8_t _dev_status; // return status after each device operation (0 = success, !0 = error) -uint8_t _fifo_buffer[64]; // FIFO storage buffer -// orientation/motion vars -Quaternion _quaternion; // [w, x, y, z] quaternion container -VectorFloat _gravity; // [x, y, z] gravity vector -float _yaw_pitch_roll[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector - -struct Data -{ - float pitch; // Rotation around the side-to-side axis - float roll; // Rotation around the front-to-back axis - float yaw; // Rotation around the vertical axis -}; -void _connect_to_WiFi() -{ - debugln("Connecting to Wi-Fi..."); - WiFi.begin(WLAN_SSID, WLAN_PASSWORD); -} +#define DEBUG 1 //set 1 for serial read statements -void _connect_to_MQTT() -{ - debugln("Connecting to MQTT..."); - _mqtt_client.connect(); -} +#include +//All definitions and libraries used have been added to the definitions.h file under include -void _on_WiFi_connect(const WiFiEventStationModeGotIP &event) -{ - debugln("Connected to Wi-Fi."); - _connect_to_MQTT(); -} +void (* resetFunc)(void) = 0; // creating pointer at memory address 0 + +//RTC_DATA_ATTR unsigned int MQTT_conf = 0; //CHECK FOR CONFIG DUE TO MQTT SERVER OFF + +WiFiClient espClient; +PubSubClient client(espClient); +unsigned long lastMsg = 0; +#define MSG_BUFFER_SIZE (50) +char msg[MSG_BUFFER_SIZE]; +int value = 0; + + +DoubleResetDetector *drd; +bool shouldSaveConfig = false; + +#include +// Timers +unsigned long timer = 0; +float timeStep = 0.01; + +float yaw = 0; +float yaw_P = 0; +float yaw_A = 0; +float pitch_g = 0 ; +float roll_g = 0 ; + +MPU6050 mpu; + +uint32_t frt_sensor; + +//MQTT +char MQTTIP[50] = "pi.local"; +int MQTT_PORT = 1883; +bool EnableDebug = true; + + +WiFiManager wm; -void _on_WiFi_disconnect(const WiFiEventStationModeDisconnected &event) +void saveConfigFile() { - debugln("Disconnected from Wi-Fi."); - _mqtt_reconnect_timer.detach(); // ensure we don't reconnect to MQTT while reconnecting to Wi-Fi - _wifi_reconnect_timer.once(2, _connect_to_WiFi); + debugln(F("Saving config")); + StaticJsonDocument<512> json; + json["MQTTIP"] = MQTTIP; + json["MQTT_PORT"] = MQTT_PORT; + json["EnableDebug"] = EnableDebug; + + File configFile = SPIFFS.open(JSON_CONFIG_FILE, "w"); + if (!configFile) + { + debugln(F("failed to open config file for writing")); + } + + serializeJsonPretty(json, Serial); + if (serializeJson(json, configFile) == 0) + { + debugln(F("Failed to write to file")); + } + configFile.close(); } -void _on_MQTT_connect(bool sessionPresent) -{ - debugln("Connected to MQTT."); - debug("Session present: "); - debugln(sessionPresent); - uint16_t packetIdSub = _mqtt_client.subscribe(MQTT_TOPIC, 2); - debug("Subscribing at QoS 2, packetId: "); - debugln(packetIdSub); - uint16_t packetIdPub = _mqtt_client.publish(MQTT_TOPIC, 2, true, "test 3"); - debug("Publishing at QoS 2, packetId: "); - debugln(packetIdPub); -} - -void onMqttDisconnect(AsyncMqttClientDisconnectReason reason) +bool loadConfigFile() { - debugf("Disconnected from MQTT: %u.\n", static_cast(reason)); + //clean FS, for testing + // SPIFFS.format(); + + //read configuration from FS json + debugln(F("mounting FS...")); + + bool success = SPIFFS.begin(); + // if (SPIFFS.begin(false) || SPIFFS.begin(true)) + if (success) + { + debugln(F("mounted file system")); + if (SPIFFS.exists(JSON_CONFIG_FILE)) + { + //file exists, reading and loading + debugln(F("reading config file")); + File configFile = SPIFFS.open(JSON_CONFIG_FILE, "r"); + if (configFile) + { + debugln(F("opened config file")); + StaticJsonDocument<512> json; + DeserializationError error = deserializeJson(json, configFile); + serializeJsonPretty(json, Serial); + if (!error) + { + debugln(F("\nparsed json")); + + strcpy(MQTTIP, json["MQTTIP"]); + MQTT_PORT = json["MQTT_PORT"].as(); + EnableDebug = json["EnableDebug"].as(); - if (WiFi.isConnected()) + return true; + } + else + { + debugln(F("failed to load json config")); + } + } + } + } + else { - _mqtt_reconnect_timer.once(2, _connect_to_MQTT); + debugln(F("failed to mount FS")); } -} -void onMqttSubscribe(uint16_t packetId, uint8_t qos) + + //end read + return false; +} +//callback notifying us of the need to save config +void saveConfigCallback() { - debugln("Subscribe acknowledged."); - debug(" packetId: "); - debugln(packetId); + debugln(F("Should save config")); + shouldSaveConfig = true; } -void onMqttUnsubscribe(uint16_t packetId) +void configModeCallback(WiFiManager *myWiFiManager) { - debugln("Unsubscribe acknowledged."); - debug(" packetId: "); - debugln(packetId); + debugln(F("Entered Conf Mode")); + + debug("Config SSID: "); + debugln(myWiFiManager->getConfigPortalSSID()); + + debug("Config IP Address: "); + debugln(WiFi.softAPIP()); } -void SendData(Data data) +void LED(bool state) { - String output; - StaticJsonDocument<96> doc; + debugln("LED: " + String(state)); + digitalWrite(LED_PIN, state); +} - doc["pitch"] = data.pitch; - doc["roll"] = data.roll; - doc["yaw"] = data.yaw; +bool force_state = false; +void blink_para(int timeout_){ + + unsigned int previous_time = millis(); + unsigned int current_time = millis(); - serializeJson(doc, output); + bool Lstate = true; - uint16_t packetIdPub2 = _mqtt_client.publish(MQTT_TOPIC, 2, true, output.c_str()); - Serial.print("Publishing at QoS 2, packetId: "); - Serial.println(packetIdPub2); + while((current_time - previous_time) < timeout_){ + + volatile unsigned int previous_t; + unsigned int current_t = millis(); + int t = blinkRate_wm; + + if((current_t - previous_t) > t){ + LED(Lstate); + Lstate = !Lstate; + previous_t = current_t; + } + + if(digitalRead(TRIGGER_PIN) == LOW) { + force_state = true; + + debugln(F("button pressed")); + LED(true); + break; + } + current_time = millis(); + } + LED(true); } + -void onMqttMessage(char *topic, char *payload, AsyncMqttClientMessageProperties properties, size_t len, size_t index, size_t total) -{ - debugln("Publish received."); - debug(" topic: "); - debugln(topic); -} +bool forceConfig = false; +void waitForceConfig(){ + + drd = new DoubleResetDetector(DRD_TIMEOUT, DRD_ADDRESS); + if (drd->detectDoubleReset()) + { + debugln(F("Forcing config mode as there was a Double reset detected -- currently disabled")); + // forceConfig = true; // uncomment to enable double reset funct + } + + bool spiffsSetup = loadConfigFile(); + if (!spiffsSetup) + { + debugln(F("Forcing config mode as there is no saved config")); + forceConfig = true; + } + + debugln(F("start b check")); + blink_para(3000); + force_state == 1 ? forceConfig = true : forceConfig = false; + debugln(F("end of b check")); + + //if(MQTT_conf == 1){ + // MQTT_conf = 0; + // forceConfig = true; + // debugln(F("Forcing config mode to change MQTT Parameters -- could not connect to MQTT server")); + //} + + //WiFi.disconnect(); + WiFi.mode(WIFI_STA); // explicitly set mode, esp defaults to STA+AP + delay(10); -void onMqttPublish(uint16_t packetId) -{ - debugln("Publish acknowledged."); - debug(" packetId: "); - debugln(packetId); } -void imuInit() -{ +void WifiManagersetup(){ + + // wm.resetSettings(); // wipe settings + + //WiFiManager wm; + + //wm.resetSettings(); // wipe settings + //set config save notify callback + wm.setSaveConfigCallback(saveConfigCallback); + //set callback that gets called when connecting to previous WiFi fails, and enters Access Point mode + wm.setAPCallback(configModeCallback); + + //set static ip + // wm.setSTAStaticIPConfig(IPAddress(10,0,1,99), IPAddress(10,0,1,1), IPAddress(255,255,255,0)); // set static ip,gw,sn + // wm.setShowStaticFields(true); // force show static ip fields + // wm.setShowDnsFields(true); // force show dns field always + + // wm.setConnectTimeout(20); // how long to try to connect for before continuing + wm.setConfigPortalTimeout(60); // auto close configportal after n seconds + // wm.setCaptivePortalEnable(false); // disable captive portal redirection + // wm.setAPClientCheck(true); // avoid timeout if client connected to softap + + // wifi scan settings + // wm.setRemoveDuplicateAPs(false); // do not remove duplicate ap names (true) + // wm.setMinimumSignalQuality(20); // set min RSSI (percentage) to show in scans, null = 8% + // wm.setShowInfoErase(false); // do not show erase button on info page + // wm.setScanDispPerc(true); // show RSSI as percentage not graph icons + + // wm.setBreakAfterConfig(true); // always exit configportal even if wifi save fails + + + //--- additional Configs params --- + + // Text box for IP adress + WiFiManagerParameter custom_text_box("key_text", "Enter MQTT server IP Address", MQTTIP, 50); // 50 == max length + + // Text box for port + char convertedValue[6]; + sprintf(convertedValue, "%d", MQTT_PORT); // Need to convert to string to display a default value. + + WiFiManagerParameter custom_text_box_num("key_num", "Enter MQTT server port", convertedValue, 7); // 7 == max length + + //Check Box + char *customHtml; + if (EnableDebug) + { + customHtml = "type=\"checkbox\" checked"; + } + else { - bool _dmp_ready = false; // set true if DMP init was successful - -// join I2C bus (I2Cdev library doesn't do this automatically) -#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE - Wire.begin(); - Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties -#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE - Fastwire::setup(400, true); -#endif - // initialize device - debugln("INITIALIZING I2C DEVICES..."); - _mpu.initialize(); - - // verify connection - debugln("TESTING DEVICE CONNECTIONS..."); - debugln(_mpu.testConnection() ? F("MPU6050 CONNECTION SUCCESSFUL") : F("MPU6050 CONNECTION FAILED")); - - // load and configure the DMP - debugln("INITIALIZING DMP..."); - _dev_status = _mpu.dmpInitialize(); - - // supply your own gyro offsets here, scaled for min sensitivity - _mpu.setXGyroOffset(220); - _mpu.setYGyroOffset(76); - _mpu.setZGyroOffset(-85); - _mpu.setZAccelOffset(1788); // 1688 factory default for my test chip - - // make sure it worked (returns 0 if so) - if (_dev_status == 0) + customHtml = "type=\"checkbox\""; + } + WiFiManagerParameter custom_checkbox("key_bool", "Enable Debug", "T", 2, customHtml); + + wm.addParameter(&custom_text_box); + wm.addParameter(&custom_text_box_num); + wm.addParameter(&custom_checkbox); + + debugln(F("hello")); + + if (forceConfig) + { + if (!wm.startConfigPortal("HM_Phanthom", "clockX20")) { - // Calibration Time: generate offsets and calibrate our MPU6050 - _mpu.CalibrateAccel(6); - _mpu.CalibrateGyro(6); - _mpu.PrintActiveOffsets(); - // turn on the DMP, now that it's ready - debugln("ENABLING DMP..."); - _mpu.setDMPEnabled(true); - - // set our DMP Ready flag so the main function knows it's okay to use it - _dmp_ready = true; + debugln(F("failed to connect and hit timeout")); + delay(300); + + //reset and try again, or maybe put it to deep sleep + ESP.restart(); + delay(5000); } - else + } + else + { + if (!wm.autoConnect("HM_Phanthom", "clockX20")) { - debug("DMP INITIALIZING FAILED (CODE "); - debug(_dev_status); - debugln(")"); + debugln(F("failed to connect and hit timeout")); + delay(3000); + // if we still have not connected restart and try all over again + ESP.restart(); + + delay(5000); + } + } + + // If we get here, we are connected to the WiFi + + debugln(F("")); + debugln(F("WiFi connected")); + debugln(F("IP address: ")); + debugln(WiFi.localIP()); + + // dealing with the user config values + + // Copy the IP value + strncpy(MQTTIP, custom_text_box.getValue(), sizeof(MQTTIP)); + debug(F("MQTTIP: ")); + debugln(MQTTIP); + + //Convert the PORT value + MQTT_PORT = atoi(custom_text_box_num.getValue()); + debug(F("MQTT_PORT: ")); + debugln(MQTT_PORT); + + //Handle the bool value + EnableDebug = (strncmp(custom_checkbox.getValue(), "T", 1) == 0); + debug(F("EnableDebug: ")); + if (EnableDebug) + { + debugln(F("Debug true")); + } + else + { + debugln(F("Debug false")); + } + + //save the custom parameters to FS + if (shouldSaveConfig) + { + saveConfigFile(); + } +} + + +void callback(char* topic, byte* payload, unsigned int length) { + debug("Message arrived ["); + debug(topic); + debug("] "); + + String payload_str = ""; + for (int i = 0; i < length; i++) { + debug((char)payload[i]); + payload_str += (char)payload[i]; + } + + debugln(F("Processing :")); + debugln(payload_str); + //e.g Message arrived [inTopic] 1 + + for(int j =0; j < (payload_str.length() + 1); j++ ){ + + if (payload_str[j] == '1') { + + debugln(F("1 Received")); + } + + } + } -Data GetData() -{ - Data _sensor_data = {}; - // if programming failed, don't try to do anything - if (!_dmp_ready) - return _sensor_data; - // read a packet from FIFO - if (_mpu.dmpGetCurrentFIFOPacket(_fifo_buffer)) - { // Get the Latest packet - // display Euler angles in degrees - _mpu.dmpGetQuaternion(&_quaternion, _fifo_buffer); - _mpu.dmpGetGravity(&_gravity, &_quaternion); - _mpu.dmpGetYawPitchRoll(_yaw_pitch_roll, &_quaternion, &_gravity); - _sensor_data.yaw = _yaw_pitch_roll[0] * 180 / M_PI; - _sensor_data.pitch = _yaw_pitch_roll[1] * 180 / M_PI; - _sensor_data.roll = _yaw_pitch_roll[2] * 180 / M_PI; - } - return _sensor_data; -} -void setup() -{ - Serial.begin(SERIAL_BAUDRATE); +void force_config(){ - imuInit(); - _wifi_connect_handler = WiFi.onStationModeGotIP(_on_WiFi_connect); - _wifi_disconnect_handler = WiFi.onStationModeDisconnected(_on_WiFi_disconnect); + LED(true); + // start portal with 200 seconds runtime + debugln(F("Restarting to Start config portal -- MQTT server connect fail")); + delay(1000); + //forceConfig = true; + //WifiManagersetup(); - _mqtt_client.onConnect(_on_MQTT_connect); - _mqtt_client.onDisconnect(onMqttDisconnect); - _mqtt_client.onSubscribe(onMqttSubscribe); - _mqtt_client.onUnsubscribe(onMqttUnsubscribe); - _mqtt_client.onPublish(onMqttPublish); - _mqtt_client.onMessage(onMqttMessage); - _mqtt_client.setServer(MQTT_HOST, MQTT_PORT); - _connect_to_WiFi(); + LED(false); + //MQTT_conf = 1; + + //esp_deep_sleep(1000000);//test time -- 1 secs(uS) + + delay(1000); + + // wm.setConfigPortalTimeout(200); + + // if (!wm.startConfigPortal("HM_Phanthom", "clockX20")) { + // debugln(F("failed to connect or hit timeout.\n Restarting...")); + // delay(300); + // LED(false); + // ESP.restart(); + // } else { + // //if you get here you have connected to the WiFi + // debugln(F("connected...yeey :)")); + // LED(false); + // } } -void loop() -{ - Data data = GetData(); - SendData(data); +void reconnect() { + int fail_count = 0; + // Loop until we're reconnected + while (!client.connected()) { + debug(F("Attempting MQTT connection...")); + // Create a random client ID + String clientId = "HANDMOTIONCLIENT-"; + clientId += String(random(0xffff), HEX); + // Attempt to connect + if (client.connect(clientId.c_str())) { + debugln(F("connected")); + // Once connected, publish an announcement... + client.publish("outTopic", "Program Started .."); + // ... and resubscribe + client.subscribe("inTopic"); + } else { + if(fail_count == 10) { + fail_count = 0; + //debugln(F("failed 10 times, starting config portal")); + //force_config(); + return; + } + + fail_count = fail_count + 1; + debug(F("failed, rc=")); + debug(client.state()); + debugln(F(" trying again in 2 seconds")); + // 2 seconds before retrying + delay(2000); + } + } +} + +void upload(float yaw_, int16_t pitch, int16_t roll, int16_t raw_AX, int16_t raw_AY, int16_t raw_AZ, int16_t raw_GX, int16_t raw_GY, int16_t raw_GZ, uint32_t lrt) +{ + //prints data to be sent + //upload code + debugln(F("\nData ____")); + String senddata = "Y: " +(String)yaw_ + " P:" + (String)pitch+ " R:" + (String)roll; + + if (EnableDebug) + { + debugln(F("Raw data sent to MQTT server")); + senddata += " RAX:" + (String)raw_AX + " RAY:" + (String)raw_AY + " RAZ:" + (String)raw_AZ + " RGX:" + (String)raw_GX + " RGY:" + (String)raw_GY + " RGZ:" + (String)raw_GZ + " LastTIME:" + String((lrt - frt_sensor)/ 1000000); //lrt is in uS dividing by 1 000 000 for seconds + } + + debugln("\n"+senddata + "_______"); + + if (!client.connected()) { + reconnect(); + if (!client.connected()) return; + } + debug("\t\tAttempting MQTT connection..."); + // Create a random client ID + String clientId = "HANDMOTIONCLIENT-"; + clientId += String(random(0xffff), HEX); + // Attempt to connect + if (client.connect(clientId.c_str())) { + debugln(F("connected")); + + debug(F("\n\nSending:\n outTopic: ")); + debugln(senddata); + + char mqttdata[100]; + senddata.toCharArray(mqttdata, 100); + + client.publish("outTopic", mqttdata); + delay(150); + } + +} + + +void check_state(){ + String Cstate; + // if else + EnableDebug == 1 ? Cstate = " Open":Cstate = " Closed"; +} + +unsigned int previous_BLINK; +unsigned int current_BLINK; +bool blynk_state = false; +void Blynk_LED(){ + current_BLINK = millis(); + if((current_BLINK - previous_BLINK) > blinkRate_ws){ + LED(blynk_state); + blynk_state = !blynk_state; + previous_BLINK = current_BLINK; + } + +} + + +void Read_Yaw(){ + //Read Yaw -- using 10ms timesteps to get a more accurate reading + if ((timeStep*1000) - (millis() - timer) > 0){ + ;// Wait to full timeStep period + } else{ + + timer = millis(); + // Read normalized values + Vector norm = mpu.readNormalizeGyro(); + + // Calculate Pitch, Roll and Yaw + pitch_g = pitch_g + norm.YAxis * timeStep; + roll_g = roll_g + norm.XAxis * timeStep; + yaw_P = yaw_P + norm.ZAxis * timeStep; + + if((abs(yaw_A - yaw_P)) > Yaw_Offset) + yaw = yaw_P; + + yaw_A = yaw_P; + // Output raw + // Serial.print(" Pitch = "); + // Serial.print(pitch_g); + // Serial.print(" Roll = "); + // Serial.print(roll_g); + // Serial.print(" Yaw = "); + // Serial.println(yaw); + } +} + +void checkButton(){ + // check for button press + if ( digitalRead(TRIGGER_PIN) == LOW ) { + // poor mans debounce/press-hold + delay(500); + if( digitalRead(TRIGGER_PIN) == LOW ){ + debugln(F("Button Pressed")); + + LED(true); + + // start portal w delay + debugln(F("Starting config portal")); + wm.setConfigPortalTimeout(120); + + if (!wm.startConfigPortal("HM_Phanthom", "clockX20")) { + debugln(F("failed to connect or hit timeout")); + delay(3000); + //ESP.restart(); + LED(false); + } else { + //if you get here you have connected to the WiFi + debugln(F("connected...yeey :)")); + LED(false); + } + } + } +} + +void setup() { + + if(DEBUG == 1) + { + Serial.begin(115200); + } + delay(100); + + + debugln(F("\n\nStarting program")); + + pinMode(LED_PIN, OUTPUT); + digitalWrite(LED_PIN, LOW); + + LED(true); + waitForceConfig(); + WifiManagersetup(); + + //setup_wifi(); + randomSeed(micros()); + + client.setServer(MQTTIP, MQTT_PORT); + client.setCallback(callback); + + + debugln(F("\n---------------------------------------------\n")); + + !mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G) ? debugln(F("Failed to find MPU6050 chip")) : debugln(F("MPU6050 Found!")); + delay(500); + // Calibrate gyroscope. The calibration must be at rest. + mpu.calibrateGyro(); + + // Set threshold sensivty. Default 3. + mpu.setThreshold(3); + + IMU::init(); + IMU::read(); + + digitalWrite(LED_PIN, LOW); + pinMode(TRIGGER_PIN, INPUT); + debugln(F("\n\t\tInit...\n")); + Read_Yaw(); + frt_sensor = IMU::getLastReadTime(); } + +void loop() { + + check_state(); Blynk_LED(); + if (!client.connected()) + { + if ((WiFi.status() != WL_CONNECTED)) + { + //resetFunc(); + ESP.restart(); + } + reconnect(); + } + + Read_Yaw(); + IMU::read(); + + debug(yaw);debug(","); debug(IMU::getRoll());debug(F(","));debugln(IMU::getPitch()); + + /// @brief /////// + upload(yaw, IMU::getPitch(), IMU::getRoll(), IMU::getRawAccelX(), IMU::getRawAccelY(), IMU::getRawAccelZ(), IMU::getRawGyroX(), IMU::getRawGyroY(), IMU::getRawGyroZ(), IMU::getLastReadTime()); + + client.loop(); + drd->loop(); //doublereset for config page functionality + checkButton(); //boot pin to force config page +} \ No newline at end of file