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